legoloam系列算法之点云处理与特征提取

一. lins和legoloam

lins和lego_loam的点云预处理环节是一样的, legoloam 在 imageProjection.cpp中完成, 而 lins 在image_projection_node.cpp中完成.

流程如下:

1.1 findStartEndAngle()

void findStartEndAngle()

作用: 获取激光的角度范围

雷达内部旋转扫描方向：Z轴俯视下来，顺时针方向（Z轴右手定则反向）

通过 findStartEndAngle() 对当前帧激光的角度进行编码

// 获取激光数据的角度范围   
void findStartEndAngle()
{
    // 雷达内部旋转扫描方向：Z轴俯视下来，顺时针方向（Z轴右手定则反向）

    // atan2(y,x)函数的返回值范围(-PI,PI],表示与复数x+yi的幅角
    // segMsg.startOrientation范围为(-PI,PI]
    // segMsg.endOrientation范围为(PI,3PI]
    // 因为内部雷达旋转是顺时针的
    segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
    // 下面这句话怀疑作者可能写错了，laserCloudIn->points.size() - 2应该是laserCloudIn->points.size() - 1
    segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
                                                 laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;

// segMsg.endOrientation - segMsg.startOrientation范围为(0,4PI)
    // 如果角度差大于3Pi或小于Pi，说明角度差有问题，进行调整。
    if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) 
    {    // 说明终点 角度为  pi - theta + 2*pi    起点的角度为 -pi + theta
        segMsg.endOrientation -= 2 * M_PI;    // 这样的话 endOrientation 就不需要 + 2*pi了    
    } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
    {   // 终点角度 < 0 (-pi左右), 起点>0 (pi左右)  
        segMsg.endOrientation += 2 * M_PI;
    }
// segMsg.orientationDiff的范围为(PI,3PI),一圈大小为2PI，应该在2PI左右
    segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
}

主要是求解3个量:

起始角度 segMsg.startOrientation 一般为0左右
终点角度 segMsg.endOrientation 一般为2pi左右
本帧角度范围 segMsg.orientationDiff

如下图, 注意激光雷达的ROS包输出的点云与激光雷达的默认坐标系不一样, 采用的是ROS的坐标系, 即前为x, 左为y.

速腾聚创16线手册给出的坐标系

1.2 projectPointCloud()

本函数主要求解下列数据:

对于任意一个激光点, 根据求得的距离图像坐标 (rowIdn, columnIdn) 生成距离图像 rangeMat

range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
rangeMat.at<float>(rowIdn, columnIdn) = range;

根据距离图像坐标(rowIdn, columnIdn) 重新编排点序号 index , 放置到 fullCloud

index = columnIdn  + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint;

构建 fullInfoCloud->points[index].intensity = range;

所以本函数关键就是对于任意一个激光点, 求取其在距离图像上的坐标 (rowIdn, columnIdn) , 求法如下:

4. 计算纵坐标(位于第几个scan)

// 计算竖直方向上的角度（雷达的第几线）
verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
// rowIdn计算出该点激光雷达是竖直方向上第几线的
// 从下往上计数，-15度记为初始线，第0线，一共16线(N_SCAN=16)
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
if (rowIdn < 0 || rowIdn >= N_SCAN)
   continue;

计算横坐标

这一步最复杂

// atan2(y,x)函数的返回值范围(-PI,PI],表示与复数x+yi的幅角
// 下方角度atan2(..)交换了x和y的位置，计算的是与y轴正方向的夹角大小(关于y=x做对称变换)
// 这里是在雷达坐标系，所以是与正前方的夹角大小
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

这一步得到的角度如下

在这里插入图片描述

得到角度后还要转换成列坐标

columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;    // ang_res_x  是指的是 激光点的角分辨率

角度分布与列数的关系如下图, H表示 Horizon_SCAN

在这里插入图片描述

可以看到, 第四像限的值大于H, 因此进行下面处理:

if (columnIdn >= Horizon_SCAN)
    columnIdn -= Horizon_SCAN;

最终得到角度对应列坐标的关系如下:

在这里插入图片描述

二. LIO-SAM

LIOSAM点云预处理主要是完成了融合IMU与里程计的畸变去除以及距离图像的生成和有效点的筛选.

特征提取的环节在 featureExtraction.cpp 中完成.

用于进行数据处理的部分位于 ImageProjection类,

同样有个 cv::Mat rangeMat 即距离图像 ,但是不进行聚类了,

构造函数里进行初始化操作,

ImageProjection():deskewFlag(0)
{   
    subImu        = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
    subOdom       = nh.subscribe<nav_msgs::Odometry>(odomTopic, 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
    subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());
    // 发布的数据 
    // 去畸变的点云以及信息  
    pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
    pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);

    allocateMemory();
    resetParameters();

    pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
}

订阅了IMU(用于计算旋转去畸变), 里程计(用于计算位移去畸变), 激光点云数据.

发布了 …

2.1 回调函数

IMU的回调函数:

// IMU回调函数     
// IMU数据处理后 存放在 imuQueue
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{   
    // 首先将IMU数据旋转到激光坐标系  
    sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
    std::lock_guard<std::mutex> lock1(imuLock);
    imuQueue.push_back(thisImu);
}

odom回调函数, 主要作用是获取前端的里程计数据然后执行畸变去除.

void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
{
    std::lock_guard<std::mutex> lock2(odoLock);
    odomQueue.push_back(*odometryMsg);
}

点云回调函数:

