LIO-SAM中利用IMU去运动畸变

Jun、、

已于 2024-01-05 15:49:43 修改

阅读量1.3k

点赞数 23

分类专栏： SLAM LIO-SAM 文章标签：自动驾驶学习算法

于 2023-11-24 14:47:07 首次发布

本文链接：https://blog.csdn.net/qq_44709827/article/details/134576437

版权

SLAM 同时被 2 个专栏收录

10 篇文章 4 订阅

订阅专栏

LIO-SAM

2 篇文章 0 订阅

订阅专栏

LIO-SAM系列文章目录

SLAM学习小白记录LIO-SAM中使用IMU数据去运动畸变的方法和程序

文章目录

LIO-SAM系列文章目录
前言
一、为什么要去运动畸变
- 1、什么是传感器的运动畸变？
二、LIO-SAM中如何去激光点云运动畸变
总结

前言

最近马上要开题了，后续的工作想要基于LIO-SAM框架进行开展，因此回过头回顾一下LIO-SAM中一些关键细节，并做记录方便自己后续回顾。

一、为什么要去运动畸变

1、什么是传感器的运动畸变？

相信大家应该都大致了解一点什么是运动畸变。简单的拿激光雷达进行举例，激光雷达是由高速旋转的激光发射器构成，雷达不断像外发射和激光，从而我们可以得到一个由大量点云组成激光点云扫描SCAN。如果搭载了激光雷达的机器人在原地不动，那么得到激光扫描自然不会有什么畸变，但是机器人一般都是在运动的，并且有些机器人运动的速度较快或者激光雷达输出频率较低，那么一个激光雷达扫描虽然是一起输出的，但是这个扫描SCAN中的各个点云它们的发射和接收的位置就有差异，这就是激光雷达的运动畸变。下图可以更直观地看出运动畸变：

在这里插入图片描述

二、LIO-SAM中如何去激光点云运动畸变

1、简单介绍LIO-SAM中去运动畸变的原理

2、代码分析

先上代码：

（1）projectPointCloud函数

   void projectPointCloud()
    {
        int cloudSize = laserCloudIn->points.size();// 获取输入点云数量
        // range image projection
        // 遍历激光点云
        for (int i = 0; i < cloudSize; ++i)
        {
            // 获取激光点云数据
            PointType thisPoint;
            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;
            thisPoint.intensity = laserCloudIn->points[i].intensity;

            // 计算点云的距离并根据该距离判断是否在激光点云最小范围和最大范围之间
            float range = pointDistance(thisPoint);
            if (range < lidarMinRange || range > lidarMaxRange)
                continue;

            // 获取激光点云的通道数 并判断该通道数是否小于激光雷达总线数N_SCAN
            int rowIdn = laserCloudIn->points[i].ring;
            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;

            if (rowIdn % downsampleRate != 0)
                continue;

            int columnIdn = -1;
            if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
            {
                //水平角分辨率                   
                float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
                //Horizon_SCAN=1800,每格0.2度
                static float ang_res_x = 360.0/float(Horizon_SCAN);
                columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
                if (columnIdn >= Horizon_SCAN)
                    columnIdn -= Horizon_SCAN;
            }
            else if (sensor == SensorType::LIVOX)
            {
                columnIdn = columnIdnCountVec[rowIdn];
                columnIdnCountVec[rowIdn] += 1;
            }
            
            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;

            if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
                continue;
            //去畸变  运动补偿 这里需要用到雷达信息中的time 这个field            
            thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
            //图像中填入欧几里得深度
            rangeMat.at<float>(rowIdn, columnIdn) = range;
            // 转换成一维索引，存校正之后的激光点
            int index = columnIdn + rowIdn * Horizon_SCAN;
            fullCloud->points[index] = thisPoint;
        }
    }

总结一下这个函数，大致的作用为：
遍历已经检查过的激光点云，再进行激光点云深度和扫描线束检测(降采样等)，根据激光点云坐标数据（x，y）和线束ring将激光点云投影到深度图中。

注意atan2(x,y);函数：
在这里插入图片描述
这里以Horizon_SCAN=1800举例，那么上面代码中得到columnIdn值对应(x,y的坐标)应该如下图所示：
横轴为x，纵轴为y

这也是为什么代码中要有以下代码的原因：

      //水平角分辨率                   
      float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
      //Horizon_SCAN=1800,每格0.2度
      static float ang_res_x = 360.0/float(Horizon_SCAN);
      columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
      if (columnIdn >= Horizon_SCAN)
          columnIdn -= Horizon_SCAN;

即根据激光点云的x，y坐标将点云投影到深度图（论文里一般叫做rang image）中。
深度图其实可以理解为：激光点云原本是一个近似半球形，然后从中间劈开，展开为一张图片，图片的分辨率为上面代码中的rowIdn*columnIdn，每个格子的像素点为激光点云距离激光雷达的距离。

