Triangulation: Ceres solver minimizes only the last residual block

Question

I have been attempting to estimate a bunch of 3d coordinates viewed via a multi-view setup with known poses, I have the code running, but the output of ceres seems baffling.

There are 7 image frames/poses, and only the re-projection error with respect to the last pose(7th frame) is close to 0. 2d Reprojections of the ceres-estimated 3dpoint onto rest of the frames(1-6) do not match with the actual pixel measurements. If I reduce the number of frames, it gives out a different 3d point whose 2d projections again match only with the last frame.

I'm adding code snippets here, can someone just look at the code and give any insights as to why this could be happening?

class ReprojectionError {
 public:
  ReprojectionError(
        const Eigen::Matrix<double, 3, 4>& projection_matrix,
        const Eigen::Vector2d& feature)
        : projection_matrix_(projection_matrix), feature_(feature) {}

  template <typename T>
  bool operator()(const T* input_point, T* reprojection_error) const {
        Eigen::Map<const Eigen::Matrix<T, 4, 1> > point(input_point);

        // Multiply the point with the projection matrix, then perform homogeneous
        // normalization to obtain the 2D pixel location of the reprojection.

        const Eigen::Matrix<T, 2, 1> reprojected_pixel =  (projection_matrix_ * point).hnormalized();
        // Reprojection error is the distance from the reprojection to the observed
        // feature location.
        cout << point << endl;
        reprojection_error[0] = feature_[0] - reprojected_pixel[0]; 
        reprojection_error[1] = feature_[1] - reprojected_pixel[1];

        return true;
  }
  static ceres::CostFunction * Create(const Eigen::Matrix<double, 3, 4>& projection_matrix_, 
        const Eigen::Vector2d& feature_) {
            return (new ceres::AutoDiffCostFunction<ReprojectionError, 2,4>
                (new ReprojectionError(projection_matrix_,feature_)));
    }

 private:
    const Eigen::Matrix<double, 3, 4>& projection_matrix_;
    const Eigen::Vector2d& feature_;
};

Eigen::Vector3d Triangulate(std::vector<std::pair
    <Eigen::Matrix<double, 3, 4>,Eigen::Vector2d>> datas) {
        Eigen::Vector4d x;
        x << 0,0,0,1;
        ceres::Problem problem;
        ceres::Solver::Options options;
        ceres::LossFunction* loss= nullptr;
        ceres::Solver::Summary summary;

        for(auto data:datas) {
            problem.AddResidualBlock(ReprojectionError::Create(data.first,data.second),loss,&x[0]);
        }
        ceres::Solve(options,&problem, &summary);
        std::cout << summary.FullReport() << "\n";
        return x.hnormalized();
}

Here's the summary:

Solver Summary (v 2.0.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  4                        4
Residual blocks                             7                        7
Residuals                                  14                       14

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                        1

Cost:
Initial                          4.220501e+04
Final                            2.554256e-13
Change                           4.220501e+04

Minimizer iterations                        5
Successful steps                            5
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.000060

  Residual only evaluation           0.000774 (5)
  Jacobian & residual evaluation     0.002446 (5)
  Linear solver                      0.000053 (5)
Minimizer                            0.003332

Postprocessor                        0.000002
Total                                0.003394

Termination:                      CONVERGENCE (Parameter tolerance reached. Relative step_norm: 1.091788e-09 <= 1.000000e-08.)

Answer 1

Suggestion by Markus was right :

I believe that your problem lies here:

for(auto data:datas) {
     problem.AddResidualBlock(ReprojectionError::Create(data.first,data.second),loss,&x[0]); 
}

data will be a copy of your matrix/point tuple. You're then storing references to data's members in your newly created residuals. This is not what you want.

Try for(auto& data : datas) instead.

https://groups.google.com/forum/#!msg/ceres-solver/2IRuiVuRE54/wJzSC3X4BQAJ

Answer 2

Tricky to look at for me, but I'm not a C++ person in general. In looking at it, though, is your ReprojectionError::Create defined correctly?

You have:

static ceres::CostFunction * Create(const Eigen::Matrix<double, 3, 4>& projection_matrix_, 
    const Eigen::Vector2d& feature_) {
        return (new ceres::AutoDiffCostFunction<ReprojectionError, 2,4>
            (new ReprojectionError(projection_matrix_,feature_)));
}

But your call to ceres::AutoDiffCostFunction uses <ReprojectionError, 2,4>. In the documentation I found online, I see the following:

Then given this class definition, the auto differentiated cost function for it can be constructed as follows.

CostFunction* cost_function
= new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
    new MyScalarCostFunctor(1.0));              ^  ^  ^
                                                |  |  |
                    Dimension of residual ------+  |  |
                    Dimension of x ----------------+  |
                    Dimension of y -------------------+

You have <ReprojectionError, 2, 4>, which would make it look like the dimension of residual is 2, which may explain your problem. Not positive this is the answer, because I don't normally do anything with C++ and have never seen this library before, but I would think that, even if it were a 2x1 vector, you'd still need <ReprojectionError, 2, 2, 1>, right?

There's a note at the bottom of that section that states the following:

WARNING 1 A common beginner’s error when first using AutoDiffCostFunction is to get the sizing wrong. In particular, there is a tendency to set the template parameters to (dimension of residual, number of parameters) instead of passing a dimension parameter for every parameter block. In the example above, that would be <MyScalarCostFunction, 1, 2>, which is missing the 2 as the last template argument.

This seems to echo my gut reaction that the parameters aren't set correctly. I would have expected some sort of error or something, but C++ is a bunch of black magic that gives you enough rope to hang yourself and then shoots you in the face.