如何解决Ceres Solver 奇怪的输出和优化方法
我是 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_,1> point_result = mat_estimated*point_source;
Eigen::Matrix<T_,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,1> point_source;
Eigen::Matrix<double,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>
(
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 来估计变换矩阵。我可以使用非线性优化算法解决线性优化问题吗?
解决方法
一方面,当您将翻译复制到矩阵时有一种类型
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 的三维残差。
您的残差函数似乎报告的残差为零,在函数内添加一些日志记录以查看发生了什么会有所帮助。
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。