loam_continuous源码逐行分析——scanRegistration
写在前面
最近在阅读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内部各帧激光统一参考系。