imageProjection

总体叙述
- 类成员变量
- 构造
- imu/odometryHandler
cloudHandler 点云回调
- cachePointCloud
- deskewInfo
- projectPointCloud
- - deskewPoint
- cloudExtraction

总体叙述

主要功能为：

利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，
对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，
变换当前激光点到起始时刻激光点的坐标系下，实现校正）；
最后用IMU数据的姿态角（RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。

struct VelodynePointXYZIRT
{PCL_ADD_POINT4D   // 位置PCL_ADD_INTENSITY;  //激光点云强度，也可存点的索引uint16_t ring;  //扫描线float time;     //时间戳，记录相对于当前帧第一个激光点的时差，第一个点time=0EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

类成员变量

互斥锁：std::mutex imuLock、odoLock
订阅原始点云
subLaserCloud、pubLaserCloud(未用)
发布当前激光帧运动畸变校正后的点云
pubExtractedCloud、pubLaserCloudInfo
imu数据队列（原始数据，转lidar系下）
subImu、imuQueue
激光点云数据队列
cloudQueue，currentCloudMsg(PointCloud2)
当前激光帧起止时刻间对应的imu数据，计算相对于起始时刻的旋转增量，以及时时间戳；
double *imuTime,*imuRotX,*imuRotY,*imuRotZ
- 目的：用于插值计算当前激光帧起止时间范围内，每一时刻的旋
当前帧起始时间+当前帧起始对应imu位姿变化 imuPointerCur
当前帧原始激光点云 + 当期帧运动畸变校正之后的激光点云
当前激光帧起止时刻对应imu里程计位姿变换，该变换对应的平移增量；
用于插值计算当前激光帧起止时间范围内，每一时刻的位置
odomIncreX odomIncreY odomIncreZ
当前帧激光点云运动畸变校正之后的数据，包括点云数据、初始位姿、姿态角等
lio_sam::cloud_info cloudInfo

构造

// 订阅原始imu数据
subImu        = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿
subOdom       = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅原始lidar数据
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/odometryHandler

imu回调

将imu原始测量数据转换到lidar系，加速度，角速度，PRY
将数据放入队列

odom回调

直接放入odom_deque

cloudHandler 点云回调

订阅原始lidar数据

 * 1、添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性* 2、当前帧起止时刻对应的imu数据、imu里程计数据处理*   imu数据：*   1) 遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角*   2) 用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0*   imu里程计数据：*   1) 遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿*   2) 用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量* 3、当前帧激光点云运动畸变校正*   1) 检查激光点距离、扫描线是否合规*   2) 激光运动畸变校正，保存激光点* 4、提取有效激光点，存extractedCloud* 5、发布当前帧校正后点云，有效点* 6、重置参数，接收每帧lidar数据都要重置这些参数

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{// 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性if (!cachePointCloud(laserCloudMsg))return;// 当前帧起止时刻对应的imu数据、imu里程计数据处理if (!deskewInfo())return;// 当前帧激光点云运动畸变校正projectPointCloud();// 提取有效激光点，存extractedCloudcloudExtraction();// 发布当前帧校正后点云，有效点publishClouds();// 重置参数，接收每帧lidar数据都要重置这些参数resetParameters();
}

cachePointCloud

添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性

缓存三帧，计算第一帧，检测数据的有效性+检测有time+ring通道

// 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性
bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{// cache point cloud cloudQueue.push_back(*laserCloudMsg);// 延迟了两帧？？if (cloudQueue.size() <= 2)return false;// convert cloud // 取出激光点云队列中最早的一帧currentCloudMsg = std::move(cloudQueue.front());cloudQueue.pop_front();if (sensor == SensorType::VELODYNE){// 转换成pcl点云格式pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);}else if (sensor == SensorType::OUSTER){// Convert to Velodyne format // 转换成Velodyne格式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_ERROR_STREAM("Unknown sensor type: " << int(sensor));ros::shutdown();}// get timestampcloudHeader = currentCloudMsg.header;timeScanCur = cloudHeader.stamp.toSec();// 当前帧结束时刻，注：点云中激光点的time记录相对于当前帧第一个激光点的时差，第一个点time=0timeScanEnd = timeScanCur + laserCloudIn->points.back().time;// check dense flag // 存在无效点，Nan或者Infif (laserCloudIn->is_dense == false){ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");ros::shutdown();}// check ring channel // 检查是否存在ring通道，注意static只检查一次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();}}// check point time  // 检查是否存在time通道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!");}return true;
}

deskewInfo

处理当前激光帧起止时刻对应的IMU数据、IMU里程计数据

要求imu数据包含激光数据，否则不往下处理了

if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
{ROS_DEBUG("Waiting for IMU data ...");return false;
}

2、当前帧对应imu数据处理

遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角
用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0

注：imu数据都已经转换到lidar系下了

