文章目录
这部分在整个框架中的位置如上图红色框图中所示，这个节点订阅了雷达的点云数据，imu数据和里程计数据，经过计算输出作者自己定义的点云信息。
功能：点云投影成深度图，并分类，利用ring和time给每个点去畸变。
订阅：
imuTopic
odomTopic+"_incremental"
pointCloudTopic
发布：
lio_sam/deskew/cloud_deskewed
lio_sam/deskew/cloud_info
程序流程如下：
- imu回调函数： 存入队列imuQueue，这里有一点注意的就是imuConverter,这个函数在utility.h中定义，它的功能是将imu坐标系的数据转移到雷达坐标系下。
- 里程计回调函数：存入队列odomQueue
- 点云话题回调函数：接收数据存入cloudQueue,并会处理以上两个回调接收的数据。
- cloudHandler回调函数包含几个重要函数：
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
//处理点云信息，判断点云数量是否大于2，cloud ring判断，时间判断
if (!cachePointCloud(laserCloudMsg))
return;
if (!deskewInfo())//计算去畸变所需的参数
return;
projectPointCloud();//将点云投影成深度图，投影到距离图像中 （类似Lego-loam）
cloudExtraction();//点云提取
publishClouds(); //发布点云
resetParameters();//重置参数
}
下面按照程序执行流程依次进行解读
cachePointCloud(laserCloudMsg)
这个函数实际上是真正的雷达接收函数，将接收的数据存入cloudQueue当中，取队列当中的front数据进行存入currentCloundMsg,并删除cloudQueue的front数据。最终会转成pcl格式的laserCloudIn数据。
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());//把前端数据拷贝到currentCloudMsg中
cloudQueue.pop_front();//前端删除
if (sensor == SensorType::VELODYNE)
{
pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);//ros的格式转到pcl的格式
}
else if (sensor == SensorType::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_ERROR_STREAM("Unknown sensor type: " << int(sensor));
ros::shutdown();
}
// get timestamp
cloudHeader = currentCloudMsg.header;
timeScanCur = cloudHeader.stamp.toSec();
timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
// check dense flag
if (laserCloudIn->is_dense == false)
{
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
// check ring channel
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，这个在处理畸变的时候会起作用。
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()
这个函数用于畸变的相关关键信息或变量值的计算
bool deskewInfo()
{
std::lock_guard<std::mutex> lock1(imuLock);
std::lock_guard<std::mutex> lock2(odoLock);
// make sure IMU data available for the scan
//imu队列顶部元素的时间需要小于当前激光扫描时间 imu队列底部时间又要大于下一帧扫描的时间戳
//其实就是保证imu有数据并且包含扫描帧间时间 ，论文中介绍的插值时间同步
//所以imu的频率要求比较高，作者使用的是500hz
if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
{
ROS_DEBUG("Waiting for IMU data ...");
return false;
}
imuDeskewInfo();//用于处理畸变：计算imu转角
odomDeskewInfo();//读取odom数据，并根据协方差判断是否相信
return true;
}
//计算当前激光时间戳前，每个时刻imu累计出的角速度，并判定imu数据是否可用（数量够）
void imuDeskewInfo()
{
//这个参数在地图优化mapOptmization.cpp程序中用到，首先为false 完成相关操作后置true
cloudInfo.imuAvailable = false;
//直到imu的时间戳到当前scan时间戳前0.01s以内，筛选imu数据
while (!imuQueue.empty())
{
if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
imuQueue.pop_front();
else
break;
}
if (imuQueue.empty())
return;
imuPointerCur = 0;
//循环取出imu队列中的数据
for (int i = 0; i < (int)imuQueue.size(); ++i)
{
sensor_msgs::Imu thisImuMsg = imuQueue[i];
double currentImuTime = thisImuMsg.header.stamp.toSec();
//当前imu时间戳小于当前帧点云时间戳转换，大于下一帧+0.01s退出
// get roll, pitch, and yaw estimation for this scan
if (currentImuTime <= timeScanCur)
imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
if (currentImuTime > timeScanEnd + 0.01)
break;
//赋0
if (imuPointerCur == 0){
imuRotX[0] = 0;
imuRotY[0] = 0;
imuRotZ[0] = 0;
imuTime[0] = currentImuTime;
++imuPointerCur;
continue;
}
// get angular velocity
double angular_x, angular_y, angular_z;
//这个也是utility.h中的函数，获得角速度
imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
// integrate rotation
//积分，得到转角，用于处理畸变
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; //把赋值0后面的自加减回去
if (imuPointerCur <= 0)//如果数量为0直接返回，不为0，该imu属性为可用
return;
cloudInfo.imuAvailable = true;//地图优化中会用到
}
void odomDeskewInfo()
{
cloudInfo.odomAvailable = false;//和imu一样，先赋值为false，处理完了再true
while (!odomQueue.empty())
{
if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
odomQueue.pop_front();
else
break;
}
if (odomQueue.empty())
return;
if (odomQueue.front().header.stamp.toSec() > timeScanCur)
return;
// get start odometry at the beinning of the scan
nav_msgs::Odometry startOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
startOdomMsg = odomQueue[i];
//由于之前已经将小于timeScanCur超过0.01的数据推出 所以startOdomMsg已经可代表起始激光扫描的起始时刻的里程计消息
if (ROS_TIME(&startOdomMsg) < timeScanCur)
continue;
else
break;
}
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
//利用里程计消息生成初始位姿估计，在地图优化中使用的，具体需要看对应代码
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;
// get end odometry at the end of the scan
//获得一帧扫描末尾的里程计消息，这个就跟初始位姿估计没有关系，只是用于去畸变运动补偿
odomDeskewFlag = false;
//相关时间先后关系的判断 odom队列中最后的数据大于下一帧扫描的时间
if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
return;
nav_msgs::Odometry endOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
endOdomMsg = odomQueue[i];
if (ROS_TIME(&endOdomMsg) < timeScanEnd)
continue;
else
break;
}
//位姿协方差矩阵判断
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);
//获得一帧扫描起始与结束时刻间的变换 这个在loam里解释的比较清楚
Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
//通过tranBt 获得增量值 后续去畸变用到
float rollIncre, pitchIncre, yawIncre;
pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
//标志变量
odomDeskewFlag = true;
}
projectPointCloud()
这个函数用于将点云信息laserCloudIn投影到深度图信息，并进行运动去畸变deskewPoint()。
void projectPointCloud()
{
int cloudSize = laserCloudIn->points.size();//点云数量
// range image projection
//距离图像：就是把点云数据按线束 按行列保存
for (int i = 0; i < cloudSize; ++i)//将laserCloudIn数据存到thisPoint中
{
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); //调用utility.h里的函数计算点到雷达的距离
if (range < lidarMinRange || range > lidarMaxRange)//点的过滤，太近太远都不要
continue;
//从点云数据中获得线束信息作为行最终距离图像就是16*1800
int rowIdn = laserCloudIn->points[i].ring;
if (rowIdn < 0 || rowIdn >= N_SCAN)//16线束
continue;
if (rowIdn % downsampleRate != 0)
continue;
//激光点的水平角度
float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
//角分辨率 360/1800
static float ang_res_x = 360.0/float(Horizon_SCAN);
//计算在距离图像上点属于哪一列
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;
//去畸变 运动补偿 这里需要用到雷达信息中的time 这个field
thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
//cv::Mat类型的rangMat生成 距离图像 16*1800
rangeMat.at<float>(rowIdn, columnIdn) = range;
//索引值 类似于图像中像素索引的概念应该比较好理解
int index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint; //fullCloud填充内容 这里是运动补偿去畸变后的点云
}
}
deskewPoint()
这个是真正的运动去畸变函数，可以当做一个工具，平时处理雷达数据的时候也可以使用。
去畸变后的函数保存点云到thisPoint—》fullCloud—》extractedCloud最后通过cloudInfo发布出去。
PointType deskewPoint(PointType *point, double relTime)
{
if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
return *point;
//点在一帧中的具体时间，这个time就有用了
double pointTime = timeScanCur + relTime;
float rotXCur, rotYCur, rotZCur; //定义相关变量用于补偿 旋转 平移
//调用本程序文件内函数，获得相关旋转量
findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
float posXCur, posYCur, posZCur;
//调用本程序文件函数，获得相关平移量
findPosition(relTime, &posXCur, &posYCur, &posZCur);
//如果是第一此收到数据
if (firstPointFlag == true)
{
// 起始变换矩阵赋初值再取逆
transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
firstPointFlag = false;
}
// transform points to start
// 把点投影到每一帧扫描的起始时刻 参考Loam里的方案
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;
}
另外运动去畸变调用了两个工具类函数：
void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
{
*rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
int imuPointerFront = 0;
while (imuPointerFront < imuPointerCur)
{
if (pointTime < imuTime[imuPointerFront])
break;
++imuPointerFront;
}
if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
{
*rotXCur = imuRotX[imuPointerFront];
*rotYCur = imuRotY[imuPointerFront];
*rotZCur = imuRotZ[imuPointerFront];
} else {
//根据点的时间信息 获得每个点的时刻的旋转变化量
int imuPointerBack = imuPointerFront - 1;
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;
}
}
void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
{
//作者这里注释掉了 如果高速移动可能有用 低速车辆提升不大 用到了里程计增量值
*posXCur = 0; *posYCur = 0; *posZCur = 0;
// If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.
// if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)
// return;
// float ratio = relTime / (timeScanNext - timeScanCur);
// *posXCur = ratio * odomIncreX;
// *posYCur = ratio * odomIncreY;
// *posZCur = ratio * odomIncreZ;
}
cloudExtraction()
点云提取函数，提取完了之后保存到pcl格式的extractedCloud中
void cloudExtraction()
{
int count = 0;
// extract segmented cloud for lidar odometry
for (int i = 0; i < N_SCAN; ++i)
{
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
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;
}
}
cloudInfo.endRingIndex[i] = count -1 - 5;
}
}
void publishClouds()
{
//最终发布函数在utility.h中的publishCloud。
//需要将pcl格式转换成ROS格式点云
cloudInfo.header = cloudHeader;
cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
pubLaserCloudInfo.publish(cloudInfo);
}