Ceres Solver 中的LocalParameterization理解

该LocalParameterization类用来解决非线性优化中的过参数化问题过参数化指的是：待优化的参数的维度大于其实际意义的维度

例如：旋转需要3个自由度，但是四元数用四个维度表达三个自由度的三维的空间旋转，所以，需要引入单位四元数 限制其中一个自由度。优化问题最重要的就是要计算雅克比矩阵和残差
四元数需要手动自定义加法运算：在ceres中要增加ceres::LocalParameterization来定义加法运算[参考博客

对于增加的参数 旋转矩阵 选择以四元数的方式加入 所以 最终选择4维度 同样 平移向量 选择3维自由度的方式加入
ICP迭代求解最优旋转矩阵与平移向量

ALOAM雷达里程计主要步骤

论文

A 特征提取

选择锐边 和 平面片上的特征点(角点与面点)。(每个激光点云是通过三维激光雷达设备扫描得到的空间点的数据集，每个点云都包含了三维坐标X，Y，Z以及激光反射强度 点云的坐标系：x轴指向汽车的前部，y轴指向汽车的左侧。由于这个坐标系采用右手定则，坐标系z轴指向汽车上方)雷达点云基础知识讲解
根据局部曲面的平滑度(曲率)去筛选特征点(S为激光扫描仪同次扫描返回的i包括一系列连续点的集合，i中左右各有一半数目的点，每两个点之间的间隔为 0.25度，计算完成曲率后，将点按照曲率c进行排序 选择曲率(max)作为角点 选择 曲率(min)作为面点。将每一次扫描分6个子区域==目的：在环境中均匀分布特征点，每个区域最多提供两个边缘点以及4个平面点 (通过for 循环实现)==同时，通过if语句判断 是否i 的 c 值大于或小于阈值且所选点的数量不超过最大值时，才可以将点 i 选择为边缘点或平面点。
特征点的选择原则：不选择与激光束平行的点 同时也不选择被遮挡的点作为特征点
特征点选择原则：选择的边缘点或平面点的数量不能超过子区域的最大值。并且它的周围点都没有被选择，并且它不能在大致平行于激光束的平面上，或位于遮挡区域的边界上。

公式解释：

直观理解：
如果这11个点均匀分布在同一个直线上，那么求曲率对应的红点附近的点求均值得到的就是点的本身 ，但是如果曲线弯曲了 那么求解完成的点与红点的偏离距离越大证明曲率越大。平面点：在三维空间中处于平滑平面上的点，其和周围点的大小差距不大，曲率较低，平滑度较低。
边缘点：在三维空间中处于尖锐边缘上的点，其和周围点的大小差距较大，曲率较高，平滑度较高。
B. Finding Feature Point Correspondence
里程计odometry在一个扫描帧（sweep）中估计激光雷达的运动。t_k为开始扫描的时间，每次扫描结束后，到下一个时刻t_k+1开始时刻，在[t_k,t_k+1]时段感知的点云P_k被通过重投影到t_k+1时刻。记为 P_k’ ，在k+1到k+2扫描期间 作为已经实现畸变校正的点云初始值与新接收的点云P_K+1一起(未实现畸变校正)估计雷达的运动。
分别在P_k’ 与P_K+1中寻找两个激光雷达点云之间的对应关系。对于P_k+1，使用曲率（上一节特征选择算法）从激光雷达点云中查找到边缘点以及平面点由于平面点与边缘点存在多个 所以定义点集Ek+1 和 Hk+1 。然后从P_k’中找出作为Ek+1 中的边缘点的对应，找到平面点作为 Hk+1 中的点的对应。
> K+1次扫描时候，P_k+1是空集，扫描过程随着接收更多的点而增长，在每次迭代中，使用当前估计的变换将 Ek+1 和 Hk+1 重新投影到扫描的开始时刻t_k+1。设 Ek+1 ‘和 Hk+1’ 为重投影点集。对于 Ek+1 ‘和Hk+1’ 中的每个点，我们将在Pk’ 中找到最邻近点注意：此时将Ek+1 和 Hk+1 重新投影到扫描的开始时刻t_k+1，得到Ek+1 ‘和 Hk+1’，同时Pk’对应的也是t_k+1时刻 所以 同时间戳 其相应的特征点能够进行匹配（两次重投影）
边缘线的寻找过程：设 i 是Ek+1’ 中的一个点， i∈Ek+1’ 。边缘线（edge line）由两点表示。定义 j 表示Pk’ 中 i 的最近邻点， j∈Pk’ ，并定义 l 是与 j 相邻的连续两条扫描线束（scan）【上下各一条线束 scan ，相邻两次扫描是为了防止运动畸变过大导致边缘线特征提取不正确】中 i 的最近邻点。 (j,l) 形成 i 的对应关系。通过计算曲率验证j和i都是边缘点。这里，我们特别要求 j 和 l 来自不同的扫描线束，因为单个扫描线束（scan）不能包含来自同一边缘线（edge line）的多个点【保证线特征与扫描线不是同一条线】
平面点的平面片的寻找过程： i 为 Hk+1’ 中的一个点， i∈Hk+1’ 。平面片由三个点表示。与上一段类似，我们在 Pk '中找到 i 的最近邻点，表示为 j 。然后，我们找另外两个点， l 和 m 均是 i 的最近邻点，一个在 j 的相同扫描线束中，另一个在 j 的相邻两个连续扫描线束中。这保证了三个点不共线。为了验证 j、l 和 m 都是平面点，我们根据公式 (1) 再次检查局部曲面的平滑度
通过计算从特征点到其对应关系的距离 通过最小化特征点的总距离来恢复激光雷达的运动

用到了点到线的距离公式：获取两帧间的数据关联情况，应尽可能的让点到直线的距离最小

对于平面点而言：采用点到平面的距离公式：为了获取两帧间的数据关联情况，应尽可能的让点到平面的距离最小（相当于让向量 ij 到向量 jn 的投影的模长最小）

理想情况下：边缘点位于边缘线上，平面点位于平面上

C. Motion Estimation (运动状态估计算法)

通过使用非线性最小二乘构建残差函数 优化距离参数 获得具有最优解的旋转矩阵。
激光雷达在扫描的过程中以恒定的角速度和线速度进行运动，这允许我们在扫描中对在不同时间接收到的点进行线性插值计算姿态变换。T_L,K+1,i为[t_k+1,t]时刻，对应的i点的在雷达坐标系(L系)反映的姿态变换，其中t为当前时间的时间戳。

所以对于每一个时刻t_i都可以进行插值，得到[t_k+1,t_i]之间的相对位姿变换。

这也就是迭代的意义：相当于假定每一个初始时刻，例如，我们考虑t_k-1时刻，经过重投影后，其对应的该时刻开始的初始位姿状态是经过运动补偿的(之前进行线性插值)，然后，在当前t_k时刻，由于不断的扫描 使得特征点的数量持续增多，每个特征点在时间增加的同时 姿态也在相应发生着变化 所以，通过线性插值方法 可以假定能够用来补偿当前时刻所在点在运动过程中的偏移。上面的插值公式因为T_L,K+1是已经进行上次插值补偿的数据 所以 利用上述公式 可以得到小的时间间隔内的姿态变换的运动畸变补偿数据

Ek+1 和 Hk+1 是从 Pk+1 中提取的边缘点集和平面点集，而 Ek+1’和 Hk+1’ 是重新投影到扫描开始的点集。为了得到激光雷达的运动，我们需要在 Ek+1 和 Ek+1’ 或者 Hk+1和 Hk+1’之间建立几何关系:
也就是：下面这张图像所表示的：


D. LiDAR Odometry algorithm (里程计算法)

激光雷达里程计算法：将最后一次扫描的点云 Pk '、当前正在扫描的增长点云 Pk+1 和最后一次递归的姿势变换 T_Lk+1 作为输入【匀速模型】。如果一个新的扫描开始， TLk+1 被设置为零（第 4 - 6 行）。然后，该算法从 PK+1 中提取特征点，在第 7 行构造 Ek+1 和 Hk+1 。对于每个特征点，我们在Pk’中找到其对应关系（第 9 - 19 行）。运动估计适用于鲁棒拟合（robust fitting）。在第 15 行中，算法为每个特征点分配一个二次方权重（bisquare weight）。对与其对应点距离较大的特征点分配较小的权重，对距离大于阈值的特征点视为异常值并分配零权重【类似于核函数】。然后，在第 16 行中，姿势变换迭代更新一次。如果发现收敛或满足最大迭代次数，则非线性优化终止。如果算法到达扫描结束时， Pk+1 将使用扫描期间的运动估计重新投影到时间戳 tk+2 【当前 lidar 直接获得一帧数据，此处判断可以忽略】。否则，下一轮递归只返回转换 T_Lk+1 (这里并没有表示：在t_k+1时刻点云不断增长情况下 对于两个特征数据集的重投影过程，只是说下次递归如果不是新的扫描的话 下一轮递归则返回T_Lk+1 )。（缺少不断增长的重投影的描述）

A-LOAM laser Odometry代码

值得借鉴的翻译论文

LiDAR Odometry

通过以下的4种类回调函数 将四类特征点分类存储到相应的buffer数据中
判断是否存在接收数据 同时也必须满足四种角点特征对应的时间戳信息一致
如果对应有效的特征点数据 我们通过fromROSMsg函数每次将一个feature point 信息传递给对应的pcl::PointCloud::Ptr 数据
判断系统是否初始化

这里需要明确：对于第一次接收到的特征点信息，并不做else对应的重投影以及畸变校正、而是直接执行第二张图片对应的程序块：将接收到的弱角点保存为上一帧弱角点,为下一帧做准备； 把当前帧弱面点保存为上一帧弱面点,为下一帧做准备，然后将对应的弱角点与弱面点 插入到KD树用来后续的几何对应搜索

ALOAM

四元数的插值不能用简单的线性插值，而是用slerp —— 当时间t匀速变化时，代表姿态矢量的角速度变化并不均匀（Eigen::Quaterniond::Identity().slerp(t, q_last_curr)能够实现四元数的球面插值。t ∈ [0, 1]为插值点，q_last_curr为两帧之间的旋转四元数，即针对两帧之间的旋转而线性插入一个四元数。）
系统已经实现初始化
那么在就需要在不断增加角点特征以及面点特征的基础上，执行迭代优化。

通过定义corner_correspondence和plane_correspondence判断平面以及角点在不断增加特征点的数量后的对应number数量。
定义局部参数LocalParameterization(LocalParameterization 类的作用是解决非线性优化中的过参数化问题)所谓过参数化，即待优化参数的实际自由度小于参数本身的自由度（四元数本身4个自由度 但是待优化参数实际的自由度是3）——>如果对于四元数进行优化 需要对待优化的四元数参数进行重构——(自定义LocalParameterization的子类实现，因为LocalParaneterization 本身是一个虚基类，用户可以自行定义自己需要使用的子类)
使用ceres::LocalParameterization 将待优化的四元数执行自定义的四元数局部参数化
设置ceres solver的问题的option 以及problem
这里注意：AddParameterBlock函数是显式的增加参数块——因为将待优化四元数进行了参数调整 所以需要显式增加参数信息
这里待优化的变量包括(四元数的旋转以及平移三维向量)寻找当前角点特征的correspondence对应关系
为优化存在的每一个角点以及面点特征寻找correspondence

寻找角点特征

整体流程：将当前的特征点cornerPointsSharp->points[i]通过重投影TransformToStart函数，得到所研究时段的初始时刻的对应的投影点pointSel，因为通过systeminit已经将弱角点对应的信息全部传递给kdtreeCornerLast存储，所以通过nearestKSearch，能够茶找到pointSel投影点对应的最近邻点信息closestPointInd；然后通过j = closestPointInd + 1以及j = closestPointInd - 1，分别搜索同扇区 不同扇区 不同扫描区的closestPointInd 以及minPointInd2，找到最近邻点后 通过LidarEdgeFactor::Create 创建自动求导函数new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>并加入problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);残差函数

代码流程分析

TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);

