loam_continuous源码分析解读

loam_continuous源码逐行分析——scanRegistration

写在前面

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

scanRegistration

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

  1. IMU,数据格式 sensor_msgs::Imu
  2. 点云,数据格式 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内部各帧激光统一参考系。

  • 0
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 4
    评论
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值