然后就是去运动畸变了，主要在deskewPoint函数中进行。

（2）deskewPoint函数

   /* 去运动畸变函数
        利用当前帧起止时刻之间的imu数据计算旋转增量，
        imu里程计数据计算平移增量，进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿
     */
    PointType deskewPoint(PointType *point, double relTime)
    {
        //这个来源于上文的时间戳通道和imu可用判断，没有或是不可用则返回点        
        if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
            return *point;

        // relTime=points[i].time 即激光点云时间戳数据
        // lasercloudin中存储的time是一帧中距离起始点的  相对时间
        // 在cloudHandler的cachePointCloud函数中，timeScanCur = cloudHeader.stamp.toSec();，即当前帧点云的初始时刻
        //二者相加即可得到当前点的准确时刻
        double pointTime = timeScanCur + relTime;

        //根据时间戳插值获取imu计算的旋转量与位置量（注意imu计算的相对于起始时刻的旋转增量）
        float rotXCur, rotYCur, rotZCur;
        findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
        // 这里位移为 0
        float posXCur, posYCur, posZCur;
        findPosition(relTime, &posXCur, &posYCur, &posZCur);

        //这里的firstPointFlag来源于resetParameters函数，而resetParameters函数每次ros调用cloudHandler都会启动
        if (firstPointFlag == true)
        {
            // 这个函数接受三个平移参数（x、y、z）和三个旋转参数（roll、pitch、yaw），然后返回一个 4x4 的 Eigen 矩阵，
            // 即表示由这些平移和旋转组成的变换矩阵。
            transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
            firstPointFlag = false;//改成false以后，同一帧激光数据的下一次就不执行了
        }

        // transform points to start
        //扫描当前点时lidar的世界坐标系下变换矩阵
        Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
        //扫描该点相对扫描本次scan第一个点的lidar变换矩阵=
        //第一个点时lidar世界坐标系下变换矩阵的逆×当前点时lidar世界坐标系下变换矩阵
        //Tij=Twi^-1 * Twj
        //注：这里准确的来说，不是世界坐标系,
        //根据代码来看，是把imu积分:
        //从imuDeskewInfo函数中，在当前激光帧开始的前0.01秒的imu数据开始积分，
        //把它作为原点，然后获取当前激光帧第一个点时刻的位姿增量transStartInverse，
        //和当前点时刻的位姿增量transFinal，根据逆矩阵计算二者变换transBt。
        //因此相对的不是“世界坐标系”，
        //而是“当前激光帧开始前的0.01秒的雷达坐标系（在imucallback函数中已经把imu转换到了雷达坐标系了）
        Eigen::Affine3f transBt = transStartInverse * transFinal;
        PointType newPoint;
	    //根据lidar位姿变换 Tij，修正点云位置: Tij * Pj  ？？？？
        newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
        newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
        newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
        newPoint.intensity = point->intensity;
        return newPoint;
    }

上面函数的主要作用是：

判断激光点云时间数据和用于去运动畸变的IMU数据是否可用
根据IMU数据计算当前时刻相对于最近一帧激光点云帧的旋转（位姿）
根据IMU数据得到相对旋转量补偿当前激光点云数据
将激光点云投影到深度图中，并保存校正之后每个点到fullCloud中并保存该点的一维索引

下面画图说明：
在这里插入图片描述
上面使用了IMU数据得到的旋转矩阵和位移组成的变换矩阵，那么这个旋转矩阵是怎么来的呢？记下来看findRotation函数。

    /* 通过IMU数据差值计算当前激光点云时刻对应的旋转量 */
    void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
    {
        *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;

        int imuPointerFront = 0;
        // 寻找大于当前激光点云时间的最前面的IMU数据
        while (imuPointerFront < imuPointerCur)
        {
            if (pointTime < imuTime[imuPointerFront])
                break;
            ++imuPointerFront;
        }
        //如果计数为0或该次imu时间戳小于了当前时间戳(异常退出)
        if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
        {
            *rotXCur = imuRotX[imuPointerFront];
            *rotYCur = imuRotY[imuPointerFront];
            *rotZCur = imuRotZ[imuPointerFront];
        } else {
            // 前后时刻插值计算当前时刻的旋转增量
            //此时front的时间是大于当前pointTime时间，back=front-1刚好小于当前pointTime时间，前后时刻插值计算
            int imuPointerBack = imuPointerFront - 1;
            //算一下该点时间戳在本组imu中的位置
            double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
            
            double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
            //这三项作为函数返回值，以形参指针的方式返回
            //按前后百分比赋予旋转量
            *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
            *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
            *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
        }
    }