-1

我是 Ceres 优化库的新手。我想估计两组 3D 对应点之间的刚体变换，但 ceres 给出了一个奇怪的总结，如下所示：

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

我的代码：

    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";

有没有人解释我的错在哪里？另外，我的另一个问题是关于优化方法。我使用 DENSE_QR 来估计变换矩阵。我可以使用非线性优化算法解决线性优化问题吗？

4

1 回答 1

-1

一方面，当您将翻译复制到矩阵中时，有一种类型

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

最后一行应该是

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

第二件事是，与其取平方范数并取其平方根，不如拥有一个等于 diff 的三维残差。

您的残差函数似乎报告的残差为零，在函数内添加一些日志记录以查看发生了什么会有所帮助。

于 2021-03-31T23:30:19.423 回答