-
LIO-SAM：点云预处理前端---畸变矫正数据预处理
前言
LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。
LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。其中点云运动畸变矫正的代码在图像投影的节点中
可以看到该节点 订阅 3种消息：- 原始点云数据
- 原始imu数据
- imu预积分后预测的imu里程计数据
其中完成的一个主要功能就是进行畸变矫正。本篇博客主要解读其畸变矫正数据预处理部分。
激光雷达畸变矫正
什么是激光雷达的运动畸变 ?
激光雷达的一帧数据是过去一周期内形成的所有数据,数据仅有一时间戳,而非某个时刻的数据,因此在这一帧时间内的激光雷达或者其载体通常会发生运动,因此,这一帧的原点不一致,会导致一些问题,这个问题就是运动畸变
所以需要去运动畸变,也叫畸变校正如何进行运动补偿?
运动补偿的目的就是把所有的点云补偿到某一时刻,这样就可以把本身在过去100ms内收集的点云统一到一个时间点上去 这个时间点可以是起始时刻,也可以是结束时刻,也可以是中间的任意时刻
常见的是补偿到起始时刻
Pstart = T_start_current * Pcurrent
畸变校准方法
因此运动补偿需要知道每个点时刻对应的位姿 T_start_current 通常有几种做法1 如果有高频里程计,可以比较方便的获取每个点相对起始扫描时刻的位姿
2 如果有imu,可以方便的求出每个点相对起始点的旋转
3 如果没有其它传感器,可以使用匀速模型假设,使用上一帧间里程计的结果,作为当前两帧之间的运动,同时假设当前帧也是匀速运动,也可以估计出每个点相对起始时刻的位姿
k-1 到 k 帧 和 k到k+1帧的运动是一至的,用k-1到k帧的位姿变换当做k到k+1帧的位姿变换, 可以求到k到k+1帧的每个点的位姿变换畸变矫正数据预处理
作者从lego-loam开始比较喜欢的一个操作，将点云投到 16*1800的cvMat上去，方便做后续的一些处理
这部分代码在 imageProjection.cpp 文件中
首先是标准的ros节点初始化
int main(int argc, char** argv) { ros::init(argc, argv, "lio_sam"); ImageProjection IP;//图像投影对象 ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m"); //三个线程 ros::MultiThreadedSpinner spinner(3); spinner.spin(); return 0; }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
开了三个线程
然后则继续看ImageProjection 这个类
首先看其构造函数ImageProjection(): deskewFlag(0) {- 1
- 2
- 3
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay()); subOdom = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay()); subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());- 1
- 2
- 3
订阅
- imu数据，
- imu预积分预测的里程计数据，
- 原始点云数据
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);- 1
- 2
发布
- 运动补偿后的点云
- 点云相关信息
allocateMemory(); // 为各个指针分配内存 resetParameters(); // 为其他参数进行初始化与重置- 1
- 2
进行各个指针分配内存及参数初始化
下面来看imu的回调函数 imuHandler
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg) { sensor_msgs::Imu thisImu = imuConverter(*imuMsg); std::lock_guard<std::mutex> lock1(imuLock); imuQueue.push_back(thisImu);- 1
- 2
- 3
- 4
- 5
对imu做一个坐标转换
加一个线程锁，把imu数据保存进队列下面来看里程计的回调函数 odometryHandler
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg) { std::lock_guard<std::mutex> lock2(odoLock); odomQueue.push_back(*odometryMsg); }- 1
- 2
- 3
- 4
- 5
把里程计数据存入到 里程计队列里面去
主要的操作在原始的点云回调中cloudHandler
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)- 1
输入就是 激光雷达 发布的点云的消息
在里面实现了 6个 函数，对应着六个功能if (!cachePointCloud(laserCloudMsg)) //缓存点云消息 并进行相关检查 return; if (!deskewInfo())//获得运动补偿所需要的信息 return; projectPointCloud(); cloudExtraction(); publishClouds(); resetParameters();- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
首先看第一个函数
cachePointCloudbool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) {- 1
- 2
cloudQueue.push_back(*laserCloudMsg); if (cloudQueue.size() <= 2) return false; currentCloudMsg = std::move(cloudQueue.front()); cloudQueue.pop_front();- 1
- 2
- 3
- 4
- 5
点云数据保存进队列
确保队列里大于两帧点云数据
缓存了足够的点云之后
取出第一帧点云if (sensor == SensorType::VELODYNE)//处理VELODYNE的激光雷达 { // 将ros格式的点云 转成 自定义的pcl格式的点云 pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);//将ros的点云类型转换为 pcl的 } else if (sensor == SensorType::OUSTER)//处理OUSTER的激光雷达 { // Convert to Velodyne format pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn); laserCloudIn->points.resize(tmpOusterCloudIn->size()); laserCloudIn->is_dense = tmpOusterCloudIn->is_dense; for (size_t i = 0; i < tmpOusterCloudIn->size(); i++) { auto &src = tmpOusterCloudIn->points[i]; auto &dst = laserCloudIn->points[i]; dst.x = src.x; dst.y = src.y; dst.z = src.z; dst.intensity = src.intensity; dst.ring = src.ring; dst.time = src.t * 1e-9f; } } else//如果是其它类型 激光雷达 报错 关闭ros { ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor)); ros::shutdown(); }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
根据不同型号的激光雷达，转成pcl的点云格式
cloudHeader = currentCloudMsg.header; timeScanCur = cloudHeader.stamp.toSec(); timeScanEnd = timeScanCur + laserCloudIn->points.back().time;- 1
- 2
- 3
获得该帧点云的时间戳
- 起始点时间
- 结束点时间
header里面的时间就是起始点的时间
points.back().time 的时间就是 最后一个点相对于第一点的时间
所以这里就要求激光雷达要开启每个点的时间偏移信息
if (laserCloudIn->is_dense == false) { ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!"); ros::shutdown(); }- 1
- 2
- 3
- 4
- 5
检测点云是否有序排列的
static int ringFlag = 0; if (ringFlag == 0) { ringFlag = -1; for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i) { if (currentCloudMsg.fields[i].name == "ring") { //有一个点有就行了 ringFlag = 1; break; } } if (ringFlag == -1) { //没有的话会 运行不下去 ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!"); ros::shutdown(); } }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
因为ringFlag 是 static 所以只会检查一次
检测点云中的点 是否又ring的消息
ring 代表点云处理第几根scan
没有信息则直接崩掉if (deskewFlag == 0) { deskewFlag = -1; for (auto &field : currentCloudMsg.fields) { if (field.name == "time" || field.name == "t") { //有一个点有就行了 deskewFlag = 1; break; } } if (deskewFlag == -1) //没有也可以运行 但是 去畸变无法生效 ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!"); }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
检查是否有时间戳消息
然后则看 deskewInfo()的函数
bool deskewInfo() { std::lock_guard<std::mutex> lock1(imuLock); std::lock_guard<std::mutex> lock2(odoLock);- 1
- 2
- 3
- 4
if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd) { ROS_DEBUG("Waiting for IMU data ..."); return false; }- 1
- 2
- 3
- 4
- 5
确保imu的数据覆盖这一帧点云
imuDeskewInfo();- 1
准备imu补偿信息
看里面具体内容void imuDeskewInfo() { cloudInfo.imuAvailable = false;- 1
- 2
- 3
先将 imu可用标志位清0
while (!imuQueue.empty()) { if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01) imuQueue.pop_front(); else break; } if (imuQueue.empty()) return;- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
扔掉过早的imu数据
for (int i = 0; i < (int)imuQueue.size(); ++i) {- 1
- 2
处理imu数据队列里面每个数据
sensor_msgs::Imu thisImuMsg = imuQueue[i]; double currentImuTime = thisImuMsg.header.stamp.toSec(); if (currentImuTime <= timeScanCur) imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);- 1
- 2
- 3
- 4
- 5
提取要处理的 imu数据
提取处理的这个imu的时间
离当前帧起始时刻最近的imu，把imu的姿态转换成欧拉角if (currentImuTime > timeScanEnd + 0.01) break;- 1
- 2
这一帧遍历完了 就 break
if (imuPointerCur == 0){ imuRotX[0] = 0; imuRotY[0] = 0; imuRotZ[0] = 0; imuTime[0] = currentImuTime; ++imuPointerCur; continue; }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
起始帧 的 角度初始为0
double angular_x, angular_y, angular_z; imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);- 1
- 2
- 3
取出当前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;- 1
- 2
- 3
- 4
- 5
- 6
计算每一个时刻的姿态角，方便后续查找对应每个点云时间的值
--imuPointerCur; if (imuPointerCur <= 0) return; cloudInfo.imuAvailable = true;//imu数据可用 标志位 至1- 1
- 2
- 3
- 4
- 5
- 6
imu数据可用 标志位 至1
odomDeskewInfo();- 1
然后是这个函数
功能是准备里程计补偿信息void odomDeskewInfo() { cloudInfo.odomAvailable = false; while (!odomQueue.empty()) { if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01) odomQueue.pop_front(); else break; }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
扔掉过早的数据
if (odomQueue.empty()) return; if (odomQueue.front().header.stamp.toSec() > timeScanCur) return;- 1
- 2
- 3
- 4
- 5
排除错误的时间戳情况
for (int i = 0; i < (int)odomQueue.size(); ++i) { startOdomMsg = odomQueue[i]; if (ROS_TIME(&startOdomMsg) < timeScanCur) continue; else break; }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
找到对应最早的点云时间的odom数据
tf::Quaternion orientation; tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);- 1
- 2
将ros消息格式中的姿态转成tf的格式
double roll, pitch, yaw; tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);- 1
- 2
将四元数转成 欧拉角
cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x; cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y; cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z; cloudInfo.initialGuessRoll = roll; cloudInfo.initialGuessPitch = pitch; cloudInfo.initialGuessYaw = yaw; cloudInfo.odomAvailable = true;- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
记录点云起始时刻的对应的odom姿态
有助于后续地图模块的初始位姿估计
cloudInfo.odomAvailable 就是标记 odom 提供了这一帧点云的初始位姿if (odomQueue.back().header.stamp.toSec() < timeScanEnd) return;- 1
- 2
这里发现没有覆盖到最后的点云，那就不能用odom数据来做运动补偿
for (int i = 0; i < (int)odomQueue.size(); ++i) { endOdomMsg = odomQueue[i]; if (ROS_TIME(&endOdomMsg) < timeScanEnd) continue; else break; }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
找到点云最晚时刻对应的odom数据
if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0]))) return;- 1
- 2
这个代表odom退化了，就置信度不高了。直接return ，不使用odom进行补偿
Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);- 1
- 2
起始位姿和结束位姿 都转成 Affine3f 这个数据结构
Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw); tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation); tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);- 1
- 2
- 3
- 4
- 5
起始位姿和结束位姿 都转成 Affine3f 这个数据结构
float rollIncre, pitchIncre, yawIncre; pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);- 1
- 2
将这个增量转换成x y z和欧拉角的形式
odomDeskewFlag = true;- 1
运行结束，前面的判断没有return掉，则表示可以用odom来进行运动补偿
总结
-
相关阅读:
Ubuntu衍生发行版使用体验（lubuntu、xubuntu、kubuntu）
ceph报错总结
IDEA创建动态web工程并配置Tomcat服务器
介绍两个LVGL开发工具，让你做出更好的UI
软件结构化设计-架构真题（二十七）
第一个servlet项目:pro07-javaweb-begin
[SQL开发笔记]WHERE子句 : 用于提取满足指定条件的记录
题目0144-最大利润
高复用性自动化脚本设计实践
解决 Xshell 无法使用 zsh 的 prompt style
- 原文地址：https://blog.csdn.net/qq_32761549/article/details/126400840