A-LOAM代码算法
原创
1.概述
ALoam 的软件架构是基于ros框架的，其运行节点关系图如下所示:
ALoam.png
2. 代码
2.1 点云特征提取
该部分函数完成激光雷达点云数据的读取，移除无效点云，计算每条扫描线点云中每个点相对于该条扫描线起始点的时间间隔(用于后续点云去几遍)，根据俯仰角判断点云的扫描线id, 并根据周围点的坐标计算每个点的曲率，根据曲率将所有的点云分为sharp点，lesssharp点，flat点以及lessflat点，最后将四类点打包发送到odometry模块
int main(int argc, char **argv) {
// 设置ros 节点名称
ros::init(argc, argv, "scanRegistration");
ros::NodeHandle nh;
// 16 线激光雷达
nh.param<int>("scan_line", N_SCANS, 16);
// 设置点和点之间的最小距离为0.1没事，如果一条三秒线两个点之间距离小于0.1m或者过滤或者认为是错误
nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
printf("scan line number %d \n", N_SCANS);
if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64) {
printf("only support velodyne with 16, 32 or 64 scan line!");
return 0;
}
// 设置ros 订阅的msg名称及回调函数，param1为msg名称，param2列大小，param3为回调函数
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
// 设置ros 要发布的的msg名称以及队列大小
pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
// 设置ros 要发布的线点云
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
// 设置ros 要发布的类线点云
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
// 设置ros 要发布的面点云
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
// 设置ros 要发布的类面点云
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
// 设置ros 要发布的删除
pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);
if(PUB_EACH_LINE) {
for(int i = 0; i < N_SCANS; i++) {
ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
ros::spin();
return 0;
}从以上main函数部分可以看到，程序一直在接受subscribe "/velodyne_points" msg并且运行回调函数laserCloudHandler，即对点云的预处理函数，同时在发布advertise 6类点云，我们主要看laserCloudHandler函数
2.1.1 点云处理
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {
if (!systemInited) {
systemInitCount++;
if (systemInitCount >= systemDelay) {
systemInited = true;
} else
return;
}
TicToc t_whole;
TicToc t_prepare;
// 保存每个scan中曲率计算的开始索引号
std::vector<int> scanStartInd(N_SCANS, 0);
// 保存每个scan中曲率计算的结束索引号
std::vector<int> scanEndInd(N_SCANS, 0);
// 用于将 ros 定义的msg转化为pcl格式的点云
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
// 移除点云值为Nan的点云
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
// 根据点点最小距离过滤掉点点距离小于0.1的点云
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
int cloudSize = laserCloudIn.points.size();
// 计算开始点的角度，其实是yaw，这里取反的原因在于激光雷达坐标系如下图所示是顺时针为帧，而我们的角度的极坐标逆时针为正, atan2 的取值是 -M_PI~+M_PI
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
// 增加 2 * M_PI 是为了使得 endOri始终为大于starOri
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) +
2 * M_PI;
if (endOri - startOri > 3 * M_PI) {
endOri -= 2 * M_PI;
} else if (endOri - startOri < M_PI) {
endOri += 2 * M_PI;
}
//printf("end Ori %f\n", endOri);
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
for (int i = 0; i < cloudSize; i++) {
point.x = laserCloudIn.points[i].x;
point.y = laserCloudIn.points[i].y;
point.z = laserCloudIn.points[i].z;
// 计算每个点俯仰角
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
if (N_SCANS == 16) {
// 俯仰角分辨率为2度，范围为-15度~+15度，+0.5是四舍五入操作
scanID = int((angle + 15) / 2 + 0.5);
if (scanID > (N_SCANS - 1) || scanID < 0) {
count--;
continue;
}
} else if (N_SCANS == 32) {
scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
if (scanID > (N_SCANS - 1) || scanID < 0) {
count--;
continue;
}
} else if (N_SCANS == 64) {
if (angle >= -8.83)
scanID = int((2 - angle) * 3.0 + 0.5);
else
scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
// use [0 50] > 50 remove outlies
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0) {
count--;
continue;
}
} else {
printf("wrong scan number\n");
ROS_BREAK();
}
//printf("angle %f scanID %d \n", angle, scanID);
float ori = -atan2(point.y, point.x);
// 扫描是否过半
if (!halfPassed) {
// 使得每个点的yaw都在合理范围之内
if (ori < startOri - M_PI / 2) {
ori += 2 * M_PI;
} else if (ori > startOri + M_PI * 3 / 2) {
ori -= 2 * M_PI;
}
// 每个scan的角度范围都是 2 * M_PI
if (ori - startOri > M_PI) {
halfPassed = true;
}
} else {
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2) {
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2) {
ori -= 2 * M_PI;
}
}
// 设ti∈[tk, tk+1], relTime = (ti - tk) / (tk+1 - tk), relTime∈[0, 1]，用作LOAM中的运动补偿
float relTime = (ori - startOri) / (endOri - startOri);
// 存储每一个点的所属线号（整数部分）+ scanPeriod * relTime（小数部分）
point.intensity = scanID + scanPeriod * relTime;
// 每个scanID 存储一条scan的所有点
laserCloudScans[scanID].push_back(point);
}
cloudSize = count;
printf("points size %d \n", cloudSize);
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
// 记录每个scan的开始索引和结束索引，+5 和 -6 操作是为了方便后面的曲率计算， laserCloud 使用类似一维数组存储记录所有的点云，具体形式可见下图larseCloud图
for (int i = 0; i < N_SCANS; i++) {
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
printf("prepare time %f \n", t_prepare.toc());
for (int i = 5; i < cloudSize - 5; i++) {
// 对于每个点(开头结束的五个点除外)，使用相邻的五个点值相加减去10倍的中间点的值
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;
cloudLabel[i] = 0;
}
TicToc t_pts;
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
float t_q_sort = 0;
for (int i = 0; i < N_SCANS; i++) {
// 每个scan的点数小于6个，不再遍历
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
for (int j = 0; j < 6; j++) {
// 对每个scan进行6份均等划分
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;
// 对每个均等划分的点云索引按照曲率进行排序，由小到大
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
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) {
// 选择曲率最大的两个点存到cornerPointsSharp中，并且标记为2类点
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20) {
// 选择曲率次大的20个点存到cornerPointsLessSharp中，并标记为1类点
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
} else {
break;
}
// 标记该点已被选中
cloudNeighborPicked[ind] = 1;
// 选中点周围10个曲率连续的点都标记为选中状态
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];
// 选择曲率最小的4个点存储到surfPoints中，并且该类点被标记为-1类点
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1) {
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}
// 该点标记为已选中
cloudNeighborPicked[ind] = 1;
// 该点周围的10个曲率比较连续的点都被标记为已选中
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;
}
}
}
for (int k = sp; k <= ep; k++) {
// 其他所有的 label 为 -1 以及 label 为0的点都为LessFlat点
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
// 调用PCL库对LessFlat的点做下采样
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
printf("sort q time %f \n", t_q_sort);
printf("seperate points time %f \n", t_pts.toc());
// ros类型的所有点云数据(携带intensity信息)打包为RosMsg
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera_init";
pubLaserCloud.publish(laserCloudOutMsg);
// 打包cornerPointsSharp 为RosMsg
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
// 打包cornerPointsLessSharp 为RosMsg
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
// 打包 surfPointsFlat 点云为 RosMsg
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera_init";
pubSurfPointsFlat.publish(surfPointsFlat2);
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera_init";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
// pub each scam
// 最后把输入的原始点云数据也进行打包发布
if(PUB_EACH_LINE)
{
for(int i = 0; i< N_SCANS; i++) {
sensor_msgs::PointCloud2 scanMsg;
pcl::toROSMsg(laserCloudScans[i], scanMsg);
scanMsg.header.stamp = laserCloudMsg->header.stamp;
scanMsg.header.frame_id = "/camera_init";
pubEachScan[i].publish(scanMsg);
}
}
printf("scan registration time %f ms *************\n", t_whole.toc());
if(t_whole.toc() > 100)
ROS_WARN("scan registration process over 100ms");
}这里的激光雷达坐标系为右手系，是x超前，y超左，z超上，即为下图
lidar_cordinate.png
larseCloud 和 laserCloudScan 以及scanStarInd、scanEndInd之间的关系如下所示
larseCloud.png
2.2 位姿估计
基于以上scanRegistration 部分发出的四类点云数据，进行帧与帧之间的匹配，得到帧间的相对位姿，就是odometry轨迹，由于帧与帧之间的匹配获得位姿存在累计误差以及漂移问题，因此后续mapping模块会对该odometry进行精修，得到误差更小的odometry。
2.2.1 帧间匹配
帧间匹配，就是对scanRegistration 部分发出来的点云即面特征和线特征进行匹配的过程，会以第一帧的点云坐标系为World坐标系，后续每一帧都匹配获得在第一帧坐标系下面的位姿，当然通过匹配后计算位姿时需要对点云进行运动补偿(类似于去畸变操作)
2.2.2 点云补偿
由于机械式点云扫描设备扫描获得的每个scan中每个点都是在不同时刻完成的，即雷达扫描的过程中车辆或者机器人也同时在运动，那么同一个scan的中的每一个点都不是在同一个坐标系下面，因此需要将这些点都投影到同一个坐标系下面
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
nh.param<int>("mapping_skip_frame", skipFrameNum, 2);
printf("Mapping %d Hz \n", 10 / skipFrameNum);
// 获取曲率比较大的点，即线特征
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节点发布的点云信息，包括每一帧点云的线特征点、面特征点、以及通过帧间匹配获得的粗略轨迹位姿
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);
nav_msgs::Path laserPath;
int frameCount = 0;
ros::Rate rate(100);
while (ros::ok())
{
ros::spinOnce();
if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
!fullPointsBuf.empty())
{
timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();
timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();
timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();
timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();
if (timeCornerPointsSharp != timeLaserCloudFullRes ||
timeCornerPointsLessSharp != timeLaserCloudFullRes ||
timeSurfPointsFlat != timeLaserCloudFullRes ||
timeSurfPointsLessFlat != timeLaserCloudFullRes)
{
printf("unsync messeage!");
ROS_BREAK();
}
// 这个是由于在laserCloudSharpHandler 函数中需要在cornerPointsSharp中push_back msg,而在这里pop msg，所以需要加锁保护
mBuf.lock();
cornerPointsSharp->clear();
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);
cornerSharpBuf.pop();
cornerPointsLessSharp->clear();
pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);
cornerLessSharpBuf.pop();
surfPointsFlat->clear();
pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);
surfFlatBuf.pop();
surfPointsLessFlat->clear();
pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);
surfLessFlatBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);
fullPointsBuf.pop();
mBuf.unlock();
TicToc t_whole;
// initializing
if (!systemInited)
{
systemInited = true;
std::cout << "Initialization finished \n";
}
else
{
int cornerPointsSharpNum = cornerPointsSharp->points.size();
int surfPointsFlatNum = surfPointsFlat->points.size();
TicToc t_opt;
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
corner_correspondence = 0;
plane_correspondence = 0;
// ceres huber loss 用来减少噪声优化误差
//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);
// 设定帧间位姿变换是优化变量，para_q 是待优化的旋转四元数，para_t 是平移，其中四元数初始化为 [0,0,0,1]
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;
// find correspondence for corner features
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;
// search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
{
// 不寻找同一条scan id的角点
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
continue;
// 不寻找非相邻scan 的角点
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)
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
// search in the direction of decreasing scan line
for (int j = closestPointInd - 1; j >= 0; --j)
{
// 另外一个方向上继续寻找距离最近的角点以及索引
if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
continue;
// 不寻找非相邻的scan id的角点
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)
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
// 如果两个要求的点都找到了
if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
{
// 取当前帧的角点curr_point 和上一帧的两个的角点坐标 last_point_a, last_point_b
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;
// 构建点到直线距离的非线性优化cost function
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);
// kd tree搜索距离最近的点
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0];
// get closest point's scan ID
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
// search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
// 不选择非临近scan的角点
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);
// 在相等scan中寻找距离最近的点
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// 在更高的scan中寻找距离最近的点
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// 遍历更低的scan中点云
for (int j = closestPointInd - 1; j >= 0; --j)
{
// 不选择非临近scan的点云
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);
// 在相同的scan中寻找距离最小的点
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// 在更低scan 中寻找距离最小的点
else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
{
// find nearer point
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++;
}
}
}
//printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence);
printf("data association time %f ms \n", t_data.toc());
if ((corner_correspondence + plane_correspondence) < 10)
{
printf("less correspondence! *************************************************\n");
}
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("solver time %f ms \n", t_solver.toc());
}
printf("optimization twice time %f \n", t_opt.toc());
// 更新平移量和旋转
t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;
}
TicToc t_pub;
// 发布 Odometry 消息
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.child_frame_id = "/laser_odom";
laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserOdometry.pose.pose.orientation.x = q_w_curr.x();
laserOdometry.pose.pose.orientation.y = q_w_curr.y();
laserOdometry.pose.pose.orientation.z = q_w_curr.z();
laserOdometry.pose.pose.orientation.w = q_w_curr.w();
laserOdometry.pose.pose.position.x = t_w_curr.x();
laserOdometry.pose.pose.position.y = t_w_curr.y();
laserOdometry.pose.pose.position.z = t_w_curr.z();
pubLaserOdometry.publish(laserOdometry);
geometry_msgs::PoseStamped laserPose;
laserPose.header = laserOdometry.header;
laserPose.pose = laserOdometry.pose.pose;
laserPath.header.stamp = laserOdometry.header.stamp;
laserPath.poses.push_back(laserPose);
laserPath.header.frame_id = "/camera_init";
pubLaserPath.publish(laserPath);
// transform corner features and plane features to the scan end point
// 保存上一帧的平面和直线点云，注意这里保存的是LessFloat 和 LessSharp点特征
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
// std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n';
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
// 每skipFrameNum帧才发布一帧odometry
if (frameCount % skipFrameNum == 0)
{
frameCount = 0;
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);
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudFullRes3.header.frame_id = "/camera";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
}
printf("publication time %f ms \n", t_pub.toc());
printf("whole laserOdometry time %f ms \n \n", t_whole.toc());
if(t_whole.toc() > 100)
ROS_WARN("odometry process over 100ms");
frameCount++;
}
rate.sleep();
}
return 0;
}其中去畸变函数的原理为:
- 首先一帧点云结束时刻相对于开始时刻的位姿变换(R, T) ^t_{t+1}
- 其次计算出待去畸变的点 i 相对于这个起始点相对变换时间 rel_time
- 然后计算出该点相对于起始时刻的位姿变换矩阵 (R,T)^t_i = (R,T)^t_{t+1} * reltime
- 最后将该点的坐标转移到起始时刻坐标系下面 P_u= (R, T)^t_i * P_i
去畸变的函数为:
// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio
double s;
if (DISTORTION)
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
else
s = 1.0;
//s = 1;
// q_last_curr 和 t_point_last 是全局变量，表示lidar两帧间的位姿变化，在cere位姿优化完毕后会自动更新
// 四元数插值和平移向量分别插值
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;
}
// transform all lidar points to the start of the next frame
void TransformToEnd(PointType const *const pi, PointType *const po)
{
// undistort point first
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();
//Remove distortion time info
po->intensity = int(pi->intensity);
}2.2.3 点线约束
点和直线的距离约束构建，假设当前帧点云为i, 上一帧点云为l 和 j
point_line.png
其中点到直线的距离loss构建如下
struct LidarEdgeFactor
{
LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
Eigen::Vector3d last_point_b_, double s_)
: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix<T, 3, 1> lp;
lp = q_last_curr * cp + t_last_curr;
Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
Eigen::Matrix<T, 3, 1> de = lpa - lpb;
residual[0] = nu.x() / de.norm();
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
const Eigen::Vector3d last_point_b_, const double s_)
{
return (new ceres::AutoDiffCostFunction<
LidarEdgeFactor, 3, 4, 3>(
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};2.2.4 点面约束
点和平面的距离约束构建，建设当前帧点云为i, 上一帧点云为l,j 和 m，那么点到平面的距离的示意图为
point_plane.png
点到平面的距离loss构建如下
struct LidarPlaneFactor
{
LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
last_point_m(last_point_m_), s(s_)
{
ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
ljm_norm.normalize();
}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
//Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};
//Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};
Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};
//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix<T, 3, 1> lp;
lp = q_last_curr * cp + t_last_curr;
residual[0] = (lp - lpj).dot(ljm);
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
const double s_)
{
return (new ceres::AutoDiffCostFunction<
LidarPlaneFactor, 1, 4, 3>(
new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
}
Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
Eigen::Vector3d ljm_norm;
double s;
};2.3 高精度建图
// 接收到所有的次级大线特征点
ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);
// 接收到次级大面特征点
ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);
// 接受到odometry节点发过来的pos
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, laserOdometryHandler);
// 接收到所有点云经过一次降采样的点云
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100, laserCloudFullResHandler);
std::thread mapping_process{process};
}其中process 函数为mapping线程的主函数，mapping线程主要功能是维护点云子地图submap, 主要优化map坐标系和当前帧间的位姿以消除odometry模块由于前后帧匹配获取的位姿的累积误差，odometry模块发送给后端的位姿为T^{odom}_{curr} , mapping 模块估计的位姿为T^{map}_{odom} ，最终的T^{map}_{curr} = T^{map}_{odom} * T^{odom}_{curr}
以下是具体过程，先将线特征点云和面特征点云以及odometry模块估计的位姿取出来，将点云转到map坐标系，
// 将odom坐标系的点云转到map的world坐标系，其中q_wmap_wodom 和 t_wmap_wodom 就是map坐标系要估计的接下来就是维护一个submap，由于内存的限制以及匹配效率的问题，无法将所有的点云拿来做匹配，因此维护一个submap,这里采用栅格地图的办法，submap维护一个21X21X11个栅格，每个栅格都是50米cube,超过这个submap的地图点云将不再匹配。
// 如果当前珊格索引小于3,就说明当前点快接近地图边界了，需要进行调整，相当于地图整体往x正方向移动
// 调整取值范围:3 < centerCubeI < 18， 3 < centerCubeJ < 18, 3 < centerCubeK < 8
// 如果cube处于边界，则将cube向中心靠拢一些，方便后续拓展cube
// laserCloudCornerArray[i] 表示一个cube, 每个cube中维护多个稀疏点云相当于c++的vector接下来首先对点云进行下采样，然后通过kdtree将当前帧点云和submap的各个cube的点云进行匹配,构建loss进行非线性优化，scan to map 的匹配以及求取loss的方法如下：
- 取当前帧的线特征点与面点，通过kdtree在找到全部地图特征点中搜索当前特征点的5个最近邻点。
- 如果是线特征点，则以这五个点的均值点为中心，以5个点的主方向为主方向，作一条直线，令该线点与直线距离最短，构建非线性优化问题。
- 如果是平面点，则寻找五个点的法方向，令这个平面点在法方向上与五个近邻点的距离最小，构建非线性优化问题。
- 优化变量是雷达位姿，求解能够让以上非线性问题代价函数最小的雷达位姿。
原创声明：本文系作者授权腾讯云开发者社区发表，未经许可，不得转载。
如有侵权，请联系 cloudcommunity@tencent.com 删除。
原创声明：本文系作者授权腾讯云开发者社区发表，未经许可，不得转载。
如有侵权，请联系 cloudcommunity@tencent.com 删除。
评论
登录后参与评论
推荐阅读
目录