本文详细解析了LOAM（Lidar Odometry and Mapping）算法中的激光雷达里程计部分，包括点云预处理、特征点关联、Jacobian矩阵构建与姿态解算、坐标转换等关键步骤。通过边缘点和平面点的匹配，使用LM算法估计位姿，并结合IMU信息修正旋转量，最终实现局部到全局坐标系的转换。
这是LOAM第二部分Lidar laserOdometry雷达里程计。
在第一章提取完特征点后，需要对特征点云进行关联匹配，之后估计姿态。
主要分为两部分：
特征点关联使用scan-to-scan方式t和t+1时刻相邻两帧的点云数据进行配准，分为边缘点匹配和平面点匹配两部分。计算点到直线的距离和点到平面的距离。
姿态解算根据匹配的特征点云使用LM算法估计接收端位姿。
这部分代码完全是放在main函数。
首先介绍前面的参数。后面出现的会重点标注下。
//一个点云周期
const float scanPeriod = 0.1;
//接收到边缘点和平面点
pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());
//上一帧点云的边缘点和平面点
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
//保存前一个节点发过来的未经处理过的特征点和权重
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());
//全部点云
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
//imu信息
pcl::PointCloud<pcl::PointXYZ>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZ>());
//上一帧边缘点、平面点构建的KD-tree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());
//当前帧相对上一帧的状态转移量，局部坐标系
float transform[6] = {
0};
//当前帧相对于第一帧的状态转移量，全局坐标系
float transformSum[6] = {
0};
//点云第一个点的RPY、最后一个点的RPY
float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0;
float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
//点云最后一个点相对于第一个点由于加减速产生的畸变位移、速度
float imuShiftFromStartX = 0, imuShiftFromStartY = 0, imuShiftFromStartZ = 0;
float imuVeloFromStartX = 0, imuVeloFromStartY = 0, imuVeloFromStartZ = 0;
0、预处理
- 订阅节点，发布消息
- 创建里程计对象、坐标转换对象
- 同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入
6个sub订阅器分别订阅scanRegistration节点发布的6个消息：/laser_cloud_sharp、/laser_cloud_less_sharp、pubSurfPointFlat、pubSurfPointLessFlat、/velodyne_cloud_2、/imu_trans消息。
调用6个Handler回调函数处理这些边特征、面特征、全部点云和IMU数据，把他们从ROS::Msg类型转换成程序可以处理的pcl点云类型；
4个pub发布器发布/laser_cloud_corner_last、/laser_cloud_surf_last、/velodyne_cloud_3、/laser_odom_to_init消息。
int main(int argc, char** argv)
{
/****************0、预处理************************/
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
// 1.订阅节点，发布消息
// 订阅scanRegistration发布的六个节点,调用六个回调函数
ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_sharp", 2, laserCloudSharpHandler);
ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);
ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_flat", 2, laserCloudFlatHandler);
ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>
("/velodyne_cloud_2", 2, laserCloudFullResHandler);
ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2>
("/imu_trans", 5, imuTransHandler);// imu队列长度5
// 发布四个节点，发布消息
ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_corner_last", 2);// 队列长度2
ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_surf_last", 2);
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
("/velodyne_cloud_3", 2);
ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);// 队列长度5
// 2.创建里程计对象、坐标转换对象
// 创建里程计对象 laserOdometry
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.child_frame_id = "/laser_odom";
tf::TransformBroadcaster tfBroadcaster;
// 创建坐标变换对象 laserOdometryTrans
tf::StampedTransform laserOdometryTrans;
laserOdometryTrans.frame_id_ = "/camera_init";
laserOdometryTrans.child_frame_id_ = "/laser_odom";
std::vector<int> pointSearchInd;//搜索到的点序
std::vector<float> pointSearchSqDis;//搜索到的点平方距离
PointType pointOri, pointSel/*选中的特征点*/, tripod1, tripod2, tripod3/*特征点的对应点*/, pointProj/*unused*/, coeff;
//退化标志
bool isDegenerate = false;
//P矩阵，预测矩阵
cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));
int frameCount = skipFrameNum;
ros::Rate rate(100);
bool status = ros::ok();
while (status) {
// 调用一次回调函数，订阅当前时刻msg和发布上一时刻处理后的msg
ros::spinOnce();
// 3.同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入
if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat &&
newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&
fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005)
{
newCornerPointsSharp = false;
newCornerPointsLessSharp = false;
newSurfPointsFlat = false;
newSurfPointsLessFlat = false;
newLaserCloudFullRes = false;
newImuTrans = false;
接下来分为：初始化、点云处理、构建Jacobian矩阵估计位姿、坐标转换。
1、初始化
将第一个点云数据集发送给laserMapping,从下一个点云数据开始配准。
- 将第一个点云订阅的数据保存为上一时刻数据
- 从ROS转化为pcl，再发布出去
- 记住原点的翻滚角和俯仰角为imu
- T平移量的初值赋值为imu加减速的位移量，为其梯度下降的方向
//********************1、初始化：将第一个点云数据集发送给laserMapping,从下一个点云数据开始配准****************
if (!systemInited) {
// 1.将订阅的数据保存为上一时刻数据。
// 将边缘点和上一帧边缘点交换，保存当前帧边缘点值cornerPointsLessSharp下轮使用
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
//将平面点和上一帧平面点交换，，保存当前帧平面点surfPointsLessFlat的值下轮使用
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
//使用上一帧的特征点（边缘+平面）构建kd-tree
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);//所有的边沿点集合
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//所有的平面点集合
// 2.将上一时刻数据（边缘+平面）从ROS转化为pcl，再发布出去
sensor_msgs::PointCloud2 laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudCornerLast2.header.frame_id = "/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
sensor_msgs::PointCloud2 laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudSurfLast2.header.frame_id = "/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
//3.记住原点的翻滚角和俯仰角
transformSum[0] += imuPitchStart;
transformSum[2] += imuRollStart;
systemInited = true;
continue;
}
//4.T平移量的初值赋值为加减速的位移量，为其梯度下降的方向
//（沿用上次转换的T（一个sweep匀速模型），同时在其基础上减去匀速运动位移，即只考虑加减速的位移量）
transform[3] -= imuVeloFromStartX * scanPeriod;
transform[4] -= imuVeloFromStartY * scanPeriod;
transform[5] -= imuVeloFromStartZ * scanPeriod;
2、点云配准
点云配准前言：首先保证上一时刻特征点（边缘+平面）数量足够再开始进行匹配，其次，后面需要25次迭代LM算法求解位姿。
重要参数：
laserCloudOri：点云过滤：保留距离小，权重大的点；舍弃距离为零的点。
coeffSel：权重
if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {
//上一时刻特征点数量足够
// 当前时刻特征点（边缘+平面）数量
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);
int cornerPointsSharpNum = cornerPointsSharp->points.size();
int surfPointsFlatNum = surfPointsFlat->points.size();
//多次迭代LM算法求解前后位姿变化，这里25次
for (int iterCount = 0; iterCount < 25; iterCount++) {
laserCloudOri->clear();
coeffSel->clear();
针对边缘点和平面点两部分分别进行匹配关系处理。
以边缘点为例：
- kd-tree查找一个最近距离点j，Ind为索引，SqDis为距离平方
- 寻找最邻近点j附近的次临近点l：分为scanID增加和减小两个方向分别进行寻找。
- 计算点到线的距离，根据距离分配权重
边缘点处理：
/********************边缘点处理************/
//从上个点云中边缘点中找两个最近距离点j和l
//一个点j使用kd-tree查找，在最邻近点j附近(向上下三条扫描线以内)找到次临近点l
for (int i = 0; i < cornerPointsSharpNum; i++) {
// 0、t+1时刻的边缘点转换到初始imu坐标系的点坐标 pointSel
TransformToStart(&cornerPointsSharp->points[i], &pointSel);
// ？？？每迭代五次，重新查找最近点（降采样）
if (iterCount % 5 == 0) {
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);
// 1、kd-tree查找一个最近距离点j，Ind为索引，SqDis为距离平方
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
//在最邻近点j附近(向上下三条扫描线以内)找到次临近点l
//再次提醒：velodyne是2度一线，scanID相邻并不代表线号相邻，相邻线度数相差2度，也即线号scanID相差2
if (pointSearchSqDis[0] < 25) {
//找到的最近点距离的确很近的话
closestPointInd = pointSearchInd[0];// 最近点j
//提取最近点线号
int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);
float pointSqDis, minPointSqDis2 = 25;//初始门槛值5米，可大致过滤掉scanID相邻，但实际线不相邻的值
// 2、寻找最邻近点j附近的次临近点l
// 向scanID增大的方向查找
for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {
//非相邻线结束，距离超过三条线跳出
if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) {
break;
}
// 计算所有点和最近邻点j距离平方
pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) {
//确保两个点不在同一条scan上（相邻线查找应该可以用scanID == closestPointScan +/- 1 来做）
if (pointSqDis < minPointSqDis2) {
//距离更近，要小于初始值5米
//更新最小距离与点序
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
//向scanID减小的方向查找，向下三条线
for (int j = closestPointInd - 1; j >= 0; j--) {
if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) {
break;
}
pointSqDis = (laserCloudCornerLast->points
最低0.47元/天 解锁文章
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