// 点云的回调函数     每一帧点云在这里完成最初的处理      
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    // 检查点云数据 
    if (!cachePointCloud(laserCloudMsg))
        return;

    if (!deskewInfo())
        return;

    projectPointCloud();
    // 依据距离图像上是否有值 来提取 用于下一阶段的特征提取的点云  
    cloudExtraction();
    // 发布点云  用于下一阶段的特征提取   
    publishClouds();
    // 参数清空  准备下一个点云    
    resetParameters();
}

2.2 点云处理

在点云回调中完成 cloudHandler()

在这里插入图片描述

2.2.1 cachePointCloud()

cachePointCloud()

用于:

将点云msg缓存在 cloudQueue 中, 然后取出队列中最早的一帧转换为:

pcl::PointCloud::Ptr laserCloudIn
然后做点云数据的检查, 检查是否去除了NaN点, 检查线束的信息的获取, 检查用于畸变的时间戳的获取, 从这里也看出 LIOSAM默认要求发送过来的点云数据是包含线束, 时间戳的信息的, 而不像 loam 那样自己求出来.

2.2.2 deskewInfo()

deskewInfo()

: 准备去除畸变所需的信息.

其中包含两个函数

imuDeskewInfo();
odomDeskewInfo();

imuDeskewInfo()

由于去除畸变需要求出每一个激光点的姿态信息, 所以这个函数先通过IMU数据求解出每一个激光点旋转信息.

思想是: 1. 先将当前激光帧时间范围内的IMU数据挑选出来 2. 先获取第一个IMU的姿态, 后续的IMU姿态通过角速度积分得出.

作用: 计算出 [timeScanCur – 0.01, timeScanNext + 0.01] 时间范围内IMU的旋转欧拉角.

要注意的是, 这个每个IMU的姿态角不是相对于世界坐标系, 而是相对于 [timeScanCur – 0.01, timeScanNext + 0.01] 时间范围内第一个IMU数据, 即第一个IMU数据的姿态被设置为了0 ,如下

// 对于第一个IMU   因此每一帧IMU的 imuRotX 都是相对于第一个IMU数据的欧拉角  
if (imuPointerCur == 0){
    imuRotX[0] = 0;
    imuRotY[0] = 0;
    imuRotZ[0] = 0;
    imuTime[0] = currentImuTime;
    ++imuPointerCur;
    continue;
}

后续IMU的姿态数据通过前一刻姿态 + 角速度 X 时间获得.

// get angular velocity   获取IMU角速度
double angular_x, angular_y, angular_z;
imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

// integrate rotation    每个IMU的姿态欧拉角由 上一时刻欧拉角与当前时刻角速度递推得到 
double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
imuTime[imuPointerCur] = currentImuTime;
++imuPointerCur;

odomDeskewInfo()

通过接收到的odom信息去计算出前后两帧的运动增量, 为后面的插值做准备.

odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre

实际上后面只用了 odomIncreX, odomIncreY, odomIncreZ 这几个位移增量.

2.2.3 projectPointCloud()

主要完成两个任务:

求取距离图像 rangeMat.
对点去畸变, 并放置到 fullCloud 点云中.

重点在与去畸变 deskewPoint().

deskewPoint()

/**
* @brief 为 point 去畸变
 * @param point 待去畸变的点 
 * @param relTime: 相对与本帧起始的时间 
 * @return PointType 去畸变后点
 */  
PointType deskewPoint(PointType *point, double relTime)
{
    // 之前会做去畸变检查  看是否满足去畸变条件 
    if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
        return *point;
    
    // 本点时间 = 相对时间 + 当前帧时间 
    double pointTime = timeScanCur + relTime;
    // 通过绝对时间插值出姿态  这个姿态指的是 相对于 deskewInfo()中选取的第一个IMU的坐标系 
    float rotXCur, rotYCur, rotZCur;
    findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
    // 通过相对时间获取位置增量即相对与起始坐标的位置         
    float posXCur, posYCur, posZCur;
    findPosition(relTime, &posXCur, &posYCur, &posZCur);
    // 每一帧的起始点会进行 
    if (firstPointFlag == true)
    {   // Tws^-1
        transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
        firstPointFlag = false;
    }
    
    // transform points to start   转换到该帧起始位置处坐标系  
    // Tworld<-curr  
    Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
    // 获得转换到第一个激光点坐标系的变换矩阵 
    Eigen::Affine3f transBt = transStartInverse * transFinal;    //  Tws^-1 * Twc = Tsc  
    // 将当前激光测量信息 转换到起始坐标系  Ps = Tsc * Pc
    PointType newPoint;
    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;
}

这样就将一帧激光数据的每一点,

转换到了该帧的第一个激光点对应的坐标系上

, 这就是去畸变的过程.

findRotation()

输入激光点的时间, 利用在imuDeskewInfo()中计算出的IMU姿态欧拉角, 插值出激光点处的欧拉角.

使用9轴IMU直接输出的姿态.

void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)

findPosition()

代码里说如果如果机器人移动速度慢, 这个位置补偿可以不用开启.

通过匀速运动模型, 根据 odomDeskewInfo()中计算的位移增量计算点的位移.

求解过程如下:

// 求时间比率 
float ratio = relTime / (timeScanNext - timeScanCur);
// 匀速运动模型  
*posXCur = ratio * odomIncreX;
*posYCur = ratio * odomIncreY;
*posZCur = ratio * odomIncreZ;

2.2.4 cloudExtraction()

依据距离图像上是否有值,来提取有效点到 extractedCloud , 并获取cloudInfo.

接着通过publishClouds()将

点云信息cloudInfo

以及

extractedCloud

发布出去.

这样LIOSAM的激光数据预处理就分析完毕!

原文链接：https://blog.csdn.net/qq_42700518/article/details/107624236

目录