SLAM 基础学习记录（2） 用载体位置轨迹来匹配点云数据得到点云地图

目的

利用位姿信息来进行点云配准，可以提高仅使用点云特征点来进行帧与帧的配准的准确性，并可以对闭环检测提供可靠的先验信息。

实现步骤：

step1：获取原始点云，并将其转换为可用的PCL点云；

step2：设置判断标志，以点云时间戳和IMU时间戳之差为判断依据；

step3：将点云经过旋转平移转换到世界坐标系；

step4：存储每一帧转换后的点云。

具体实现步骤

step1：获取原始点云，并将其转换为可用的PCL点云

通过velodyne 激光雷达的消息直接获取点云，是不能直接处理的，要先将获取到的点云转换为可用的ROSMsg类型。代码如下：

 pcl::PointCloud<pcl::PointXYZ>::Ptr laserIn(new pcl::PointCloud<pcl::PointXYZ> ());
            pcl::fromROSMsg(*velodyne_msg, *laserIn);

其中，*velodyne_msg为原始点云，*laserIn为转换消息类型的点云。

step2：设置判断标志，以点云时间戳和IMU时间戳之差为判断依据

1. 《SLAM 基础学习记录（1）》中提到，位姿信息有可能会出现NaN的情况，首先需要对IMU的处理后的位姿信息进行判断，判断其为非NaN，才进行点云转换的下一步判断，代码如下：

if (int(isnormal(_imuCur.positonX)) == 1 && int(isnormal(_imuCur.positonY)) == 1 && int(isnormal(_imuCur.positonY)) == 1) //判断数据非nan

{ }

2. velodyne 激光雷达的频率为10Hz， IMU 频率为100Hz，所以为了进行时间配准，以时间差0.1s为一个判断标志。

 float diffstamp = abs((_imuCur.stamp -  scanTime).toSec());
           if (diffstamp < 0.1)

{ }

step3：将点云经过旋转平移转换到世界坐标系

点云转换到世界（ENU）坐标系，先看一下LIDAR坐标系和IMU坐标系是如何定义的。