文章目录
代码：
https://github.com/TixiaoShan/LIO-SAM
imageProjection.cpp主要分为两大块，一个是类似于Lego-loam里的将点云投射到距离图像，另一个是去除运动畸变。
1. 初始化
deskewFlag(0)
{
//订阅话题进入回调函数 imu数据 地图优化程序中发布的里程计话题（增量式） 激光点云
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());
//发布去畸变的点云
pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
//发布激光点云信息 这里建议看一下自定义的lio_sam::cloud_info的msg文件 里面包含了较多信息
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);
//分配内存
allocateMemory();
//重置部分参数 关于参数在开头的定义这里不一一介绍 都比较好理解
resetParameters();
pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
}
void allocateMemory()
{ //根据params.yaml中给出的N_SCAN Horizon_SCAN参数值分配内存
laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
fullCloud.reset(new pcl::PointCloud<PointType>());
extractedCloud.reset(new pcl::PointCloud<PointType>());
fullCloud->points.resize(N_SCAN*Horizon_SCAN);
cloudInfo.startRingIndex.assign(N_SCAN, 0);
cloudInfo.endRingIndex.assign(N_SCAN, 0);
cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);
cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);
resetParameters();
}
void resetParameters()
{ //清零操作 比较简单易懂 都是程序刚开始的必备步骤
laserCloudIn->clear();
extractedCloud->clear();
// reset range matrix for range image projection
rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
imuPointerCur = 0;
firstPointFlag = true;
odomDeskewFlag = false;
for (int i = 0; i < queueLength; ++i)
{
imuTime[i] = 0;
imuRotX[i] = 0;
imuRotY[i] = 0;
imuRotZ[i] = 0;
}
}
2. 回调函数
2.1 imuHandler
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{ //IMU回调里没有执行太多复杂的操作
//首先imuConverter在utility.h中定义 就是将imu坐标系的数据转到雷达坐标系下
//上一篇文章中有针对每个人的数据的适配提示
sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
//锁存就是保证一次处理完
std::lock_guard<std::mutex> lock1(imuLock);
//放入队列中 这之后会有pop的操作 也均为基于imuQueue的
imuQueue.push_back(thisImu);
//下面的注释部分是调试打印数据 可以看看数据是否准确 尤其是欧拉角 原始的四元数不够直观
// debug IMU data
// cout << std::setprecision(6);
// cout << "IMU acc: " << endl;
// cout << "x: " << thisImu.linear_acceleration.x <<
// ", y: " << thisImu.linear_acceleration.y <<
// ", z: " << thisImu.linear_acceleration.z << endl;
// cout << "IMU gyro: " << endl;
// cout << "x: " << thisImu.angular_velocity.x <<
// ", y: " << thisImu.angular_velocity.y <<
// ", z: " << thisImu.angular_velocity.z << endl;
// double imuRoll, imuPitch, imuYaw;
// tf::Quaternion orientation;
// tf::quaternionMsgToTF(thisImu.orientation, orientation);
// tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
// cout << "IMU roll pitch yaw: " << endl;
// cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;
}
2.2 odometryHandler
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
{ //锁存后存入队列
//在前期版本中作者在地图优化程序中只发布了一个里程计消息 现在增加了增量式里程计话题 更方便处理
std::lock_guard<std::mutex> lock2(odoLock);
odomQueue.push_back(*odometryMsg);
}
2.3 cloudHandler
主要是进行了点云去畸变以及投影到距离图像等工作 如果自己的数据集跑算法没有反应建议调试下这里运行是否正常.
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{ //缓存点云信息
if (!cachePointCloud(laserCloudMsg))
return;
//计算去畸变所需的参数
if (!deskewInfo())
return;
//投影到距离图像中 （类似Lego-loam）
projectPointCloud();
//点云提取
cloudExtraction();
//发布点云
publishClouds();
//重置参数
resetParameters();
}
3. 点云缓存
bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
// cache point cloud
//点云信息存入队列
//关注下cloudQueue的类型 std::deque<sensor_msgs::PointCloud2>
cloudQueue.push_back(*laserCloudMsg);
//点云太少就退出 其实似乎等于2也能用了
//不过通常来说不会出现这样太少的情况所以不用纠结 因为用的时候还都是帧间的关系
if (cloudQueue.size() <= 2)
return false;
else
{
//点云队列先进先出 存到currentCloudMsg其类型是sensor_msgs::PointCloud2
currentCloudMsg = cloudQueue.front();
cloudQueue.pop_front(); //推出队列顶部数据 该数据上一行被存好了
//header赋值不用多说
cloudHeader = currentCloudMsg.header;
//时间需要关注 当前帧的时间以及下一帧时间
//由于刚刚的pop操作 因此cloudQueue队列顶的元素就是下一帧了已经
timeScanCur = cloudHeader.stamp.toSec();
timeScanNext = cloudQueue.front().header.stamp.toSec();
}
// convert cloud 这个必要的步骤不用多说了把
pcl::fromROSMsg(currentCloudMsg, *laserCloudIn);
// 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
//上一篇文章说过这一小块 检查ring这个field是否存在 veloodyne和ouster都有 不用再计算线束
// 如果报错需要注释掉该块
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();
}
}
//类似的 检查time这个field 用于去畸变时起作用
// check point time
if (deskewFlag == 0)
{
deskewFlag = -1;
for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
{
if (currentCloudMsg.fields[i].name == timeField)
{
deskewFlag = 1;
break;
}
}
if (deskewFlag == -1)
ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
}
return true;
}
4. 去畸变
用于去畸变的相关关键信息或变量值计算
4.1 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数据
//作者用的是500Hz的 100左右的也还可以效果
//imu队列顶部元素的时间需要小于当前激光扫描时间 imu队列底部时间又要大于下一帧扫描的时间戳
//其实就是保证imu有数据并且包含扫描帧间时间 用于后续操作
if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanNext)
{
ROS_DEBUG("Waiting for IMU data ...");
return false;
}
//imu去畸变参数计算 具体跳转至该程序
imuDeskewInfo();
//里程计去畸变参数计算 具体跳转至该程序 这块表达的可能有点问题 不够准确
odomDeskewInfo();
return true;
}
4.2 imuDeskewInfo
void imuDeskewInfo()
{
//这个参数在地图优化mapOptmization.cpp程序中用到 首先为false 完成相关操作后置true
cloudInfo.imuAvailable = false;
while (!imuQueue.empty())
{
//本来是满足imuQueue.front().header.stamp.toSec() < timeScanCur 的
//继续以0.01为阈值 舍弃较旧的imu数据
if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
imuQueue.pop_front();
else
break;
}
if (imuQueue.empty())
return;
imuPointerCur = 0;
for (int i = 0; i < (int)imuQueue.size(); ++i)
{ //取出imu队列中的数据
sensor_msgs::Imu thisImuMsg = imuQueue[i];
//时间戳
double currentImuTime = thisImuMsg.header.stamp.toSec();
//关键的一点 用imu的欧拉角做扫描的位姿估计 这里直接把值给了cloudInfo在地图优化程序中使用
// get roll, pitch, and yaw estimation for this scan
if (currentImuTime <= timeScanCur)
imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
//如果当前Imu时间比下一帧时间大于0.01 退出
if (currentImuTime > timeScanNext + 0.01)
break;
//因为第一次初始化时以下值都是0
if (imuPointerCur == 0){
imuRotX[0] = 0;
imuRotY[0] = 0;
imuRotZ[0] = 0;
imuTime[0] = currentImuTime;
++imuPointerCur;
continue;
}
// get angular velocity
//从imu信息中直接获得角速度 调用utility.h中函数
double angular_x, angular_y, angular_z;
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;
if (imuPointerCur <= 0)
return;
//这里将该变量置true 在地图优化程序中会用到
cloudInfo.imuAvailable = true;
}
4.3 odomDeskewInfo
void odomDeskewInfo()
{ //类似于IMU数据处理 先给该变量置fasle
cloudInfo.odomAvailable = false;
//以下同样与对IMU数据的操作类似
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() < timeScanNext)
return;
nav_msgs::Odometry endOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
endOdomMsg = odomQueue[i];
if (ROS_TIME(&endOdomMsg) < timeScanNext)
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;
}
5. 点云投影
上面一部分结束了用于去畸变的相关参数的获取，但是没有进行去畸变的工作，具体的工作在后面进行，下面是一个关键的函数用于将点云进行投射到range image.
往往运行launch文件后RVIZ中没有点云就是这里可能出错了 projectPointCloud(),将点云投影得到类似于一个二维距离图像的过程，其实LEGO-LOAM就是一样做的，作者沿用了之前的思路，但是可以看到这段程序里用了点云信息的ring这个field，如果是其他雷达可能没有这个，因此直接报错，所以如果雷达是其他品牌且没有ring或者类似于直接给出线束的field，建议按线束的角度直接算，可参考lego-loam中原始代码
void projectPointCloud()
{ //点云数
int cloudSize = laserCloudIn->points.size();
// range image projection 距离图像 这里比较重要但是原理很简单
//就是把点云数据按线束 按行列保存
//上一篇文章中已经提出了在配置上的建议 这里不再叙述 如果运行算法不报错 但是RVIZ中没有任何显示 重点排查该函数
for (int i = 0; i < cloudSize; ++i)
{
//将laserCloudIn数据存到thisPoint中 laserCloudIn在本程序文件中生成的 往前翻可以看到
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;
//从点云数据中获得线束信息 作为行 最终距离图像就是16*1800
int rowIdn = laserCloudIn->points[i].ring;
if (rowIdn < 0 || rowIdn >= N_SCAN)
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;
//调用utility.h里的函数计算点到雷达的距离
float range = pointDistance(thisPoint);
//点的过滤 太近太远都不要
if (range < 1.0)
continue;
if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
continue;
//去畸变 运动补偿 这里需要用到雷达信息中的time 这个field
thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time); // Velodyne
// thisPoint = deskewPoint(&thisPoint, (float)laserCloudIn->points[i].t / 1000000000.0); // Ouster
// cv::Mat类型的rangMat生成 距离图像 16*1800
rangeMat.at<float>(rowIdn, columnIdn) = pointDistance(thisPoint);
//索引值 类似于图像中像素索引的概念应该比较好理解
int index = columnIdn + rowIdn * Horizon_SCAN;
//fullCloud填充内容 这里是运动补偿去畸变后的点云
fullCloud->points[index] = thisPoint;
}
}
上述代码块中调用了去畸变的程序deskewPoint()
PointType deskewPoint(PointType *point, double relTime)
{
//运动补偿函数 值得一看 可当作一种工具使用
//这是调用该函数的语句 thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time); // Velodyne
//可以关注下第二个参数 后续有用
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;
}
6. 点云提取及发布
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()
{ //发布点云 在地图优化程序中被订阅
cloudInfo.header = cloudHeader;
cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
pubLaserCloudInfo.publish(cloudInfo);
}
643
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
