IVox第一帧点云坐标系问题

YWL0720 commented 6 months ago

高博您好，感谢您的工作，在学习您的代码时，有这样的一个问题。按照我的理解，faster-lio估计的是IMU坐标系在世界坐标系下的状态，而世界坐标系应该与第一帧IMU坐标系重合。但在函数 LaserMapping::Run() 中，处理第一帧点云时，却直接将去畸变后，在雷达坐标系下的点云 scanundistort 加入到了ivox中

/// the first scan
    if (flg_first_scan_) {
        ivox_->AddPoints(scan_undistort_->points);
        first_lidar_time_ = measures_.lidar_bag_time_;
        flg_first_scan_ = false;
        return;
    }

这里是否应该也和后续的地图更新函数 LaserMapping::MapIncremental() 中的做法一致，将点云转换到世界坐标系下再加入到ivox中呢？

PointBodyToWorld(&(scan_down_body_->points[i]), &(scan_down_world_->points[i]));

期待着您的答复。

gaoxiang12 commented 6 months ago

这个其实有一些个人的考虑：因为fastlio系列整体用的点数比较少（大概每帧2k-3k点），但初始的时候如果点太少了，运动快了就可能找不到最近邻（而且初始的时候imu和系统状态的估计不够准确）。所以就在第一帧的时候多用一些点数来做后续帧的匹配。sad书也是类似的策略，并且sad书用的增量ndt，点少的时候无法准确估计ndt的分布。所以甚至可以把前一段时间的scan不作降采样，来增加前期可用的点数。

YWL0720 commented 6 months ago

感谢高博的回复，我理解您的意思。但是为什么不将第一帧的点云转换到世界坐标系下再加入到ivox中呢？ scanundistort 中点的坐标是在雷达坐标系下的，而第二帧以后 lasermapping::MapIncremental() 中均是使用转换后的世界坐标系下的点云来更新地图。如果IMU坐标系和雷达坐标系几乎重合，那可能不会有太大影响。但当二者之间存在较大的旋转或平移时，ivox中前两帧点云可能会发生错位。

YWL0720 commented 6 months ago

高博您好，我做了个简单的实验验证了这个问题，如果有分析不对的地方还希望您能给出指正。首先我注释掉了 laser_mapping.cc 中的第323行

// Timer::Evaluate([&, this]() { MapIncremental(); }, "    Incremental Mapping");

即不再进行地图更新，地图中仅保存第一帧点云。正常情况下仅依靠第一帧点云，在雷达移动距离较近时，也会约束住。应该表现为能维持一段时间的稳定位姿估计，当新的点云与第一帧点云共同区域较小时位姿估计失败。接下来我分别进行了三次测试

1. UrbanLoco Hong Kong Dataset Dataset 5: HK-Data20190117

roslaunch faster_lio mapping_velodyne_ulhk.launch

现象是直接沿横向飞走。 Screenshot from 2023-12-08 18-00-26

2. utbm utbm_robocar_dataset_20180713_noimage.bag

roslaunch faster_lio mapping_velodyne_utbm.launch

现象是平移震荡后维持了一段时间的稳定的估计。 Screenshot from 2023-12-08 17-52-58

3. hku_main_building.bag

roslaunch faster_lio mapping_avia.launch

现象是持续了一段时间的稳定估计，雷达与第一帧点云无共同视野时飘走。

4. 分析

ulhk数据集中雷达坐标系与IMU坐标系之间存在较大的旋转角度偏差，由于该角度偏差，导致随后世界坐标系下的点云无法与第一帧雷达坐标系下的点云配准成功，进而导致飞走。
```
extrinsic_T: [ 0, 0, -0.28]
extrinsic_R: [ 0, -1,  0, 1,  0,  0, 0,  0,  1]
```
utbm数据集中雷达坐标系与IMU坐标系之间外参平移分量较大，而旋转保持相同，因而能在几帧之内将位姿纠正，体现为平移震荡。
```
extrinsic_T: [ -0.5, 1.4, 1.5]
extrinsic_R: [ 1, 0, 0,  0, 1, 0,   0, 0, 1]
```
hku_main_building数据集使用avia雷达，其内置IMU，雷达坐标系和IMU坐标系在旋转和平移上都没有太大差异，因而测试起来效果最合乎预期。
```
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
extrinsic_R: [ 1, 0, 0, 0, 1, 0,  0, 0, 1 ]
```
5. 补充实验

我将第一帧的点云转换到世界坐标系下后，再加入到IVox中，重复上述实验，均为正常表现，即维持了一段时间的稳定位姿估计，当新的点云与第一帧点云共同区域较小时位姿估计失败。

6. 总结

根据上面的实验，我还是认为第一帧点云应当转换到世界坐标系下后再加入到IVox中。虽然实际上由于每次估计后会对地图进行更新，新的点云很快会覆盖掉第一帧点云，导致最终的差异并不明显。

期待着您的回复。

gaoxiang12 / faster-lio