判断是否存在畸变运动补偿DISTORTION，而对于点云的畸变运动补偿的处理方法此处选择：通过四元数slerp插值的方法实现
因为假定为匀速运动 所以每一帧之间的相对姿态变换都是相同的 所以通过调用Quaterniond.slerp函数 能够实现相对时刻的插值（插值得到当前点相对于该帧起始点的旋转）
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
然后这里需要注意：Eigen::Vector3d un_point = q_point_last * point + t_point_last;是将重投影的关键(将相对时间中的任何时刻都统一到开始时刻 也就相当于去除了畸变 完成了运动补偿)

重投影到每一个相对变换的时间间隔的初始时刻start time，通过nearestKSearch函数搜索pointSel(当前初始时刻投影点对应的最近邻) 也就是论文中的j点
因为是边缘线 ，在已经校正的点云上面找到对应的最近邻角点信息
（因为之前论文阐述的角点筛选原则：不能在同scan以及不是在连续的相邻scan中 那么分别执行
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)// if in the same scan line, continue continue; if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))// if not in nearby scans, end the loop break;）
如果找到对应的最近邻点来自相邻射线计算相邻的short distance (pointSqDis)
j = closestPointInd + 1在下方查找j点的最近邻l点 j = closestPointInd - 1 在上方查找最邻近点，查找j的最近邻l点
如果 当前j的最近邻点与j同scan那么 continue
如果 非 consecutive scan那么break
如果满足consecutive scan 同时满足最近邻点筛选的条件 那么计算pointSqDis，并且通过：
if (pointSqDis < minPointSqDis2) { // find nearer point minPointSqDis2 = pointSqDis; minPointInd2 = j; }迭代选择最终的最近邻点
构建损失函数 为问题增加残差块
> 同理 对于边缘线特征来说，如果投影点位于最近邻点所在的线上，那么，residual就为0，但是 由于误差的存在 不可能使得residual为0 所以 出现edge factor因子。
过LidarEdgeFactor::Create函数构建自动求导构建自动求导的损失函数 这个损失函数 第一维数据为residual3维，第二个为经过localparameterazation的4维四元数数据 第三位为平移向量的3维度数据。创建LidarEdgeFactor因子> 根据距离公式计算残差residual
这里首先将当前位置的current_pose信息经过四元数插值转换到所研究时段的初始时刻。然后，根据点到线的距离公式 计算残差 这里将点到直线的距离分别分配到x,y,z三个方向上的residual

寻找面点特征

前面的重投影以及在重投影集中寻找i点对应的最近邻j点是相同的步骤
> 基于搜索到的最近邻点j，通过两个for循环实现向前查找以及向后查找(注意这里laserCloudSurfLast->points是根据曲率进行排序得到的，如果不是nearest scan 直接break)两个for 循环都是用来寻找consecutive scan以及same scan的，所以当int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID 或者<=closestPointScanID 时候 改变的都是minPointInd2 以及minPointSqDis2 代表same scan存在(l点) 同理int(laserCloudSurfLast->points[j].intensity) > closestPointScanID或者 < closestPointScanID代表consecutive scan的邻近点 最终选择minPointInd3 以及以及minPointSqDis2存储(m点)
closestPointInd对应论文中的j点、minPointInd2对应论文中的、minPointInd3对应论文中的。

搜索到三个非共线平面点后 构建损失函数
基于LidarPlaneFactor的因子
利用点到平面的距离公式 得出一维残差，其实可以这么理解：求点到平面的距离（如果是平面点的话，那么最近邻点和搜索点共面 那么残差就为0了 但是因为误差的存在 不可能残差为0 所以 计算点积）

求解器设置

这里的t_last_curr与q_last_curr通过ceres solver 不断优化para_q以及para_t来