void imuDeskewInfo()
{cloudInfo.imuAvailable = false;// 从imu队列中删除当前激光帧0.01s前面时刻的imu数据while (!imuQueue.empty()){if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)imuQueue.pop_front();elsebreak;}if (imuQueue.empty())return;imuPointerCur = 0;// 遍历当前激光帧起止时刻（前后扩展0.01s）之间的imu数据for (int i = 0; i < (int)imuQueue.size(); ++i){sensor_msgs::Imu thisImuMsg = imuQueue[i];double currentImuTime = thisImuMsg.header.stamp.toSec();// get roll, pitch, and yaw estimation for this scan// 提取imu姿态角RPY，作为当前lidar帧初始姿态角if (currentImuTime <= timeScanCur)imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);// 超过当前激光帧结束时刻0.01s，结束if (currentImuTime > timeScanEnd + 0.01)break;// 第一帧imu旋转角初始化if (imuPointerCur == 0){imuRotX[0] = 0;imuRotY[0] = 0;imuRotZ[0] = 0;imuTime[0] = currentImuTime;++imuPointerCur;continue;}// 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;}--imuPointerCur;// 没有合规的imu数据if (imuPointerCur <= 0)return;cloudInfo.imuAvailable = true;
}

3、当前帧对应imu里程计处理

遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿
用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量

注：imu数据都已经转换到lidar系下了

void odomDeskewInfo()
{cloudInfo.odomAvailable = false;// 从imu里程计队列中删除当前激光帧0.01s前面时刻的imu数据while (!odomQueue.empty()){if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)odomQueue.pop_front();elsebreak;}if (odomQueue.empty())return;// 要求必须有当前激光帧时刻之前的imu里程计数据if (odomQueue.front().header.stamp.toSec() > timeScanCur)return;// get start odometry at the beinning of the scan// 提取当前激光帧起始时刻的imu里程计nav_msgs::Odometry startOdomMsg;for (int i = 0; i < (int)odomQueue.size(); ++i){startOdomMsg = odomQueue[i];if (ROS_TIME(&startOdomMsg) < timeScanCur)continue;elsebreak;}// 提取imu里程计姿态角tf::Quaternion orientation;tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);double roll, pitch, yaw;tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);// Initial guess used in mapOptimization// 用当前激光帧起始时刻的imu里程计，初始化lidar位姿，后面用于mapOptmizationcloudInfo.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;// get end odometry at the end of the scanodomDeskewFlag = false;// 如果当前激光帧结束时刻之后没有imu里程计数据，返回if (odomQueue.back().header.stamp.toSec() < timeScanEnd)return;// 提取当前激光帧结束时刻的imu里程计nav_msgs::Odometry endOdomMsg;for (int i = 0; i < (int)odomQueue.size(); ++i){endOdomMsg = odomQueue[i];if (ROS_TIME(&endOdomMsg) < timeScanEnd)continue;elsebreak;}// 如果起止时刻对应imu里程计的方差不等，返回if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))return;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);// 起止时刻imu里程计的相对变换Eigen::Affine3f transBt = transBegin.inverse() * transEnd;// 相对变换，提取增量平移、旋转（欧拉角）float rollIncre, pitchIncre, yawIncre;pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);odomDeskewFlag = true;
}

projectPointCloud

当前帧激光点云运动畸变校正

void projectPointCloud()
{int cloudSize = laserCloudIn->points.size();// range image projectionfor (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;// 激光点`线数`不在范围内，舍弃int rowIdn = laserCloudIn->points[i].ring;if (rowIdn < 0 || rowIdn >= N_SCAN)continue;// 该点为降频点时，舍弃if (rowIdn % downsampleRate != 0)continue;// 计算水平角度，直接转为角度了 horizonAngle = 90 -anglefloat horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;static float ang_res_x = 360.0/float(Horizon_SCAN); // 角分辨率// 计算图像的列，[-180,180] int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;if (columnIdn >= Horizon_SCAN)columnIdn -= Horizon_SCAN;// 列数不能超过范围，否则 舍弃if (columnIdn < 0 || columnIdn >= Horizon_SCAN)continue;if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)continue;// 运动去畸变thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);// rangeMat 存放的长度rangeMat.at<float>(rowIdn, columnIdn) = range;// fullCloud 存放去畸变后的点云int index = columnIdn + rowIdn * Horizon_SCAN;fullCloud->points[index] = thisPoint;}
}

deskewPoint

input: 点云去畸变，当前点+距离起点的时间戳
output: 去畸变后的点云
function:
- 利用当前帧起止时刻之间的imu数据计算旋转增量，imu里程计数据计算平移增量，
- 进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿

Code:

