LOAM详解（二）激光里程计

一、用之前获得的角度插值来对点云去畸变

二、用KD树搜索最近点

pcl::KdTreeFLANN对象radiusSearch和nearestKSearch接口的性能分析
【量化课堂】kd 树算法之详细篇

三、构建scan2scan点到线和点到面的距离

1. 寻找线
   用KD树在上一帧中找到一个最近点$j$，在$j$所在的相邻线上再找一个距离最近的点$l$，$j$和$l$则构成了线。
2. 寻找面
   同理找到$j$和$m$，$l$为同线上的最近点。
   
3. 计算点到线和面的距离
   目的是找到一个变换矩阵使得下图的两个残差函数最小
   

四、使用Ceres进行优化

就像14讲中第六章ceres例题，用100个常量去优化三个未知数，这里也用数个点到线点到面的约束去优化位姿。
改进方式：加入信息矩阵，根据点到线面的距离设置权重

1. 设置loss_function

ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);

2. 使用四元数进行优化

ceres::LocalParameterization *q_parameterization =
                        new ceres::EigenQuaternionParameterization();
                   

3. 构建Problem存放参数块

ceres::Problem::Options problem_options;
                    
ceres::Problem problem(problem_options);
//将参数块存放入问题中
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);

4. 构建cost_function并存放入残差块
   
5. 手动推导雅可比
   
   
   原理：在不同时刻，某一静态物体相对于运动的激光雷达的位姿是不同的，我们可以通过位姿的变化计算激光雷达的运动状态

五、里程计优化流程