loam_continuous源码分析解读

loam_continuous源码逐行分析——scanRegistration

写在前面
scanRegistration
- imuHandler
- laserCloudHandler
- 总结

写在前面

最近在阅读loam_continuous的源代码，发现相关的解读文章不是很多，自己看的过程中也比较费劲。经过一段时间的摸索，把自己的所得分享出来，希望能帮助后面需要的人！(仅供参考，可能有理解不对的地方)。
另外，为方便阅读，先做一些解释说明：
1.一次激光帧数据表示为一个scan,多个scan累积后为一个Sweep。

scanRegistration

scanRegistration是提取特征点的节点。
节点订阅了两个数据：

IMU，数据格式 sensor_msgs::Imu
点云，数据格式 sensor_msgs::PointCloud2

ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/sync_scan_cloud_filtered", 2, laserCloudHandler);
ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu>("/microstrain/imu", 5, imuHandler);

节点发布了两个话题：
1.在当前Sweep的过程中，提取到的两类特征点（最大曲率边缘点，最小曲率平面点）
2.在上一个Sweep的过程中，累计提取出的四类特征点（最大曲率边缘点，较大曲率边缘点，最小曲率平面点，较小曲率平面点）

ros::Publisher pubLaserCloudExtreCur = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_extre_cur", 2);
ros::Publisher pubLaserCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_last", 2);

imuHandler

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation); // 获取的是IMU的姿态  tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
int imuPointerBack = imuPointerLast;
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack]; // 前后两帧IMU数据的时间差
if (timeDiff < 0.1) // 0.1s内的iMU数据有效
{
//imuAccuRoll += timeDiff * imuIn->angular_velocity.x;
//imuAccuPitch += timeDiff * imuIn->angular_velocity.y;
imuAccuYaw += timeDiff * imuIn->angular_velocity.z;
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = -pitch;
//imuYaw[imuPointerLast] = -yaw;
//imuRoll[imuPointerLast] = imuAccuRoll;
//imuPitch[imuPointerLast] = -imuAccuPitch;
imuYaw[imuPointerLast] = -imuAccuYaw; //yaw会漂移，所以不信任yaw。更加信任角速度对时间的累加。
//imuAccX[imuPointerLast] = -imuIn->linear_acceleration.y;
//imuAccY[imuPointerLast] = -imuIn->linear_acceleration.z - 9.81;
//imuAccZ[imuPointerLast] = imuIn->linear_acceleration.x;
AccumulateIMUShift();
}

使用到IMU的数据有，orientation,angular_velocity.z。
这里加速度被注释掉了，但是后面有用到的，感觉这里会有点问题。
通过IMU的orientation获取翻滚角和俯仰角，偏航角通过角速度累积积分得到。可能是考虑到IMU的yaw会漂移的缘故，所以不信任。
获取了IMU的姿态后，通过积分估计IMU的位置。

void AccumulateIMUShift()
{
float roll = imuRoll[imuPointerLast];
float pitch = imuPitch[imuPointerLast];
float yaw = imuYaw[imuPointerLast];
// ACC其实没有维护。
float accX = imuAccX[imuPointerLast];
float accY = imuAccY[imuPointerLast];
float accZ = imuAccZ[imuPointerLast];
//**************************************************************
// IMU坐标系转换到世界坐标系，世界坐标系为Z轴向前，X轴向左，Y轴向上
// （这个地方之前没有转换到zhangji的坐标系!!!）
// 绕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;
//**************************************************************
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
if (timeDiff < 0.1)
{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;
// 如果是匀速运动，这里推导出来的速度为0，
imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;    imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;    imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
}
}

先讨论一下roll,yaw,pitch的定义和方向
机体坐标系与地面坐标系的关系是三个Euler角，反应了飞机相对地面的姿态。地面坐标系为什么X轴向前，Z轴垂直向下，y轴向右满足右手螺旋坐标系。

上图中绿色的表示地面坐标系，红色的表示机体坐标系.
俯仰角θ（pitch）：机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。
偏航角ψ（yaw）：机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。
滚转角Φ（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。

再讨论一下三维坐标系下绕轴旋转的方向

规定在右手坐标系中，物体旋转的正方向是右手螺旋方向，即从该轴正半轴向原点看是逆时针方向。

该函数先根据IMU的姿态将角速度转换到世界坐标系下，得到车体在世界坐标系下三轴的加速度，然后积分出车体世界坐标系下的坐标和角速度，并用一个50大小的列表维护。

从这里可以看到，代码利用IMU数据得到了车体在世界坐标系下的实时坐标和速度。

laserCloudHandler

前面是对激光的一些转换和过滤等，筛掉了距离小于0.5米的点。然后给每个点额外添加了一个时间的属性。

laserPointIn.h = timeLasted; //从初始化到当前帧的时间

这个时间表示从初始化到当前激光帧的时间差，在当前激光帧中每个点的这个时间戳都是一样的，这样可以判断哪些点属于同一个scan，哪些点属于不同scan。(因为zhangji使用的是单线激光，在使用多线激光的时候，这里会出现问题)。

接下来判断当前激光所在的Sweep是否结束

