1原理
利用激光雷达数据进行机器人的位姿估计。具体来说,它通过订阅激光雷达的不同类型点云话题,获取尖锐角点、不尖锐角点、平坦平面和不平坦平面等信息。然后,对这些点云数据进行处理和关联,利用 Ceres 库进行优化求解,估计机器人的位姿变换。最后,将位姿信息发布到 ROS 话题中,供其他节点使用。
- 点云数据处理:代码首先对订阅到的点云数据进行处理,包括去畸变、坐标变换等操作。
- 特征提取:从点云中提取尖锐角点和平坦平面等特征,并建立相应的 Kd 树用于快速搜索最近邻点。
- 对应关系建立:通过遍历特征点,利用 Kd 树搜索最近邻点,并根据距离和扫描线信息建立对应关系。
- 位姿优化:使用 Ceres 库构建优化问题,将对应关系作为约束,通过最小化误差函数来求解机器人的位姿变换。
- 位姿发布:将优化得到的位姿信息发布到 ROS 话题中
2 实现
2.1去畸变函数
先转换到初始点,依据匀速模型。
再转换到结束点,迭代优化。
// 对激光雷达点进行去畸变
void TransformToStart(PointType const *const pi, PointType *const po)
{
// 插值比例
double s;
// 如果考虑畸变
if (DISTORTION)
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
else
s = 1.0;
//s = 1;
// 上一帧的四元数
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
// 上一帧的平移
Eigen::Vector3d t_point_last = s * t_last_curr;
// 当前点的坐标
Eigen::Vector3d point(pi->x, pi->y, pi->z);
// 去畸变后的点坐标
Eigen::Vector3d un_point = q_point_last * point + t_point_last;
po->x = un_point.x();
po->y = un_point.y();
po->z = un_point.z();
po->intensity = pi->intensity;
}
// 将所有激光雷达点变换到下一帧的开始
void TransformToEnd(PointType const *const pi, PointType *const po)
{
// 先去畸变
pcl::PointXYZI un_point_tmp;
TransformToStart(pi, &un_point_tmp);
Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);
Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);
po->x = point_end.x();
po->y = point_end.y();
po->z = point_end.z();
// 去除畸变时间信息
po->intensity = int(pi->intensity);
}
2.2主函数
功能:
- main 函数:
- 初始化 ROS 节点。
- 订阅激光雷达点云话题。
- 创建 ROS 发布者,发布激光里程计、激光路径和处理后的点云数据。
- 在主循环中,处理接收到的点云数据。
- 如果系统尚未初始化,则进行初始化操作。
- 否则,进行点云数据的关联和优化。
- 发布激光里程计和路径。
- 转换和发布处理后的点云数据。
- 统计时间并输出警告信息。
(1)订阅和发布的话题
// 订阅尖锐角点话题
ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);
// 订阅不尖锐角点话题
ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);
// 订阅平坦平面话题
ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);
// 订阅不平坦平面话题
ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);
// 订阅全分辨率激光云话题
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);
// 发布上一帧尖锐角点的激光云话题
ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100);
// 发布上一帧平坦平面的激光云话题
ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100);
// 发布全分辨率激光云话题
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100);
// 发布激光里程计话题
ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);
// 发布激光路径话题
ros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/laser_odom_path", 100);
(2)点云数据的关联和优化
利用Ceres库(一个非线性优化器)解决基于激光雷达数据的形状配准问题。首先,它初始化了一些必要的变量和对象,然后进入一个循环,针对边缘点和平面点分别进行配准。同时在每个循环中,它首先找到当前点在上一帧激光雷达数据中的 nearest neighbor,然后根据扫描ID的差异来搜索更近或更远的点。
// 寻找平面特征的对应关系
for (int i = 0; i < surfPointsFlatNum; ++i)
{
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0];
// 获取最近点的扫描 ID
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
// 在扫描线增加的方向搜索
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
// 如果不在附近扫描,结束循环
if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// 如果在同一或更低扫描线,更新 minPointSqDis2
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// 如果在更高扫描线,更新 minPointSqDis3
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// 在扫描线减少的方向搜索
for (int j = closestPointInd - 1; j >= 0; --j)
{
// 如果不在附近扫描,结束循环
if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// 如果在同一或更高扫描线,更新 minPointSqDis2
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// 如果在更低扫描线,更新 minPointSqDis3
else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
if (minPointInd2 >= 0 && minPointInd3 >= 0)
{
Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,
surfPointsFlat->points[i].y,
surfPointsFlat->points[i].z);
Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,
laserCloudSurfLast->points[closestPointInd].y,
laserCloudSurfLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
laserCloudSurfLast->points[minPointInd2].y,
laserCloudSurfLast->points[minPointInd2].z);
Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,
laserCloudSurfLast->points[minPointInd3].y,
laserCloudSurfLast->points[minPointInd3].z);
double s;
if (DISTORTION)
s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
plane_correspondence++;
}
}
}
// 优化两次
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
// 重置对应点数量
corner_correspondence = 0;
plane_correspondence = 0;
//ceres::LossFunction *loss_function = NULL;
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);
pcl::PointXYZI pointSel;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
TicToc t_data;
// 寻找角点特征的对应关系
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0];
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// 搜索在扫描线增加的方向
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
{
// 如果在同一扫描线,继续
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
continue;
// 如果不在附近扫描,结束循环
if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
double 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 (pointSqDis < minPointSqDis2)
{
// 找到更近的点
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
// 搜索在扫描线减少的方向
for (int j = closestPointInd - 1; j >= 0; --j)
{
// 如果在同一扫描线,继续
if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
continue;
// 如果不在附近扫描,结束循环
if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double 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 (pointSqDis < minPointSqDis2)
{
// 找到更近的点
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
// 找到角点特征的对应关系
if (minPointInd2 >= 0) // 两个最近点都有效
{
Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
cornerPointsSharp->points[i].y,
cornerPointsSharp->points[i].z);
Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
laserCloudCornerLast->points[closestPointInd].y,
laserCloudCornerLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
laserCloudCornerLast->points[minPointInd2].y,
laserCloudCornerLast->points[minPointInd2].z);
double s;
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
corner_correspondence++;
}
}
// 寻找平面特征的对应关系
for (int i = 0; i < surfPointsFlatNum; ++i)
{
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0];
// 获取最近点的扫描 ID
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
// 搜索在扫描线增加的方向
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
// 如果不在附近扫描,结束循环
if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// 如果在同一或更低扫描线,更新 minPointSqDis2
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// 如果在更高扫描线,更新 minPointSqDis3
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// 搜索在扫描线减少的方向
for (int j = closestPointInd - 1; j >= 0; --j)
{
// 如果不在附近扫描,结束循环
if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j