0. 前言

在了解SLAM的原理、流程后，个人经常实时困惑该如何去从零开始去设计编写一套能够符合我们需求的SLAM框架。作者认为Ceres、Eigen、Sophus、G2O这几个函数库无法避免，尤其是Ceres函数库在激光SLAM和V-SLAM的优化中均有着大量的应用。所以作者从Ceres作为开端，来对手写SLAM开个头，来方便各位后续的开发。
这里分享以为博主写的博客，个人看了感觉写的不错，对SLAM的剖析很细致

1.Ceres示例

首先我们需要明确Ceres函数库在SLAM中主要起到的作用是优化。目前Bundle Adjustment 其本质还是离不开最小二乘原理(几乎所有优化问题其本质都是最小二乘),目前Bundle Adjustment 优化框架最为代表的是Ceres solver和G2O(这里主要介绍ceres solver)。Ceres中的优化需要四步，构建优化的残差函数，构建优化问题，在每次获取到数据后添加残差块，总体优化。

构建残差函数：

//构建代价函数结构体，residual为残差。
//last_point_a_为这一帧中的点a，curr_point_b_为点a旋转后和上一帧里最近的点
//curr_point_c_为点b同线或上线号的点，curr_point_d_为点b下线号的点
//b，c，d与a点距离不超过3m
//plane_norm为根据向量bc和bd求出的法向量
struct CURVE_FITTING_COST
{
	//类似构造函数
  CURVE_FITTING_COST(Eigen::Vector3d _curr_point_a_, Eigen::Vector3d _last_point_b_,
		     Eigen::Vector3d _last_point_c_, Eigen::Vector3d _last_point_d_):
		     curr_point_a_(_curr_point_a_),last_point_b_(_last_point_b_),
		     last_point_c_(_last_point_c_),last_point_d_(_last_point_d_)
	{
	    plane_norm = (last_point_d_ - last_point_b_).cross(last_point_c_ - last_point_b_);
	    plane_norm.normalize();
	}
  template <typename T>
  //plane_norm点乘向量ab为a点距面bcd的距离，即残差
  bool operator()(const T* q,const T* t,T* residual)const
  {
    Eigen::Matrix<T, 3, 1> p_a_curr{T(curr_point_a_.x()), T(curr_point_a_.y()), T(curr_point_a_.z())};
    Eigen::Matrix<T, 3, 1> p_b_last{T(last_point_b_.x()), T(last_point_b_.y()), T(last_point_b_.z())};
    Eigen::Quaternion<T> rot_q{q[3], q[0], q[1], q[2]};
    Eigen::Matrix<T, 3, 1> rot_t{t[0], t[1], t[2]};
    Eigen::Matrix<T, 3, 1> p_a_last;
    p_a_last=rot_q * p_a_curr + rot_t;
    residual[0]=abs((p_a_last - p_b_last).dot(plane_norm));
    return true;
  }
  const Eigen::Vector3d curr_point_a_,last_point_b_,last_point_c_,last_point_d_;
  Eigen::Vector3d plane_norm;
};

构建优化问题：

//优化问题构建
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);

每次求出abcd点后，将他们的坐标构建成Eigen::Vector3d数据，添加残差块：

Eigen::Vector3d curr_point_a(laserCloudIn_plane.points[i].x,
			    laserCloudIn_plane.points[i].y,
			    laserCloudIn_plane.points[i].z);
Eigen::Vector3d last_point_b(laserCloudIn_plane_last.points[closestPointInd].x,laserCloudIn_plane_last.points[closestPointInd].y,
			    laserCloudIn_plane_last.points[closestPointInd].z);
Eigen::Vector3d last_point_c(laserCloudIn_plane_last.points[minPointInd2].x,
			    laserCloudIn_plane_last.points[minPointInd2].y,
			    laserCloudIn_plane_last.points[minPointInd2].z);
Eigen::Vector3d last_point_d(laserCloudIn_plane_last.points[minPointInd3].x,
			    laserCloudIn_plane_last.points[minPointInd3].y,
			    laserCloudIn_plane_last.points[minPointInd3].z);
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,4,3>
			(new CURVE_FITTING_COST(last_point_a,curr_point_b,
			  curr_point_c,curr_point_d)),loss_function,para_q,para_t);

遍历过所有的a点后，就可以优化求解了。

//所有前一帧里的点都当a点遍历过后，进行优化
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
//迭代数
options.max_num_iterations = 5;
//进度是否发到STDOUT
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

而在V-SLAM中Ceres则用的更多，我们可以看下下面的例子

…详情请参照古月居