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() - 1segMsg.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 + thetasegMsg.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^-1transStartInverse = (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 * PcPointType 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的激光数据预处理就分析完毕!