YMWM_的博客

本书大部分Demo都基于NDT实现，与PCL版本的NDT有明显不同。为本书的NDT设计一个匹配度评估指标，利用该指标可以判断NDT匹配的好坏。可以参考PCL NDT的transprobability值，也可以自己来设计。利用第1题的指标，修改程序，实现mapping部分的回环检测。注意要利用分值好坏来判断回环检测是否有效。将建图结果导出为NDT map，即将NDT体素内的均值和协方差都存储成文件。同样建议分块存储。实现基于NDT map的激光定位。

实现基于点面ICP的IEKF LIO系统，给出实现代码和实验展示（注意local map的处理方式和NDT有所不同）。如果在IEKF中引入点面ICP作为观测方程，请说明观测方程的具体处理方式，以及IEKF和纯激光ICP之间的关系。

将第1题的robustloss引入IncNDTLO和LooselyLIO，给出实现和运行效果。为三类ICP和NDT配准添加一个Cauchy核的robustloss，讲述原理并代码实现。从概率层面解释NDT残差和协方差矩阵的关系，说明为什么NDT协方差矩阵可以用于最小二乘。*为LOAMlikeLO设计一个地面点云提取流程，并单独为这些点使用点面ICP。

实现对似然场图像进行插值后进行scan matching的算法。讨论在实际机器人中，如何处理悬空物体和低矮物体（开放问题）。基于直线拟合方法，讨论并实现对单个Scan退化检测的算法。实现基于优化器的点到点ICP、点到线ICP。

将本节的最近邻算法与一些常见的近似最近邻算法进行对比，比如nanoflann，给出精度指标和时间效率指标。在三维体素中定义 NEARBY14，实现 14 格最近邻的查找。的最大特征向量或奇异向量，

利用数值求导工具，验证本书实验中的雅可比矩阵的正确性。实现由Odom数据触发的图优化（g2o）。推导以下等式(书中式4.48d)，

证明：若某个高斯随机变量为零均值，协方差为对角线矩阵且大小相同（各向同性），那么在乘任意旋转矩阵以后，其均值仍为零，且协方差不变。具有零均值和对角协方差矩阵，且协方差矩阵是各向同性的（即协方差矩阵是对角矩阵且大小相同），那么在乘上一个任意的旋转矩阵。矩阵拆开，分别为每种状态变量实现运动方程。因此，旋转变换不会改变高斯随机变量的均值和协方差矩阵。后，其均值仍然为零，且协方差矩阵保持不变。-维的高斯随机变量，具有零均值和协方差矩阵。我们需要证明的是：若某个高斯随机变量。是零均值的高斯随机变量，即。

说明高斯-牛顿法和列文伯格-马夸尔特法在处理非线性优化时的差异。：将实践环节的运动学修改成以一定角速度的平抛运动。方向的重力加速度影响。请修改程序，给出动画演示。轴角速度影响，具有一定的初始水平速度。：请分别使用左右扰动模型，计算。：分别使用左右扰动模型，计算。