c++ - 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_,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 来估计变换矩阵。我可以使用非线性优化算法解决线性优化问题吗?
解决方案
一方面,当您将翻译复制到矩阵中时,有一种类型
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 的三维残差。
您的残差函数似乎报告的残差为零,在函数内添加一些日志记录以查看发生了什么会有所帮助。
推荐阅读
- javascript - 功能组件中的新道具不会重新渲染
- ruby-on-rails - Capistrano Rails 部署 - 身份验证错误
- c# - 通过实现的接口解析服务,而无需在 .NET Core DI 中显式注册它们
- javascript - 未捕获的 TypeError:Object.getOwnPropertyNames 不是 P5.js 中的函数
- reactjs - 代理通行证在 nginx 中反应应用程序以获取域路径
- java - HHH000342:无法获得与查询元数据的连接:用户 'root'@'localhost' 的访问被拒绝(使用密码:否)
- mysql - 如何减少 AWS RDS 数据库的延迟?
- python - 无法使用 Robot Framework 的 --Variable 为不同的浏览器运行
- java - 为单个 Feign 客户端禁用 Hystrix
- python - 在 Python 中调度任务