这是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;