LOAM源码解析1一scanRegistration
鉴于工作和学习需要，学习了激光
salm算法loam，并阅读了作者的原版论文，现将学习过程中的理解与一些源码剖析记录整理下来，也是对于学习slam的阶段性总结!!!
一、综述
LOAM这篇论文是发表于2014年RSS的文章，全称为：LOAM: Lidar Odometry and Mapping in Real-time . LOAM是基于激光雷达而搭建的在ROS平台下的SLAM系统，一共分为scanRegistration提取特征点、laserOdometry 10HZ估计位姿、laserMapping 1HZ构建三维地图transforMaintenance 位姿优化等四个模块。
本文对今天对第一部分Point Clond Registration模板，scanRegistration.cpp进行分析，主要完成的工作有：对点云和IMU数据预处理、对接收的点云数据划分到不同线中存储、特征点（边缘点+平面点）提取等。
二、scanRegistration.cpp
从main函数开始 在ros中订阅了2个话题，发布了6个话题。两个主要的回调函数：laserCloudHandler、imuHandler分别处理点云和IMU数据。其中，最主要的是laserCloudHandler回调函数。
2.1 main函数
int main(int argc, char** argv)
{
//ros::init(argc, argv, "scanRegistration");
ros::init(argc, argv, "scanRegistration");
ros::NodeHandle nh;
//2个订阅者，6个发布者
/*
* 参数1：话题名称
* 参数2：信息队列长度
* 参数3：回调函数，每当一个信息到来的时候，这个函数会被调用
* 返回一个ros::Subscriber类的对象，当此对象的所有引用都被销毁是，本节点将不再是该话题的订阅者
*/
ros::Subscriber subLaserCloud = nh.subscribe
("/velodyne_points", 2, laserCloudHandler);//订阅雷达数据
ros::Subscriber subImu = nh.subscribe ("/imu/data", 50, imuHandler);订阅imu数据
/*
* 我们通过advertise() 函数指定我们如何在给定的topic上发布信息
* 它会触发对ROS master的调用，master会记录话题发布者和订阅者
* 在advertise()函数执行之后，master会通知每一个订阅此话题的节点
* 两节点间由此可以建立直接的联系
* advertise()会返回一个Publisher对象，使用这个对象的publish方法我们就可以在此话题上发布信息
* 当返回的Publisher对象的所有引用都被销毁的时候，本节点将不再是该话题的发布者
* 此函数是一个带模板的函数，需要传入具体的类型进行实例化
* 传入的类型就是要发布的信息的类型，在这里是String
* 第一个参数是话题名称
* 第二个参数是信息队列的长度，相当于信息的一个缓冲区
* 在我们发布信息的速度大于处理信息的速度时
* 信息会被缓存在先进先出的信息队列里
*/
//指定发布者话题的对象
pubLaserCloud = nh.advertise
("/velodyne_cloud_2", 2);//发布按线分类后的点云
pubCornerPointsSharp = nh.advertise
("/laser_cloud_sharp", 2);//边界线上特殊点云
pubCornerPointsLessSharp = nh.advertise
("/laser_cloud_less_sharp", 2);
pubSurfPointsFlat = nh.advertise
("/laser_cloud_flat", 2);//面上特征点云
pubSurfPointsLessFlat = nh.advertise
("/laser_cloud_less_flat", 2);
pubImuTrans = nh.advertise ("/imu_trans", 5);//发布imu处理数据
ros::spin();//每执行一次一直等待处理回调函数
return 0;
}
2.2 回调函数 laserCloudHandler
主要包括以下内容：
1、点云预处理：过滤无效点、计算起始和终止点的方位角 2、根据每个点的仰角将点划入不同线中，共16线，记录线束号和获取的相对时间（也就是代码中的intensity，这个不是强度） 3、使用IMU数据插值计算点云中点的位置，消除由于非匀速运动造成的运动畸变 4、计算所有点的曲率，剔除两类点 5、提取边缘点和平面点 6、ROS发布消息
/*
对接受到的点云进行预处理，完成分类
将点云划入不同的线性存储；对其进行特征分类
*/
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
//-------------------1、点云预处理-----------------------//
if (!systemInited) {
systemInitCount++;
if (systemInitCount >= systemDelay) {
//延时作用，保证imu有数据后再调用该回调函数
systemInited = true;
}
return;//不满足条件到此处就不往下执行了，最下面的也不会执行了，直接跳出回调函数
}
//N_SCANS（16）保存每一点的起始与终止索引
std::vector scanStartInd(N_SCANS, 0);//定义了16个整形变量，每个变量的初值为1
std::vector scanEndInd(N_SCANS, 0);
//雷达的时间戳
double timeScanCur = laserCloudMsg->header.stamp.toSec();
pcl::PointCloud laserCloudIn;
//转化为模板点云
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector indices;
//去除无效点
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
int cloudSize = laserCloudIn.points.size();
//计算点云的起始角度与终止角度
//atan2的范围是[-180,180]，atan的范围是[-90,90]
//扫描开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
/*将起止角度差值约束到pi-3pi之间*/
if (endOri - startOri > 3 * M_PI) {
endOri -= 2 * M_PI;
} else if (endOri - startOri < M_PI) {
endOri += 2 * M_PI;
}
//-----------------2、根据角度将点划入不同的线中------------------//
/*
遍历所有点，计算点的仰角，根据仰角过滤16线以外的点，并为每个点分配激光线号scanID
根据点云中点旋转角度和整个周期旋转角度的比率获得相对时间relTime，并获得点强度（线号+点相对时间）
根据扫描线是否过半，选择与起始位置还是终止位置进行差值计算，从而进行补偿
*/
bool halfPassed = false;//为了判断雷达扫描的点是否过半
int count = cloudSize;
PointType point;//承载雷达数据点的数据类型
std::vector > laserCloudScans(N_SCANS);//存储16线点云的vector，根据仰角排列激光线号，velodyne每两个scan之间间隔2度
//遍历所有点，计算每个点的角度
for (int i = 0; i < cloudSize; i++) {
// 雷达坐标系是x向前，y向左，z向上
// 机器坐标系为z向前，x向左，y向上
point.x = laserCloudIn.points[i].y;
point.y = laserCloudIn.points[i].z;
point.z = laserCloudIn.points[i].x;
//计算点的仰角，过滤16线以外的点
float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
int scanID;//每个id里面都有不止一个角
int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); //针对仰角进行四舍五入
if (roundedAngle > 0){
scanID = roundedAngle;
}
else {
scanID = roundedAngle + (N_SCANS - 1);
}
//过滤点，只挑选[-15度，+15度]范围内的点,scanID属于[0,15]，剔除16线以外的杂点。
if (scanID > (N_SCANS - 1) || scanID < 0 ){
count--;
continue;
}
// 3.根据扫描线是否旋转过半，选择与起始位置还是终止位置进行差值计算，从而进行补偿
float ori = -atan2(point.x, point.z);//（0--2pi）
//如果没有旋转过半
if (!halfPassed) {
//确保-pi/2 < ori - startOri < 3*pi/2
if (ori < startOri - M_PI / 2) {
ori += 2 * M_PI;
} else if (ori > startOri + M_PI * 3 / 2) {
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI) {
halfPassed = true;
}
}
//如果旋转过半
else {
ori += 2 * M_PI;//先加2*pi
//确保-3*pi/2 < ori - endOri < pi/2
if (ori < endOri - M_PI * 3 / 2) {
ori += 2 * M_PI;
} else if (ori > endOri + M_PI / 2) {
ori -= 2 * M_PI;
}
}
//点云中点的相对时间:点旋转的角度与整个周期旋转角度的比率, -0.5 < relTime < 1.5
float relTime = (ori - startOri) / (endOri - startOri);
//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号scanID，小数部分是该点的相对时间）
//匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
point.intensity = scanID + scanPeriod * relTime;// scanPeriod 扫描一圈0.1s
// -----------------3、使用IMU数据进行插值计算点云的中点的位置，消除由于非匀速运动造成的运动畸变----------------//
if (imuPointerLast >= 0) {//当imu有数据时
float pointTime = relTime * scanPeriod;//当前激光点的偏移时间=激光点相对时间所占比例*周期
while (imuPointerFront != imuPointerLast) {
//点时间=当前点云时间戳+当前激光点的偏移时间
//寻找是否有点云时间 imuTime[imuPointerFront]) {
imuRollCur = imuRoll[imuPointerFront];
imuPitchCur = imuPitch[imuPointerFront];
imuYawCur = imuYaw[imuPointerFront];
imuVeloXCur = imuVeloX[imuPointerFront];
imuVeloYCur = imuVeloY[imuPointerFront];
imuVeloZCur = imuVeloZ[imuPointerFront];
imuShiftXCur = imuShiftX[imuPointerFront];
imuShiftYCur = imuShiftY[imuPointerFront];
imuShiftZCur = imuShiftZ[imuPointerFront];
}
//找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间
//据此线性插值（根据距离分配比重），计算点云点的速度，位移和欧拉角
else {
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
//按时间距离计算权重分配比率
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
} else {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}
//更新速度和偏移量
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}
//如果是第一个点,记住点云起始位置的速度，位移，欧拉角
if (i == 0) {
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
}
//计算之后每个点相对于第一个点的由于加减速非匀速运动产生的位移、速度畸变，并对点云中的每个点位置信息重新补偿矫正
// Lidar位移、速度转移到IMU起始坐标系下
else {
//当不为初始值时，去除imu加速度造成运动畸变的影响
ShiftToStartIMU(pointTime);//相对于原始点的位移畸变
VeloToStartIMU();//相对于原始点的位移畸变
TransformToStartIMU(&point);//减去非匀速位移畸变的值
}
}
//接着将激光点存入laserCloudScans对应的scanID中，从而完成按线分类工作。按照线号从小到大
//通过laserCloud指针将N_SCANS线的激光数据组成pcl点云
laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器中
}
cloudSize = count;
//------------------------4、计算所有点的曲率、剔除两类点-------------------------//
//1、被斜面挡住的点 2、入射角平面与激光平行的点
//特征点的提取，通过遍历每个点与相邻10个点坐标（除了前五个和后五个）差值的平方来代表曲率，计算各点曲率并找到所有线的起点终点位置
pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud());
for (int i = 0; i < N_SCANS; i++) {
*laserCloud += laserCloudScans[i];//将所有的点按照线号从小到大放入一个容器
}
int scanCount = -1;
//1针对按线分类后的点云，通过激光点左右各5个点进行曲率计算：
//该点与周围10个点的偏差
for (int i = 5; i < cloudSize - 5; i++) {
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x
+ laserCloud->points[i - 3].x + laserCloud->points[i - 2].x
+ laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x
+ laserCloud->points[i + 1].x + laserCloud->points[i + 2].x
+ laserCloud->points[i + 3].x + laserCloud->points[i + 4].x
+ laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y
+ laserCloud->points[i - 3].y + laserCloud->points[i - 2].y
+ laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y
+ laserCloud->points[i + 1].y + laserCloud->points[i + 2].y
+ laserCloud->points[i + 3].y + laserCloud->points[i + 4].y
+ laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z
+ laserCloud->points[i - 3].z + laserCloud->points[i - 2].z
+ laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z
+ laserCloud->points[i + 1].z + laserCloud->points[i + 2].z
+ laserCloud->points[i + 3].z + laserCloud->points[i + 4].z
+ laserCloud->points[i + 5].z;
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;//记录每个点的曲率
cloudSortInd[i] = i;//表示当前点在点云中索引
cloudNeighborPicked[i] = 0;//用于标记是否为筛选过的点
cloudLabel[i] = 0;//用于标记特征点为边界线还是平面特征点
//每个scan，只有第一个符合的点会进来，因为每个scan的点都在一起存放
if (int(laserCloud->points[i].intensity) != scanCount) {
scanCount = int(laserCloud->points[i].intensity);//控制每个scan只进入第一个点
if (scanCount > 0 && scanCount < N_SCANS) {
scanStartInd[scanCount] = i + 5;// scan起始位置的索引（去除前5个点）
scanEndInd[scanCount - 1] = i - 5;// scan终止位置的索引（去除后5个点）
}
}
}
//第一个scan曲率点有效点序从第5个开始，最后一个激光线结束点序size-5，直接赋值即可
scanStartInd[0] = 5;//第一条线的起始位置
scanEndInd.back() = cloudSize - 5; //最后一条线的终止位置
// 2.挑选点，排除容易被斜面挡住的点以及离群点，有些点容易被斜面挡住，
// 而离群点可能出现带有偶然性，这些情况都可能导致前后两次扫描不能被同时看到
for (int i = 5; i < cloudSize - 6; i++) {//与后一个点的差值，所以减6
float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
//计算有效曲率点与后一个点之间的距离平方和
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
if (diff > 0.1) {//若两个点之间距离的平方大于0.1，说明这两点存在一定的距离，
//计算点的深度信息
float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
laserCloud->points[i].y * laserCloud->points[i].y +
laserCloud->points[i].z * laserCloud->points[i].z);
float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x +
laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
if (depth1 > depth2) {
//将较长边（i）按比例收缩到与短边i+1平齐，构成以i,i+1的长度为腰的等腰三角形
diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;
//根据 x=sinx（近似）=底/边 ,求得当前激光点与后一点的夹角
//实际表示i与i+1夹角小于5.732度 sin(5.732) ~= 0.1 认为被遮挡
// 边长比也即是弧度值，若小于0.1，说明夹角比较小，斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上
//3、排除容易被斜面挡住的点
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {
//深度大的点以及前面5个点，全部标记为不是特征点，因为不稳定
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}
} else {//将较长边（i+1）按比例收缩到与短边i平齐，构成以i,i+1的长度为腰的等腰三角形
diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;
//深度大的点以及后面5个点，全部标记为不是特征点，因为不稳定
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
}
//4、排除与激光平行的点
//计算当前点与前一点的距离的平方
float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;
//计算当前点的深度（x2+y2+z2）
float dis = laserCloud->points[i].x * laserCloud->points[i].x
+ laserCloud->points[i].y * laserCloud->points[i].y
+ laserCloud->points[i].z * laserCloud->points[i].z;
//如果当前点与前后点距离的平方都>0.0002倍当前点的深度，则当前点入射角与物理表面平行，当前点不可作为特征点
if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
cloudNeighborPicked[i] = 1;
}
}
//--------------------------5、根据曲率排序、提取边缘点、平面点，进行特征分类----------------------------//
pcl::PointCloud cornerPointsSharp;//存储每段中曲率较大的两个点
pcl::PointCloud cornerPointsLessSharp;//存储边缘点
pcl::PointCloud surfPointsFlat;//用于存储曲率最小的几个点
pcl::PointCloud surfPointsLessFlat;//存储平面点
//首先对于每一层激光点(总16层)，将每层区域分成6份，起始位置为sp，终止位置为ep，然后对曲率从小到大进行排序
for (int i = 0; i < N_SCANS; i++) {
pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);
//每个线分6段，确保周围都有点被选作特征点，sp/ep分别为各段起始和终止位置
for (int j = 0; j < 6; j++) {
//六等份起点：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6
int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6;
//六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6
int ep = (scanStartInd[i] * (5 - j) + scanEndInd[i] * (j + 1)) / 6 - 1;
//每一段，曲率按照升序排放
for (int k = sp + 1; k <= ep; k++) {
for (int l = k; l >= sp + 1; l--) {
if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {
int temp = cloudSortInd[l - 1];
cloudSortInd[l - 1] = cloudSortInd[l];
cloudSortInd[l] = temp;
}
}
}
//筛选边缘点：每段曲率很大或则比较大的点
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
int ind = cloudSortInd[k];//设定为曲率最大的点序
//如果该点序的点没有被标记为不可取特征点且曲率大于阈值
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1) {
largestPickedNum++;
//挑选曲率最大的前2个点放入sharp点集合
if (largestPickedNum <= 2) {
cloudLabel[ind] = 2;//2代表曲率很大
cornerPointsSharp.push_back(laserCloud->points[ind]);//保存sharp点云中
cornerPointsLessSharp.push_back(laserCloud->points[ind]);//同时保存在lessSharp点云中
}//挑选曲率再打的前20个点保存在lessSharp点云中（包括前两个标记为2的点）
else if (largestPickedNum <= 20) {
cloudLabel[ind] = 1;//后18个lesssharp特征线点标记为1
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
} else {
break;
}
cloudNeighborPicked[ind] = 1;//标记为不可用点
//将曲率比较大的点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀
//对ind点周围的点是否能作为特征点进行判断 除非距离大于阈值，否则前后各五个点不能作为特征点
for (int l = 1; l <= 5; l++) {
float diffX = laserCloud->points[ind + l].x
- laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y
- laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z
- laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[ind + l] = 1;//被标记为不可用点
}
for (int l = -1; l >= -5; l--) {
float diffX = laserCloud->points[ind + l].x
- laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y
- laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z
- laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
//平面点：挑选每个分段的曲率很小比较小的点
//从每一段曲率最小(前端)开始查找 用于确定平面点
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
int ind = cloudSortInd[k];
//如果没有被标记为不可用特征点，且曲率小于0.1
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1) {
cloudLabel[ind] = -1;//-1代表曲率很小的点
surfPointsFlat.push_back(laserCloud->points[ind]);//用于后面补偿所用
smallestPickedNum++;
if (smallestPickedNum >= 4) {只选最小的四个，剩下的Label==0,就都是曲率比较小的
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {//同样防止特征点聚集，判断周围点是否是不可标记点
float diffX = laserCloud->points[ind + l].x
- laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y
- laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z
- laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
float diffX = laserCloud->points[ind + l].x
- laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y
- laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z
- laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
//除了被标记过的点(本身不能作为特征点以及被标记为边界线的特征点)，其它点都作为lessFlatPoints存储到surfPointsLessFlatScan中
for (int k = sp; k <= ep; k++) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
//由于less flat点最多，对每个分段less flat的点进行体素栅格滤波
pcl::PointCloud surfPointsLessFlatScanDS;
pcl::VoxelGrid downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
//less flat点汇总
surfPointsLessFlat += surfPointsLessFlatScanDS;//存储平面点
}
//------------------------6、将不同的特征点打包成消息发送出去-----------------------------//
//publich消除非匀速运动畸变后的所有的点
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera";
pubLaserCloud.publish(laserCloudOutMsg);
//publich消除非匀速运动畸变后的边缘点
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
//publich消除非匀速运动畸变后的平面点
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera";
pubSurfPointsFlat.publish(surfPointsFlat2);
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
//publich IMU消息,由于循环到了最后，因此是Cur都是代表最后一个点，即最后一个点的欧拉角，畸变位移及一个点云周期增加的速度
pcl::PointCloud imuTrans(4, 1);
//起始点欧拉角
imuTrans.points[0].x = imuPitchStart;
imuTrans.points[0].y = imuYawStart;
imuTrans.points[0].z = imuRollStart;
//最后一个点的欧拉角
imuTrans.points[1].x = imuPitchCur;
imuTrans.points[1].y = imuYawCur;
imuTrans.points[1].z = imuRollCur;
//最后一个点相对于第一个点的畸变位移和速度
imuTrans.points[2].x = imuShiftFromStartXCur;
imuTrans.points[2].y = imuShiftFromStartYCur;
imuTrans.points[2].z = imuShiftFromStartZCur;
imuTrans.points[3].x = imuVeloFromStartXCur;
imuTrans.points[3].y = imuVeloFromStartYCur;
imuTrans.points[3].z = imuVeloFromStartZCur;
sensor_msgs::PointCloud2 imuTransMsg;
pcl::toROSMsg(imuTrans, imuTransMsg);
imuTransMsg.header.stamp = laserCloudMsg->header.stamp;
imuTransMsg.header.frame_id = "/camera";
pubImuTrans.publish(imuTransMsg);
}
2.3 回调函数 imuHandler()
减去重力对imu的影响，求出xyz轴的加速度实际值，将加速度转换到世界坐标系
//IMU回调函数首先读取方向角四元素到orientation，然后转换成欧拉角roll/pitch/yaw
//减去重力对imu的影响，求出xyz轴的加速度实际值，将加速度转换到世界坐标系
//接收imu消息，imu坐标系为x轴向前，y轴向右，z轴向上的右手坐标系
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
//统一到z轴向前,x轴向左的右手坐标系
//交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll).
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
//imuPointerLast作为imu各个参数数组的索引，循环累加，
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;
AccumulateIMUShift();
}
2.4 AccumulateIMUShift()
进行航距推算，假定为匀速运动推算出当前时刻的位置、速度
//积分速度与位移
void AccumulateIMUShift()
{
float roll = imuRoll[imuPointerLast];
float pitch = imuPitch[imuPointerLast];
float yaw = imuYaw[imuPointerLast];
float accX = imuAccX[imuPointerLast];
float accY = imuAccY[imuPointerLast];
float accZ = imuAccZ[imuPointerLast];
// 绕RPY旋转转换得到世界坐标系下的加速度值
//绕z轴旋转(roll)
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;
//绕x轴旋转(pitch)
float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
//绕y轴旋转(yaw)
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;
accZ = -sin(yaw) * x2 + cos(yaw) * z2;
//上一个imu点
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
//上一个点到当前点所经历的时间，即计算imu测量周期
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
//求每个imu时间点的位移Shift与速度Velo,两点之间视为匀加速直线运动
//要求imu的频率至少比lidar高，这样的imu信息才使用，后面校正也才有意义
if (timeDiff < scanPeriod) {//（隐含从静止开始运动）
imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff
+ accX * timeDiff * timeDiff / 2;
imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff
+ accY * timeDiff * timeDiff / 2;
imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff
+ accZ * timeDiff * timeDiff / 2;
imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
}
}