Ceres Solver诡异output及优化方法

Question

I'm new to the Ceres optimization library. I would like to estimate the rigid body transformation between two sets of 3D correspondence points, but ceres gives a weird summary like this:

Ceres Solver Report: Iterations: -2, Initial cost: 0.000000e+00, Final cost: 0.000000e+00, Termination: CONVERGENCE

My Codes:

    struct MyCostFunctor
    {
    public:
        MyCostFunctor(Eigen::Vector4d point_source_,
                      Eigen::Vector4d point_target_)
                      :point_source(point_source_),
                      point_target(point_target_){}

        template <typename T_>
        bool operator () (const T_* const qx,
                          const T_* const qy,
                          const T_* const qz,
                          const T_* const qw,
                          const T_* const tx,
                          const T_* const ty,
                          const T_* const tz,
                          T_* residual) const
        {

            Eigen::Quaternion<T_> q_estimated;
            q_estimated.x() = qx[0];
            q_estimated.y() = qy[0];
            q_estimated.z() = qz[0];
            q_estimated.w() = qw[0];
            Eigen::Matrix<T_,3,3> rot_estimated = q_estimated.matrix();
            Eigen::Matrix<T_,4,4> mat_estimated;
            mat_estimated.setIdentity();
            mat_estimated.topLeftCorner(3,3) = rot_estimated;
            mat_estimated(0,3) = tx[0];
            mat_estimated(1,3) = ty[0];
            mat_estimated(1,3) = tz[0];

            Eigen::Matrix<T_,4,1> point_result = mat_estimated*point_source;
            Eigen::Matrix<T_,4,1> diff = point_result-point_target;

            T_ distance = sqrt(pow(diff(0),2) + pow(diff(1),2) + pow(diff(2),2));

            residual[0] = distance;
        }

    private:
        Eigen::Matrix<double,4,1> point_source;
        Eigen::Matrix<double,4,1> point_target;
    };


    double qx = 5;
    double qy = 5;
    double qz = 10;
    double qw = 11;
    double tx = 1;
    double ty = 2;
    double tz = 3;

    size_t count_point = cloud_source->size();

    ceres::Problem problem;

    for (int i = 0; i < count_point; ++i) {
        problem.AddResidualBlock(
                new ceres::AutoDiffCostFunction<MyCostFunctor, 1,  1,1,1,1,1,1,1>
                (
                new MyCostFunctor(vector_points_source[i],vector_points_target[i])),
                nullptr,
                &qx, &qy, &qz, &qw, &tx, &ty, &tz
                );
    }

    ceres::Solver::Options options;
    options.max_num_iterations = 25;
    options.num_threads = 12;
    options.linear_solver_type= ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << "\n";

Is there anybody to explain where is my fault? Also, my other question is about the optimzation method. I used DENSE_QR in order to estimate the transformation matrix. Can I solve the linear optimization problems by using non-linear optimization algorithms?

Answer 1

For one thing, there is a type when you are copying the translation into the matrix

mat_estimated(0,3) = tx[0];
mat_estimated(1,3) = ty[0];
mat_estimated(1,3) = tz[0];

The last line should be

mat_estimated(2,3) = tz[0];

The second thing is that instead of taking the squared norm and taking its square root, you are better off having a three dimensional residual which is equal to diff.

Your residual function seems to be reporting a residual of zero, adding some logging inside the function to see what is going on will help.