pcl::PointXYZ laserPointFirst = laserCloudIn->points[0];
pcl::PointXYZ laserPointLast = laserCloudIn->points[cloudInSize - 1];
float rangeFirst = sqrt(laserPointFirst.x * laserPointFirst.x + laserPointFirst.y * laserPointFirst.y + laserPointFirst.z * laserPointFirst.z);
laserPointFirst.x /= rangeFirst;
laserPointFirst.y /= rangeFirst;
laserPointFirst.z /= rangeFirst;
float rangeLast = sqrt(laserPointLast.x * laserPointLast.x + laserPointLast.y * laserPointLast.y + laserPointLast.z * laserPointLast.z);
laserPointLast.x /= rangeLast;
laserPointLast.y /= rangeLast;
laserPointLast.z /= rangeLast;
// 激光帧第一个点和最后一个点之间的角度[-pi,pi]   // atan2(double x,double y)
float laserAngle = atan2(laserPointLast.x - laserPointFirst.x, laserPointLast.y - laserPointFirst.y);
bool newSweep = false;
// 这一帧激光的花费的时间大于0.7秒，论文中雷达竖直旋转从-90 ～ 90度需要花费1秒。
// 激光已经积累了0.7s，并且角度相反的时候
if (laserAngle * laserRotDir < 0 && timeLasted - timeStart > 0.7)
{
laserRotDir *= -1;
newSweep = true;
}

论文中使用的雷达是2D的单线雷达，视角为180°。
两个条件判断当前Sweep是否结束：
1.当前scan的第一个点和最后一个点之间的角度来判断，当使用单线的连续旋转激光时，雷达每旋转180度，laserAngle的正负号都会被改变一次。每转过180度为一个Sweep，激光扫描完一个球面。
2.该Sweep的持续时间要大于0.7秒，根据论文里180度/s的转速，转180度需要1秒钟.

如果当前激光帧是一个新的Sweep的开始，则打包好这个Sweep的相关姿态位置信息，打包好提取好的特征点信息，重置新的Sweep的参考位姿。

pcl::PointCloud<pcl::PointXYZHSV>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZHSV>(4, 1));
//*****该Sweep第一帧时候的姿态********
imuTrans->points[0].x = imuPitchStart;
imuTrans->points[0].y = imuYawStart;
imuTrans->points[0].z = imuRollStart;
imuTrans->points[0].v = 10;
//*****该Sweep最后一帧的姿态********
imuTrans->points[1].x = imuPitchCur;
imuTrans->points[1].y = imuYawCur;
imuTrans->points[1].z = imuRollCur;
imuTrans->points[1].v = 11;
// ******该Sweep最后一帧相对第一帧的平移*********
imuTrans->points[2].x = imuShiftFromStartXCur;
imuTrans->points[2].y = imuShiftFromStartYCur;
imuTrans->points[2].z = imuShiftFromStartZCur;
imuTrans->points[2].v = 12;
//******该Sweep最后一帧相对第一帧的速度差*******
imuTrans->points[3].x = imuVeloFromStartXCur;
imuTrans->points[3].y = imuVeloFromStartYCur;
imuTrans->points[3].z = imuVeloFromStartZCur;
imuTrans->points[3].v = 13;

如果有IMU数据，会插值更新当前的位姿信息。如果没有IMU数据，位姿信息都会是0.

// imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)
// 当前激光帧相对于第一个激光帧的距离 = 当前帧相对世界坐标 - 当前Sweep第一个激光帧时的坐标 - 当前Sweep第一个激光帧在当前帧时间内移动的坐标
// 第一个激光帧的运动距离= 速度*时间。匀速运动假设？
imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * (timeLasted - timeStart);
imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * (timeLasted - timeStart);
imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * (timeLasted - timeStart);
ShiftToStartIMU(); //  IMU相对于当前sweep第一个激光帧的坐
imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart; // 加减速运动，相对当前SWeep第一个激光帧的速度增量
imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;  imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
VeloToStartIMU(); //  IMU相对于当前Sweep第一个激光帧的速度
for (int i = 0; i < cloudSize; i++)
{
//当前激光帧转换到当前Sweep的第一帧的坐标系，如果没有IMU，多帧激光应该会堆叠在一起，
//这么来看，在loam_continous里，IMU不可以少。
TransformToStartIMU(&laserCloud->points[i]);
}

上述操作把激光帧转换到了当前Sweep第一个激光帧时候的坐标，畸变矫正了单个激光帧的位置，但并没有矫正激光帧内单个点的畸变。单个激光点的畸变其实没有做。

激光矫正好之后
首先计算了曲率，然后筛除可信度较低了点
筛除了跟激光方向角度比较小的点

// //按照两点的深度的比例，将深度较大的点拉回后计算距离
// 相当于让两个点在一个圆周上。depth2就是半径，
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;
//边长比也即是弧度值，若小于0.1，说明夹角比较小，斜面比较陡峭,
// 点深度变化比较剧烈,点处在近似与激光束平行的斜面上
// sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ)为点1到点2的弧长
// 弧长/半径为夹角
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1)
{         // 夹角太小，与激光方向接近，Picked置为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       {        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;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;        }

然后筛除了离群点

if (diff > (0.25 * 0.25) / (20 * 20) * dis && diff2 > (0.25 * 0.25) / (20 * 20) * dis)
{
cloudNeighborPicked[i] = 1;
}

然后是挑选特征点的过程
点云分成4组，每组选择2个最大曲率点，18个较大曲率点，4个曲率最小的点，剩下较小的点。
一共是有8个最大曲率点，72个较大曲率点，16个最小曲率点，剩下都是较小曲率点。这部分讲解的博客比较多，这里就不细说了。
特征点提取之后累加起来。
每隔2帧发布一次当前Sweep的最大最小特征点。
每隔2帧发布一次上次Sweep的所有特征点。

总结

scanRegistration跟IMU的融合
通过对IMU的积分估计点云的位置和姿态，实现Sweep内部各帧激光统一参考系。