代码研究时遇到的问题

[lzhich]

@作者，感谢开源优秀的研究成果！
在学习和研究您发布的SuperOdom库和相关的两篇论文的过程中，有部分不理解的部分（具体问题见下文），希望您能提供一些帮助。期待您的回答！
superodom问题.docx

[shibowing]

Sorry for missing your question. I will reply it as soon as possible.

[shibowing]

Thank you so much for such insightful questions. I just pasted all the questions here for quick reference.

感谢开源优秀的研究成果！
在学习和研究您发布的SuperOdom库和相关的两篇论文的过程中，有部分不理解的部分（具体问题见下文），希望您能提供一些帮助。期待您的回答！
一、 IMU测量值的使用问题
在“featureExtraction”和“lasermapping”节点中使用的IMU数据都是来自IMU仪器的测量值，而非来自“imuPreintegration”节点即IMU里程计优化后的数据，如图1所示。请问目前提供的库中是否不包含IMU里程计向雷达里程计提供数据的部分功能？
另外，在后续的去畸变过程中，代码中也只是使用了IMU测量值中的横滚角和俯仰角，IMU测量值的平移和偏航角数据都没有被使用，如图2所示。而在去畸变的过程中没有平移数据会导致点云平移上的畸变无法正常处理，这是否会对后续的优化产生影响？
并且在雷达里程计对应的两个节点中，IMU测量值的零偏不会更新，只使用初始化时得到的零偏值对后续所有的IMU测量值进行修正，这会导致IMU测量值存在较大的误差。是否是由于这个原因才不使用IMU测量值提供的平移量和偏航角？
在去畸变和粗定位位姿估计的过程中，如果是来自IMU里程计即“imuPreintegration”节点中零偏更新后的数据，是否可以直接使用全部的IMU数据？

图1. 库运行时ROS2话题展示

图2. super_odometry\src\FeatureExtraction\featureExtraction.cpp文件removePointDistortion函数，第233-238行代码
二、 点到边ICP配准没有使用
在雷达里程计的“lasermapping”节点中包含完整的点到边ICP配准代码流程，但在“featureExtraction”节点中进行点云分类时并没有向边点云分配任何数据，如图3所示。其中，plannerPoints、edgePoints、bobPoints三个变量都被用于保存点云数据，根据论文内容和变量命名可以理解为这三个变量分别用于保存点到面、点到边、点到点的点云数据。但在后续的处理中只有plannerPoints变量中输入了数据，其余两个变量中都没有输入数据。这导致后续的点到边ICP配准代码都不会不使用。
是点云分类的功能暂时没有实现还是点到边ICP配准这部分功能已经被舍弃了，不再使用？

图3. super_odometry\src\LidarProcess\featureExtraction.cpp文件
extractFeatures函数，第422-437行代码
三、 SuperLoc论文中功能实现问题
（1） 统计可观测性标签时存在逻辑问题。
在论文的B部分（Observability Estimation for State Estimation）中统计了各平移方向和旋转方向的可观测性标签计数，这部分功能的实现在代码库中如图4所示（仅展示x平移方向的统计代码）。在该部分代码中，变量PlaneFeatureHistogramObs保存了各个维度的可观测性标签计数，其中第6、7、8维度分别是x、y、z平移方向上的可观测性标签计数。但是EstimateLidarUncertainty函数（进行可观测性标签计数和退化检测的函数）会在performLocalizationAndMapping函数（进行特征约束计算和ICP优化的函数）之前被调用，如图5所示。而变量中保存的数据是在点到面特征约束的计算过程中（processPlannerFeatures函数，在performLocalizationAndMapping函数中被调用）添加的，如图6所示。
这就会导致一个问题，在调用定位函数时，变量PlaneFeatureHistogramObs中还没有存入这一帧点云的数据时就会被调用，而该变量内数据只会在每一次进行迭代优化之前被清空。所以代码中每一次调用定位函数进行可观测性标签的计数时，统计的不是这一帧点云的可观测性标签，而是保存在变量PlaneFeatureHistogramObs中的上一帧点云的可观测性标签，这会导致后续的不确定度计算和退化检测都存在逻辑上的问题。

图4. super_odometry\src\LidarProcess\LidarSlam.cpp文件EstimateLidarUncertainty函数，第991-1000行代码

图5. super_odometry\src\LidarProcess\LidarSlam.cpp文件
Localization函数，第46-55行代码

图6. super_odometry\src\LidarProcess\LidarSlam.cpp文件
processPlannerFeatures函数，第363-372行代码

（2） 论文中的C部分（Active Sensor Fusion）并没有被实现。

论文中使用可观测性标签计数相对于总标签数的比列来判断退化并使用该比列构成的协方差矩阵增加一个约束因子，这个因子被用于处理退化场景。而在代码中有该部分有多个功能没有被实现。
首先，用于退化检测的代码并没有被采用，如图7所示。判断退化和退化后处理代码都被注释了，没有采用。

图7. super_odometry\src\LidarProcess\LidarSlam.cpp文件
EstimateLidarUncertainty函数，第1052-1061行代码
其次，ICP优化时没有额外的约束补充。由于代码中使用ceres库进行雷达里程计中的ICP配准，所以代码向优化问题中添加了一个绝对位姿约束替代论文中的先验因子，如图8所示。但这部分代码并没有被使用，在使用包含退化场景的数据集时，该if分支内添加的任何输出命令都不会输出到终端屏幕上。

图8. super_odometry\src\LidarProcess\LidarSlam.cpp文件
setupOptimizationProblem函数，第237-242行代码
另外，该绝对位姿约束的权重设置存在问题。具体代码如图9所示。其中information矩阵即为向ceres库中添加绝对位姿约束时的权重，也就是论文中的先验信息：Covprior = I6x6 −Σcov。代码中的信息矩阵中的前三维分别有三部分组成。第一部分和论文中描述一样，为1减去可观测性标签的比例（即代码中的不确定度）；第二部分为约束数目即每帧点云中ICP配准时可以使用的点的数目；第三项为视觉置信因子，设定为1。其中第二项数量级过大，且没有进行归一化，ceres库是否能够正常使用？而且当ICP配准约束越多，则对绝对位姿约束的需求应该更小，但这里是相反的。

图9. super_odometry\src\LidarProcess\LidarSlam.cpp文件
addAbsolutePoseConstraints函数，第306-320行代码
（3） 雷达里程计中没有场景退化后的措施。
在雷达里程计的代码中，实现了可观测性标签的计数后，没有退化检测和后续额外添加约束，只是一个正常流程的点到面ICP配准。在退化场景中是如何实现的误差极小的定位和建图？

[wangdebin00223]

请问问题都解决了嘛？我也想了解一下

[mingren11]

@lzhich @shibowing Has it been resolved?

[wangdebin00223]

@lzhich @shibowing问题解决了吗？

no