1.Ceres中求解一个优化问题的结构
背景:在SLAM中,很多问题都是在求解Translation(包含旋转和平移量),因此这里以其为代表,来分析使用ceres如何对其近求导。
void Calibrator::Optimize(Eigen::Matrix4d& tf)
{
//待优化参数分别为rotation和t
Eigen::Matrix3d rot = T_.topLeftCorner(3,3);
Eigen::Quaterniond q (rot);
Eigen::Vector3d p = T_.topRightCorner(3,1);
ceres::Problem problem;
ceres::Solver::Summary summary;
ceres::Solver::Options options;
ceres::LossFunction* loss_function_edge (new ceres::SoftLOneLoss(1));
ceres::LocalParameterization* quaternion_local_parameterization =
new ceres::EigenQuaternionParameterization;
for(const auto& ply : polygons_)
{
// add edge 误差来源:每一条3D点云边上的点,到2D平面的直线的距离
for(uint32_t i=0; i<size_; i++)
{
if(ply.pc->edges[i].points.cols() == 0)
{
continue;
}
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<Edge2EdgeError, 1,4,3>(new Edge2EdgeError( K_, ply.pc->edges[i].points, ply.img->edges[ply.ids[i]].coef ));//
problem.AddResidualBlock(cost, loss_function_edge,q.coeffs().data(), p.data());
}
}
problem.SetParameterization(q.coeffs().data(), quaternion_local_parameterization);
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5000;
options.num_threads= boost::thread::hardware_concurrency() - 1;
options.num_linear_solver_threads = options.num_threads;
ceres::Solve(options, &problem,