有没有做语文题的网站,学电商出来一般干什么工作,新手怎么做电商,网站开发语言是什么系列文章目录
【3D激光SLAM】LOAM源代码解析–scanRegistration.cpp 【3D激光SLAM】LOAM源代码解析–laserOdometry.cpp 【3D激光SLAM】LOAM源代码解析–laserMapping.cpp 【3D激光SLAM】LOAM源代码解析–transformMaintenance.cpp 写在前面
本系列文章将对LOAM源代码进行讲解…系列文章目录
·【3D激光SLAM】LOAM源代码解析–scanRegistration.cpp ·【3D激光SLAM】LOAM源代码解析–laserOdometry.cpp ·【3D激光SLAM】LOAM源代码解析–laserMapping.cpp ·【3D激光SLAM】LOAM源代码解析–transformMaintenance.cpp 写在前面
本系列文章将对LOAM源代码进行讲解在讲解过程中涉及到论文中提到的部分会结合论文以及我自己的理解进行解读尤其是对于其中坐标变换的部分将会进行详细的讲解。
本来是懒得写的一个是怕自己以后忘了另外是我在学习过程中其实没有感觉哪一个博主能讲解的通篇都能让我很明白特别是坐标变换部分的代码所以想着自己学完之后按照自己的理解也写一个LOAM解读希望能对后续学习LOAM的同学们有所帮助。
之后也打算录一个LOAM讲解的视频大家可以关注一下。 文章目录 系列文章目录写在前面整体框架一、main函数二、接收IMU的消息-imuHandler()三、积分IMU的速度与位移-AccumulateIMUShift()四、接收点云数据-laserCloudHandler()4.1 预处理与线性插值4.2 计算曲率4.3 剔除不好的点4.4 特征提取4.5 发布话题 总结 整体框架
LOAM多牛逼就不用多说了直接开始
先贴一下我详细注释的LOAM代码在这个版本的代码上加入了我自己的理解。
我觉得最重要也是最恶心的一部分是其中的坐标变换在代码里面真的看着头大所以先明确一下坐标系(都是右手坐标系)
IMU(IMU坐标系imu)x轴向前y轴向左z轴向上LIDAR(激光雷达坐标系l)x轴向前y轴向左z轴向上CAMERA(相机坐标系也可以理解为里程计坐标系c)z轴向前x轴向左y轴向上WORLD(世界坐标系w也叫全局坐标系与里程计第一帧init重合)z轴向前x轴向左y轴向上MAP(地图坐标系map一定程度上可以理解为里程计第一帧init)z轴向前x轴向左y轴向上
坐标变换约定 为了清晰变换矩阵的形式与《SLAM十四讲中一样》即 R A _ B R_{A\_B} RA_B表示B坐标系相对于A坐标系的变换B中一个向量通过 R A _ B R_{A\_B} RA_B可以变换到A中的向量。
首先对照ros的节点图和论文中提到的算法框架来看一下 可以看到节点图和论文中的框架是一一对应的这几个模块的功能如下
scanRegistration对原始点云进行预处理计算曲率提取特征点laserOdometry对当前sweep与上一次sweep进行特征匹配计算一个快速(10Hz)但粗略的位姿估计laserMapping对当前sweep与一个局部子图进行特征匹配计算一个慢速(1Hz)比较精确的位姿估计transformMaintenance对两个模块计算出的位姿进行融合得到最终的精确地位姿估计
本文首先介绍scanRegistration模块它的具体功能如下
将输入的无序点云转化为按照线号存储的有序点云接收IMU数据(如果有的话)并基于匀速运动假设使用IMU数据进行插值计算每个点云的曲率剔除论文中提到的不合要求的点提取edge point和planar point
一、main函数
main函数很简单主要是创建了一系列的发布者和订阅者然后ros::spin()进行无限循环其中的主要程序都在回调函数中进行
imuHandler接收IMU数据进行处理laserCloudHandler接收激光雷达数据进行处理这个是该程序的主要函数包含了算法的核心部分。
int main(int argc, char** argv)
{ros::init(argc, argv, scanRegistration);ros::NodeHandle nh;ros::Subscriber subLaserCloud nh.subscribesensor_msgs::PointCloud2 (/velodyne_points, 2, laserCloudHandler);ros::Subscriber subImu nh.subscribesensor_msgs::Imu (/imu/data, 50, imuHandler);pubLaserCloud nh.advertisesensor_msgs::PointCloud2(/velodyne_cloud_2, 2);pubCornerPointsSharp nh.advertisesensor_msgs::PointCloud2(/laser_cloud_sharp, 2);pubCornerPointsLessSharp nh.advertisesensor_msgs::PointCloud2(/laser_cloud_less_sharp, 2);pubSurfPointsFlat nh.advertisesensor_msgs::PointCloud2(/laser_cloud_flat, 2);pubSurfPointsLessFlat nh.advertisesensor_msgs::PointCloud2(/laser_cloud_less_flat, 2);pubImuTrans nh.advertisesensor_msgs::PointCloud2 (/imu_trans, 5);ros::spin();return 0;
}二、接收IMU的消息-imuHandler()
这一部分比较难理解的部分是去除重力影响主要对这部分进行解释。
IMU坐标系为x轴向前y轴向左z轴向上的右手坐标系
首先接收ROS的IMU话题分解出PRY角IMU系相对于全局坐标系的变换是XYZ的变换顺序即旋转矩阵 R w _ i m u R z R y R x R_{w\_imu} R_z R_y R_x Rw_imuRzRyRx重力为全局坐标系中一个向量 g w [ 0 , 0 , 9.81 ] g_w[0,0,9.81] gw[0,0,9.81]需要先变换到IMU坐标系有如下关系 g i m u R i m u _ w g w R w _ i m u − 1 g w R − x R − y R − z ∗ g w g_{imu} R_{imu\_w} g_w R_{w\_imu}^{-1} g_w R_{-x} R_{-y} R_{-z} * g_w gimuRimu_wgwRw_imu−1gwR−xR−yR−z∗gw 计算出来的结果为 g i m u [ − 9.8 s i n β , 9.8 s i n α c o s β , 9.8 c o s α c o s β ] g_{imu} [-9.8sin\beta,9.8sin\alpha cos\beta,9.8cos\alpha cos\beta] gimu[−9.8sinβ,9.8sinαcosβ,9.8cosαcosβ] 而我们测量出来的加速度值是收到重力影响的可以表述为 a i m u m e a s u r e a i m u t r u e g i m u a_{imu}^{measure} a_{imu}^{true} g_{imu} aimumeasureaimutruegimu 所以加速度真值为 a i m u t r u e a i m u m e a s u r e − g i m u a_{imu}^{true} a_{imu}^{measure} - g_{imu} aimutrueaimumeasure−gimu 最后进行坐标系交换变换z轴向前x轴向左y轴向上就有了代码中的样子。
注意这里没有进行坐标变换只是换了一下坐标轴的位置是为了后面计算全局坐标系下的速度和位移积分。
//接收imu消息imu坐标系为x轴向前y轴向左z轴向上的右手坐标系
void imuHandler(const sensor_msgs::Imu::ConstPtr imuIn)
{double roll, pitch, yaw;tf::Quaternion orientation;//convert Quaternion msg to Quaterniontf::quaternionMsgToTF(imuIn-orientation, orientation);//This will get the roll pitch and yaw from the matrix about fixed axes X, Y, Z respectively. Thats R Rz(yaw)*Ry(pitch)*Rx(roll).//Here roll pitch yaw is in the global(init) frametf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);//减去重力的影响,求出xyz方向的加速度实际值并进行坐标轴交换统一到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 (imuPointerLast 1) % imuQueLength; // const int imuQueLength 200imuTime[imuPointerLast] imuIn-header.stamp.toSec();imuRoll[imuPointerLast] roll;imuPitch[imuPointerLast] pitch;imuYaw[imuPointerLast] yaw;imuAccX[imuPointerLast] accX;imuAccY[imuPointerLast] accY;imuAccZ[imuPointerLast] accZ;AccumulateIMUShift();
}三、积分IMU的速度与位移-AccumulateIMUShift()
首先明确一点在进行了坐标系转换(转换成了z轴向前x轴向左y轴向上)后原来的当前帧(局部坐标系)到世界坐标系(全局坐标系)的变换变成了 R Z X Y R y R x R z R_{ZXY} R_y R_x R_z RZXYRyRxRz。
现在的加速度还是在局部坐标系(imu)中现在需要将其变换到世界坐标系然后与之前的速度、位移进行积分。 a w R y R x R z ∗ a i m u a_w R_y R_x R_z * a_{imu} awRyRxRz∗aimu 下面就是根据初中学的公式进行积分 x i x i − 1 v i − 1 ∗ Δ t 1 2 ∗ a i ∗ Δ t 2 v i v i − 1 a i ∗ Δ t x_i x_{i-1} v_{i-1} * \Delta t \frac{1}{2} * a_i * \Delta t^2 \\ v_i v_{i-1} a_i * \Delta t xixi−1vi−1∗Δt21∗ai∗Δt2vivi−1ai∗Δt
//积分速度与位移
void AccumulateIMUShift()
{float roll imuRoll[imuPointerLast];float pitch imuPitch[imuPointerLast];float yaw imuYaw[imuPointerLast];// accX、accY、acc都是世界坐标系下float accX imuAccX[imuPointerLast];float accY imuAccY[imuPointerLast];float accZ imuAccZ[imuPointerLast];//将当前时刻的加速度值绕交换过的ZXY固定轴原XYZ分别旋转(roll, pitch, yaw)角转换得到世界坐标系下的加速度值(right hand rule)//绕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的频率至少比lidar高这样的imu信息才使用后面校正也才有意义if (timeDiff scanPeriod) {//隐含从静止开始运动//求每个imu时间点的位移与速度,两点之间视为匀加速直线运动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;}
}四、接收点云数据-laserCloudHandler()
4.1 预处理与线性插值
首先接收到当前sweep的点云数据后先计算一下点云的起始角度startOri和终止角度endOri对应于下图的α角然后将结束角与起始角差值控制在(PI,3*PI)范围允许lidar不是一个圆周扫描。 下面这个for循环做了这些事情
将点云从雷达坐标系转换到相机坐标系(是坐标系转换不是坐标变换)计算每个点的俯仰角对应于上图的ω角用于计算scanID如果收到IMU数据使用IMU进行插值计算了每个点由于加减速产生的位移和速度畸变将每个点变换到当前sweep的初始时刻里程计的start时刻坐标系
要进行说明的部分如下
点云强度保存
point.intensity scanID scanPeriod * relTime;这里点云强度值保存的格式为“线号 扫描周期(10Hz0.1s) * 点云相对角度”这样保存的好处就是对强度值取整可以得到线号强度值-取整可以计算出相对角度。
IMU去畸变 if (imuPointerLast 0) {//如果收到IMU数据,使用IMU进行插值float pointTime relTime * scanPeriod;//计算点的周期时间//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFrontwhile (imuPointerFront ! imuPointerLast) {if (timeScanCur pointTime imuTime[imuPointerFront]) {break;}imuPointerFront (imuPointerFront 1) % imuQueLength;}
......这里的imuPointerLast表示最新收到IMU数据的位置imuPointerFront表示时间戳刚好大于当前点云时间戳的位置在对点云进行插值时需要使用imuPointerFront和它之前一个位置imuPointerBack。
当找到了一个满足要求的imuPointerFront就是用下式进行插值 v c u r r e n t v i − 1 ( v i − v i − 1 ) ∗ t c u r r e n t − t i − 1 t i − t i − 1 v_{current} v_{i-1} (v_i-v_{i-1})*\frac{t_{current}-t_{i-1}}{t_i-t_{i-1}} vcurrentvi−1(vi−vi−1)∗ti−ti−1tcurrent−ti−1 文章中的式子如下本质上是一样的 v c u r r e n t v i ∗ t c u r r e n t − t i − 1 t i − t i − 1 v i − 1 ∗ t i − t c u r r e n t t i − t i − 1 v_{current} v_i * \frac{t_{current}-t_{i-1}}{t_i-t_{i-1}} v_{i-1} * \frac{t_i - t_{current}}{t_i-t_{i-1}} vcurrentvi∗ti−ti−1tcurrent−ti−1vi−1∗ti−ti−1ti−tcurrent 第一个点云的数值都保存在以Start结尾的变量中。
ShiftToStartIMU(pointTime)函数
这个函数用来计算相对于当前sweep初始时刻 由于加减速产生的位移畸变注意这里的变量都是在全局坐标系下计算的 当前时刻相对于该sweep初始时刻的畸变量。
然后将畸变量由全局坐标系w变换到当前sweep的初始时刻坐标系(start)而现在我们已知的量RPY角所构成的变换为 R w _ s t a r t R y R x R z R_{w\_start} R_y R_x R_z Rw_startRyRxRz 所以这里需要的变换为 R s t a r t _ w R w _ s t a r t − 1 R − z R − x R − y R_{start\_w} R_{w\_start}^{-1} R_{-z} R_{-x} R_{-y} Rstart_wRw_start−1R−zR−xR−y 这样得到了如下代码。
//计算局部(start)坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变
void ShiftToStartIMU(float pointTime)
{//计算相对于第一个点由于加减速产生的畸变位移(全局(w)坐标系下畸变位移量delta_Tg)//imuShiftFromStartCur imuShiftCur - (imuShiftStart imuVeloStart * pointTime)imuShiftFromStartXCur imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;imuShiftFromStartYCur imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;imuShiftFromStartZCur imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;/********************************************************************************delta_Tstart Rz(roll).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Tgtransfrom from the global(w) frame to the local(start) frame*********************************************************************************///绕y轴旋转(-imuYawStart)即Ry(yaw).inversefloat x1 cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;float y1 imuShiftFromStartYCur;float z1 sin(imuYawStart) * imuShiftFromStartXCur cos(imuYawStart) * imuShiftFromStartZCur;//绕x轴旋转(-imuPitchStart)即Rx(pitch).inversefloat x2 x1;float y2 cos(imuPitchStart) * y1 sin(imuPitchStart) * z1;float z2 -sin(imuPitchStart) * y1 cos(imuPitchStart) * z1;//绕z轴旋转(-imuRollStart)即Rz(pitch).inverseimuShiftFromStartXCur cos(imuRollStart) * x2 sin(imuRollStart) * y2;imuShiftFromStartYCur -sin(imuRollStart) * x2 cos(imuRollStart) * y2;imuShiftFromStartZCur z2;
}VeloToStartIMU()函数 这个函数与上面函数的功能一致是将计算由于加减速产生的速度畸变并变换到start坐标系中不再赘述。
//计算局部(start)坐标系下点云中的点相对第一个开始点由于加减速产生的的速度畸变增量
void VeloToStartIMU()
{//计算相对于第一个点由于加减速产生的畸变速度(全局(w)坐标系下畸变速度增量delta_Vg)imuVeloFromStartXCur imuVeloXCur - imuVeloXStart;imuVeloFromStartYCur imuVeloYCur - imuVeloYStart;imuVeloFromStartZCur imuVeloZCur - imuVeloZStart;/********************************************************************************delta_Vstart Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Vgtransfrom from the global(w) frame to the local(start) frame*********************************************************************************///绕y轴旋转(-imuYawStart)即Ry(yaw).inversefloat x1 cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;float y1 imuVeloFromStartYCur;float z1 sin(imuYawStart) * imuVeloFromStartXCur cos(imuYawStart) * imuVeloFromStartZCur;//绕x轴旋转(-imuPitchStart)即Rx(pitch).inversefloat x2 x1;float y2 cos(imuPitchStart) * y1 sin(imuPitchStart) * z1;float z2 -sin(imuPitchStart) * y1 cos(imuPitchStart) * z1;//绕z轴旋转(-imuRollStart)即Rz(pitch).inverseimuVeloFromStartXCur cos(imuRollStart) * x2 sin(imuRollStart) * y2;imuVeloFromStartYCur -sin(imuRollStart) * x2 cos(imuRollStart) * y2;imuVeloFromStartZCur z2;
}TransformToStartIMU(point)函数 这个函数将当前sweep中的所有点云去除由于加减速产生的运动畸变然后都变换到里程计坐标系w下的初始时刻。
我们现在已知当前时刻的PRY角那么可以构成当前时刻坐标系(curr坐标系)相对于世界坐标系(w)的变换 R w _ i m u c u r r R y R x R z R_{w\_imucurr} R_y R_x R_z Rw_imucurrRyRxRz 同样已知了该sweep初始时刻的PRY角可以构成世界坐标系w到该sweep的坐标变换和上面畸变量变换类似 R i m u s t a r t _ w R w _ i m u s t a r t − 1 R − z R − x R − y R_{imustart\_w} R_{w\_imustart}^{-1} R_{-z} R_{-x} R_{-y} Rimustart_wRw_imustart−1R−zR−xR−y p c u r r i m u s t a r t p_{curr}^{imustart} pcurrimustart表示curr坐标系下对应于imustart姿态静止扫描得到的点那么最终的变换为 p c u r r i m u s t a r t R i m u s t a r t _ w ∗ R w _ i m u c u r r ∗ p c u r r i m u c u r r p_{curr}^{imustart} R_{imustart\_w} * R_{w\_imucurr} * p_{curr}^{imucurr} pcurrimustartRimustart_w∗Rw_imucurr∗pcurrimucurr 最后再加上上面计算出的由于加减速产生的位移畸变量就得到了如下代码。
这个过程完成的工作应该如下图所示 得到了一个去畸变后的点云结果效果相当于得到了在点云扫描开始位置静止扫描得到的点云位置也就是说没有对点云坐标系进行变换第i个点云依然处在里程计坐标系下的curr时刻坐标系中这一步的操作只是对点云的位置进行调整然后这一帧点云扫描过程中按照这个调整后位置送入传感器中。
还有一种解释是说这个函数操作完了之后current时刻的坐标系变成了仍然以current时刻为原点坐标轴的姿态与初始时刻start的姿态相同我觉得这个理解也可以。
//将当前时刻curr坐标系下的的点云变换到世界坐标系w然后在变换到当前帧的初始时刻start坐标系下
void TransformToStartIMU(PointType *p)
{/********************************************************************************Pinit Ry * Rx * Rz * Pcurrtransform current camara(curr) frame point to the global(w) frame*********************************************************************************///绕z轴旋转(imuRollCur)float x1 cos(imuRollCur) * p-x - sin(imuRollCur) * p-y;float y1 sin(imuRollCur) * p-x cos(imuRollCur) * p-y;float z1 p-z;//绕x轴旋转(imuPitchCur)float x2 x1;float y2 cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;float z2 sin(imuPitchCur) * y1 cos(imuPitchCur) * z1;//绕y轴旋转(imuYawCur)float x3 cos(imuYawCur) * x2 sin(imuYawCur) * z2;float y3 y2;float z3 -sin(imuYawCur) * x2 cos(imuYawCur) * z2;/********************************************************************************Pstart Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * Pinittransfrom global(w) points to the local(start) frame*********************************************************************************///绕y轴旋转(-imuYawStart)float x4 cos(imuYawStart) * x3 - sin(imuYawStart) * z3;float y4 y3;float z4 sin(imuYawStart) * x3 cos(imuYawStart) * z3;//绕x轴旋转(-imuPitchStart)float x5 x4;float y5 cos(imuPitchStart) * y4 sin(imuPitchStart) * z4;float z5 -sin(imuPitchStart) * y4 cos(imuPitchStart) * z4;//绕z轴旋转(-imuRollStart)然后叠加平移量p-x cos(imuRollStart) * x5 sin(imuRollStart) * y5 imuShiftFromStartXCur;p-y -sin(imuRollStart) * x5 cos(imuRollStart) * y5 imuShiftFromStartYCur;p-z z5 imuShiftFromStartZCur;
}当上面这些操纵都做完之后将得到的start坐标系下去畸变的点放入按scanID存储的点云容器laserCloudScans所有代码如下
//接收点云数据velodyne雷达坐标系安装为x轴向前y轴向左z轴向上的右手坐标系
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr laserCloudMsg)
{if (!systemInited) {//丢弃前20个点云数据systemInitCount;if (systemInitCount systemDelay) {systemInited true;}return;}//记录每个scan有曲率的点的开始和结束索引std::vectorint scanStartInd(N_SCANS, 0);std::vectorint scanEndInd(N_SCANS, 0);//当前sweep的时间double timeScanCur laserCloudMsg-header.stamp.toSec();pcl::PointCloudpcl::PointXYZ laserCloudIn;//消息转换成pcl数据存放pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);std::vectorint indices;//移除空点pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);//点云点的数量int cloudSize laserCloudIn.points.size();//lidar scan开始点的旋转角,atan2范围[-pi,pi],计算旋转角时取负号是因为velodyne是顺时针旋转float startOri -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);//lidar scan结束点的旋转角加2*pi使点云旋转周期为2*pifloat endOri -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) 2 * M_PI;//结束方位角与开始方位角差值控制在(PI,3*PI)范围允许lidar不是一个圆周扫描//正常情况下在这个范围内pi endOri - startOri 3*pi异常则修正if (endOri - startOri 3 * M_PI) {endOri - 2 * M_PI;} else if (endOri - startOri M_PI) {endOri 2 * M_PI;}//lidar扫描线是否旋转过半bool halfPassed false;int count cloudSize;PointType point;std::vectorpcl::PointCloudPointType laserCloudScans(N_SCANS);/* 这个for循环做了这些事情* 1.将点云从雷达坐标系转换到相机坐标系* 2.计算每个点的俯仰角用于计算scanID* 3.如果收到IMU数据,使用IMU进行插值* 4.计算了每个点由于加减速产生的位移和速度畸变* 5.将每个点变换到当前sweep的初始时刻start坐标系*/for (int i 0; i cloudSize; i) {//把雷达坐标系(前-左-上)中的点云转换到里程计标系(左上前) X-Z Y-X Z-Ypoint.x laserCloudIn.points[i].y;point.y laserCloudIn.points[i].z;point.z laserCloudIn.points[i].x;//计算点的仰角(根据lidar文档垂直角计算公式),根据仰角排列激光线号velodyne每两个scan之间间隔2度float angle atan(point.y / sqrt(point.x * point.x point.z * point.z)) * 180 / M_PI;int scanID;//仰角四舍五入(加减0.5截断效果等于四舍五入)int roundedAngle int(angle (angle0.0?-0.5:0.5)); if (roundedAngle 0){scanID roundedAngle;}else {scanID roundedAngle (N_SCANS - 1);}//过滤点只挑选[-15度15度]范围内的点,scanID属于[0,15]if (scanID (N_SCANS - 1) || scanID 0 ){count--;continue;}//该点的旋转角float ori -atan2(point.x, point.z);if (!halfPassed) {//根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算从而进行补偿//确保-pi/2 ori - startOri 3*pi/2if (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;//确保-3*pi/2 ori - endOri pi/2if (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);//点强度线号点相对时间即一个整数一个小数整数部分是线号小数部分是该点的相对时间,匀速扫描根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间point.intensity scanID scanPeriod * relTime;if (imuPointerLast 0) {//如果收到IMU数据,使用IMU进行插值float pointTime relTime * scanPeriod;//计算点的周期时间//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFrontwhile (imuPointerFront ! imuPointerLast) {if (timeScanCur pointTime imuTime[imuPointerFront]) {break;}imuPointerFront (imuPointerFront 1) % imuQueLength;}if (timeScanCur pointTime imuTime[imuPointerFront]) {//没找到,此时imuPointerFrontimtPointerLast,只能以当前收到的最新的IMU的速度位移欧拉角作为当前点的速度位移欧拉角使用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];} else {//找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间据此线性插值计算点云点的速度位移和欧拉角int imuPointerBack (imuPointerFront imuQueLength - 1) % imuQueLength; // imuPointerBack是imuPointerFront的上一个位置//按时间距离计算权重分配比率,也即线性插值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[imuPointerback] (imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFrontimuVeloXCur 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;} else {ShiftToStartIMU(pointTime);VeloToStartIMU();TransformToStartIMU(point);//将当前时刻curr坐标系下的的点云变换到世界坐标系w然后在变换到当前帧的初始时刻start坐标系下}}laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器}4.2 计算曲率
这一部分对应论文中提到的曲率计算公式 c 1 ∣ S ∣ ⋅ ∣ ∣ X ( k , i ) L ∣ ∣ ∣ ∣ ∑ j ∈ S , j ≠ i ( X ( k , i ) L − X ( k , j ) L ) ∣ ∣ c \frac{1}{|S|·||X_{(k,i)}^L||} ||\sum _{j \in S, j\ne i} (X_{(k,i)}^L - X_{(k,j)}^L)|| c∣S∣⋅∣∣X(k,i)L∣∣1∣∣j∈S,ji∑(X(k,i)L−X(k,j)L)∣∣ X ( k , i ) L X_{(k,i)}^L X(k,i)L是第i个点云的位置 X ( k , j ) L X_{(k,j)}^L X(k,j)L是 X ( k , i ) L X_{(k,i)}^L X(k,i)L左右两边各5个点注意它们两个都是矢量那么 ( X ( k , i ) L − X ( k , j ) L ) (X_{(k,i)}^L - X_{(k,j)}^L) (X(k,i)L−X(k,j)L)就是一个 X ( k , i ) L X_{(k,i)}^L X(k,i)L指向 X ( k , j ) L X_{(k,j)}^L X(k,j)L的向量。 如上图所示如果一个点是edge point那么向量求和后得到结果的模很大如果一个点是planar point那么与两侧五个向量求和后结果是0通过这种方式区分特征点。
求解完所有点的曲率后scanStartInd[]和scanEndInd[]数组用于记录下每个scanID有曲率的起始点和终止点的索引。
//获得有效范围内的点的数量cloudSize count;//这里就按照scanID变成了有序点云pcl::PointCloudPointType::Ptr laserCloud(new pcl::PointCloudPointType());//将所有的点按照线号从小到大放入一个容器for (int i 0; i N_SCANS; i) {*laserCloud laserCloudScans[i];}int scanCount -1;//使用每个点的前后五个点计算曲率因此前五个与最后五个点跳过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;//初始化为less flat点cloudLabel[i] 0;//每个scan只有第一个符合的点会进来因为每个scan的点都在一起存放if (int(laserCloud-points[i].intensity) ! scanCount) {scanCount int(laserCloud-points[i].intensity);//控制每个scan只进入第一个点//曲率只取同一个scan计算出来的跨scan计算的曲率非法排除也即排除每个scan的前后五个点if (scanCount 0 scanCount N_SCANS) {scanStartInd[scanCount] i 5;scanEndInd[scanCount - 1] i - 5;}}}//第一个scan曲率点有效点序从第5个开始最后一个激光线结束点序size-5scanStartInd[0] 5;scanEndInd.back() cloudSize - 5;4.3 剔除不好的点 这部分对应于论文中提到的计算完曲率后最终的特征点需要满足以下要求
所选边缘点或平面点的个数不能超过子区域的最大值-------保证取点均匀这一点下面会讲到它周围的点都没有被选中-------保证点不过于密集它不能位于与激光束大致平行的平面上也不能位于遮挡区域的边界上-------剔除一些不好的点(下面的操作) 下面代码中的这个if用于剔除类似于图b中A点这样的易遮挡点 if (diff 0.1) 当传感器在这个角度时A点看起来是edge point但稍微移动时A点变为planar或者不可见这种是不靠谱的需要剔除。
代码中的意思是如果A和B距离相差0.1米以上就求解它们两个的深度将深度大的点放缩到同一距离水平然后用深度距离(距离很小近似为弧长)/深度这个得到的就是两者夹角的弧度值如果这个夹角很小说明就是图b中的情况A很容易被遮挡。
下面代码中的这个if用于剔除类似于图a中B点这样的所在平面与激光束近似平行的点 if (diff 0.0002 * dis diff2 0.0002 * dis) diff和diff2分别是与后一个点、前一个点距离的平方如果出现图a中B点这样的情况diff和diff2值会很大如果大于当前点深度的0.0002则认为出现图a中的情况需要剔除。 //这个for循环排除容易被斜面挡住的点、所在平面近似与激光束平行的点以及离群点(噪点)for (int i 5; i cloudSize - 6; i) {//与后一个点差值所以减6float 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;//排除一些易遮挡的点对应论文中图4(b)的A点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) {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说明夹角比较小斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上if (sqrt(diffX * diffX diffY * diffY diffZ * diffZ) / depth2 0.1) {//排除容易被斜面挡住的点//该点及前面五个点大致都在斜面上全部置为筛选过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;}}}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;//点深度的平方和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;//与前后点的平方和都大于深度平方和的万分之二这些点视为离群点包括陡斜面上的点强烈凸凹点和空旷区域中的某些点置为筛选过弃用//对应于论文中的图4(a)中的Bif (diff 0.0002 * dis diff2 0.0002 * dis) {cloudNeighborPicked[i] 1;}}4.4 特征提取
这部分与论文中说的有点不一样论文中说将当前sweep分为4个相同区域而代码中是分为了6个区域每个区域的起始点和终止点索引分别为sp和ed其计算本质如下 六等份起点sp scanStartInd (scanEndInd - scanStartInd)*j/6 六等份终点ep scanStartInd - 1 (scanEndInd - scanStartInd)*(j1)/6 1.按照曲率从小到大进行冒泡排序A-LOAM中使用的是sort函数。
2.然后如果曲率值大于0.1则选择为edge point(边缘特征点)或less edge point(没那么尖锐的边缘特征点)edge point对应论文中提到的每个区域选择2个less edge point每个区域选择20个。
3.每选择一个点后就将曲率比较大的点的前后各5个连续距离比较近的点筛选出去防止特征点聚集使得特征点在每个方向上尽量分布均匀。
4.然后如果曲率值小于0.1则选择为planar point(平面特征点)或less planar point(没那么平坦的平面特征点)planar point对应论文中提到的每个区域选择4个而该区域剩下的全都归入less edge point。
5.同样的每选择一个点后就将曲率比较大的点的前后各5个连续距离比较近的点筛选出去防止特征点聚集使得特征点在每个方向上尽量分布均匀。
6.最后由于less planar point点最多对每个区域less planar point的点进行体素栅格滤波 pcl::PointCloudPointType cornerPointsSharp;pcl::PointCloudPointType cornerPointsLessSharp;pcl::PointCloudPointType surfPointsFlat;pcl::PointCloudPointType surfPointsLessFlat;//这个for循环将每条线上的点分入相应的类别边沿点和平面点for (int i 0; i N_SCANS; i) {pcl::PointCloudPointType::Ptr surfPointsLessFlatScan(new pcl::PointCloudPointType);//将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点for (int j 0; j 6; j) {//六等份起点sp scanStartInd (scanEndInd - scanStartInd)*j/6int sp (scanStartInd[i] * (6 - j) scanEndInd[i] * j) / 6;//六等份终点ep scanStartInd - 1 (scanEndInd - scanStartInd)*(j1)/6int 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;// 这里对应选点规则第二点if (largestPickedNum 2) {//挑选曲率最大的前2个点放入sharp点集合cloudLabel[ind] 2;//2代表点曲率很大cornerPointsSharp.push_back(laserCloud-points[ind]);cornerPointsLessSharp.push_back(laserCloud-points[ind]);} else if (largestPickedNum 20) {//挑选曲率最大的前20个点放入less sharp点集合cloudLabel[ind] 1;//1代表点曲率比较尖锐cornerPointsLessSharp.push_back(laserCloud-points[ind]);} else {break;}cloudNeighborPicked[ind] 1;//筛选标志置位//这里对应选点规则第三点//将曲率比较大的点的前后各5个连续距离比较近的点筛选出去防止特征点聚集使得特征点在每个方向上尽量分布均匀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];//如果曲率的确比较小并且未被筛选出if (cloudNeighborPicked[ind] 0 cloudCurvature[ind] 0.1) {cloudLabel[ind] -1;//-1代表曲率很小的点surfPointsFlat.push_back(laserCloud-points[ind]);smallestPickedNum;if (smallestPickedNum 4) {//只选最小的四个剩下的Label0,就都是曲率比较小的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;}}}//将剩余的点包括之前被排除的点全部归入平面点中less flat类别中for (int k sp; k ep; k) {if (cloudLabel[k] 0) {surfPointsLessFlatScan-push_back(laserCloud-points[k]);}}}//由于less flat点最多对每个分段less flat的点进行体素栅格滤波pcl::PointCloudPointType surfPointsLessFlatScanDS;pcl::VoxelGridPointType downSizeFilter;downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.setLeafSize(0.2, 0.2, 0.2);downSizeFilter.filter(surfPointsLessFlatScanDS);//less flat点汇总surfPointsLessFlat surfPointsLessFlatScanDS;}4.5 发布话题
最后这部分就是ROS中的发布话题没什么可讲的总结一下发布的话题都是什么
/velodyne)cloud_2经过处理后的所有点云(按照scanID排序的有序点云)/laser_cloud_sharpedge point特征点一共1662192个/laser_cloud_less_sharpless edge point特征点一共166201920个/laser_cloud_flatplanar point特征点一共1664384个/laser_cloud_less_flatless planar point特征点其余的满足要求的个/imu_trans包含了当前sweep的IMU测量的起始RPY角、终止RPY角、最后一个点相对于第一个点的畸变位移和速度 //publich消除非匀速运动畸变后的所有的点sensor_msgs::PointCloud2 laserCloudOutMsg;pcl::toROSMsg(*laserCloud, laserCloudOutMsg);laserCloudOutMsg.header.stamp laserCloudMsg-header.stamp;laserCloudOutMsg.header.frame_id /camera;pubLaserCloud.publish(laserCloudOutMsg);//publish消除非匀速运动畸变后的平面点和边沿点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);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::PointCloudpcl::PointXYZ imuTrans(4, 1); // 1行4列的有序点云//起始点欧拉角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);
}总结
本篇文章介绍了LOAM代码的第一个节点文件scanRegistration我感觉这个代码还是很好懂得坐标变换部分也不是很复杂对照论文慢慢看就能看懂。
下一篇将介绍laserOdometry.cpp也就是雷达里程计部分
