从零开始激光雷达slam

0. 参考文献

https://zhuanlan.zhihu.com/p/104791974
任乾的激光雷达入门系列

1. kitti数据格式相关问题

1.1 lidar_to_imu变换矩阵

先看一下代码：

// 在主函数中构造一个tf listener类
std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");
// 定义一个变换矩阵
Eigen::Matrix4f lidar_to_imu = Eigen::Matrix4f::Identity();
// 然后进行一下查询
lidar_to_imu_ptr->LookupData(lidar_to_imu)

下面看一下lookupData函数，可以看到，是直接调用ros tf的函数

bool TFListener::LookupData(Eigen::Matrix4f& transform_matrix) {
    try {
        tf::StampedTransform transform;
        // base_frame_id = "velo_link"
        // child 
        listener_.lookupTransform(base_frame_id_, child_frame_id_, ros::Time(0), transform);
        TransformToMatrix(transform, transform_matrix);
        return true;
    } catch (tf::TransformException &ex) {
        return false;
    }
}

下面看一下tf文档的解释

velo_frame 是target_frame也就是说，寻找的是imu在velo下的表示${}_{imu}^{velo}T$，
通过运行代码，读取出这个变换矩阵的平移部分为：[-0.8086，0.3195，-0.7997]。
结合kitti官方文档图：

可以看出，确实是${}_{imu}^{velo}T$无疑。

在tag3.的代码中，下面这个应该是有错误，可以看到，代码首先把局部enu坐标，放到变换阵里，说明这个变换阵是${}_{imu}^{w}T$，其中w表示世界坐标系原点，然后为了得到激光雷达在世界坐标系的变换，应当这么做${}_{imu}^{w}T{\cdot }_{velo}^{imu}T{=}_{imu}^{w}T{\cdot }_{imu}^{velo}{T}^{-1}$
所以应该有个矩阵求逆才对。见下面代码的修改。

gnss_data.UpdateXYZ();
odometry_matrix(0,3) = gnss_data.local_E;
odometry_matrix(1,3) = gnss_data.local_N;
odometry_matrix(2,3) = gnss_data.local_U;
odometry_matrix.block<3,3>(0,0) = imu_data.GetOrientationMatrix();
// 原代码：odometry_matrix *= lidar_to_imu;
// 修改后：
odometry_matrix *= lidar_to_imu.inverse();