PointType deskewPoint(PointType *point, double relTime)
{if (deskewFlag == -1 || cloudInfo.imuAvailable == false)return *point;// relTime是当前激光点相对于激光帧起始时刻的时间，pointTime则是当前激光点的时间戳double pointTime = timeScanCur + relTime;float rotXCur, rotYCur, rotZCur;// 在当前激光帧起止时间范围内，计算某一时刻的旋转（相对于起始时刻的旋转增量）// 由imu队列计算得到findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);float posXCur, posYCur, posZCur;// 在当前激光帧起止时间范围内，计算某一时刻的平移（相对于起始时刻的平移增量）// 由里程计队列得到findPosition(relTime, &posXCur, &posYCur, &posZCur);// 第一个点的位姿增量（0），求逆if (firstPointFlag == true){transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();firstPointFlag = false;}// transform points to start // 当前时刻激光点与第一个激光点的位姿变换Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);Eigen::Affine3f transBt = transStartInverse * transFinal;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;
}

cloudExtraction

提取有效激光点，把长度不符合去掉，且只保留有效点
记录了每条线起始和终止5个点的索引 cloudInfo.startRingIndex/endRingIndex

void cloudExtraction()
{// 有效激光点数量int count = 0;// extract segmented cloud for lidar odometryfor (int i = 0; i < N_SCAN; ++i){// 记录每根扫描线起始第5个激光点在一维数组中的索引cloudInfo.startRingIndex[i] = count - 1 + 5;for (int j = 0; j < Horizon_SCAN; ++j){// 有效激光点，if (rangeMat.at<float>(i,j) != FLT_MAX){// mark the points' column index for marking occlusion later// 记录激光点对应的Horizon_SCAN方向上的索引cloudInfo.pointColInd[count] = j;// save range info// 激光点距离cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);// save extracted cloud // 加入有效激光点extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);// size of extracted cloud++count;}}// 记录每根扫描线倒数第5个激光点在一维数组中的索引cloudInfo.endRingIndex[i] = count -1 - 5;}
}

LIO-SAM imageProjection相关推荐

SLAM相关学习资料：综述/激光/视觉/数据集/常用库
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨菠萝包包包@知乎来源丨https://zhuanlan.zhihu.com/p/4348743 ...
八种常用激光雷达和视觉SLAM算法的评估与比较
文章:Evaluation and comparison of eight popular Lidar and Visual SLAM algorithms 作者:Bharath Garigipati ...
国内外LiDAR SLAM实验室总结
知乎上有一个关于视觉的SLAM实验室以及20年前的视觉SLAM开源算法的汇总SLAM 领域国内外优秀实验室汇总,非常详细,但是偏视觉一点,本文主要关注LiDAR SLAM. 这篇文章一开始是非常简短的 ...
激光SLAM论文简单导读--LOAM、VLOAM、LeGO-LOAM、LIO-SAM、LVI-SAM、LIMO、LIC-FUSION、TVL-SLAM、R2LIVE、R3LIVE
激光SLAM论文简单导读--LOAM.VLOAM.LeGO-LOAM.LIO-SAM.LVI-SAM.LIMO.LIC-FUSION.TVL-SLAM.R2LIVE.R3LIVE 时间线开篇巨作LO ...
iSCSI target介绍及LIO实操入门
前文介绍了iSCSI的基本架构及启动器的基本操作,也就是在客户端的操作.今天我们介绍一下目标器的相关概念.开源实现和基本操作.Linux操作系统下面有很多目标器的开源实现,比如LIO.SCST和TGT ...
lio linux工具,Linux中三种SCSI target的介绍之LIO
1. 简介 Linux-IO Target在Linux内核中(linux 2.6.38后),用软件实现各种SCSI Target,其支持的SAN技术中所有流行的存储协议包括Fibre Channel( ...
2019.03.01 bzoj2555: SubString（sam+lct）
传送门题意简述: 要求在线支持两个操作 (1):在当前字符串的后面插入一个字符串 (2):询问字符串s在当前字符串中出现了几次?(作为连续子串) 思路: 考虑用lctlctlct来动态维护samsa ...
4566: [Haoi2016]找相同字符 SAM
折腾了好久.不过收获还是很多的.第一次自己去画SAM所建出来fail树.深入体会了这棵树的神奇性质. 当然,我最终靠着自己A掉了.(这是我第一次推SAM的性质(以前都是抄别人的,感觉自己好可耻),不过 ...
BZOJ1396:识别子串(SAM)
Description Input 一行,一个由小写字母组成的字符串S,长度不超过10^5 Output L行,每行一个整数,第i行的数据表示关于S的第i个元素的最短识别子串有多长. Sample I ...
由粗到精学习 LVI-SAM： imageProjection模块
一.LVI-SAM的节点关系运行LVI-SAM,并打开rqt_graph,查看节点关系如图所示: 激光雷达原始数据的topic为/points_raw,观察发现它仅被imageProjection节 ...

LIO-SAM imageProjection