A-LOAM的源码解析（3）
发布时间：2024年01月03日
这部分主要解析laserMapping.cpp，也就是mapping部分。
一、主函数
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
float lineRes = 0;
float planeRes = 0;
//设置体素滤波参数
nh.param<float>("mapping_line_resolution", lineRes, 0.4);
nh.param<float>("mapping_plane_resolution", planeRes, 0.8);
printf("line resolution %f plane resolution %f \n", lineRes, planeRes);
downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);
downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);
//订阅角点，面点，位姿和点云，并且接收时调用回调函数
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);
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);
//发布
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 100);
pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 100);
pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 100);
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100);
pubOdomAftMappedHighFrec = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 100);
pubLaserAfterMappedPath = nh.advertise<nav_msgs::Path>("/aft_mapped_path", 100);
//为智能指针分配内存
for (int i = 0; i < laserCloudNum; i++)
{
laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());
laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());
}
//创建了一个新的线程，这个线程将执行 process 函数
std::thread mapping_process{process};
ros::spin();
return 0;
}1.1主要流程
以下这段代码主要的流程， laserMapping.cpp主要的内容都在process()函数里
初始化 ROS 节点：
- 使用
ros::init(argc, argv, "laserMapping")初始化 ROS 节点，节点名称为 "laserMapping"。- 创建一个
ros::NodeHandle对象nh，用于处理节点的通信功能。设置体素滤波参数：
- 从 ROS 参数服务器读取两种分辨率参数：
mapping_line_resolution和mapping_plane_resolution，并为它们提供默认值（0.4 和 0.8）。- 使用这些参数设置两个体素滤波器
downSizeFilterCorner和downSizeFilterSurf，以减小点云数据的体积和复杂度。订阅消息：
- 订阅不同类型的 ROS 消息，包括角点云、面点云、激光里程计和完整分辨率点云。
- 每种消息类型都有相应的回调函数处理接收到的数据。
发布消息：
- 设置几个 ROS 主题发布器，用于发布处理后的点云、里程计数据和路径信息。
内存分配：
- 为
laserCloudCornerArray和laserCloudSurfArray分配内存，这些数组用于存储点云数据。多线程处理：
- 创建一个新线程来运行
process函数，该函数负责地图构建的主要处理逻辑。ROS 循环：
- 调用
ros::spin()进入主循环，等待并处理回调函数，直到节点被关闭。
1.2回调函数
//向队列里添加点云信息，加锁
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
{
mBuf.lock();
cornerLastBuf.push(laserCloudCornerLast2);
mBuf.unlock();
}
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2)
{
mBuf.lock();
surfLastBuf.push(laserCloudSurfLast2);
mBuf.unlock();
}
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
mBuf.lock();
fullResBuf.push(laserCloudFullRes2);
mBuf.unlock();
}
//receive odomtry
//接受里程计，把位姿转换为地图位姿
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{
mBuf.lock();
odometryBuf.push(laserOdometry);
mBuf.unlock();
// high frequence publish
//ROS-》eigen
Eigen::Quaterniond q_wodom_curr;
Eigen::Vector3d t_wodom_curr;
q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
t_wodom_curr.z() = laserOdometry->pose.pose.position.z;
Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
//eigen->ros
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = laserOdometry->header.stamp;
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMappedHighFrec.publish(odomAftMapped);
}前三个回调函数就是简单的向BUFFER中加入点云信息 ，laserOdometryHandler()接受里程计，首先将ros格式的消息转换成eigen，然后通过T_wmap_wodom把里程计位姿转换为地图位姿，最后转换成ros格式发布。
这里的T表示四元数q和相对平移t，下文亦如此。
二、主处理线程process()
2.1数据预处理部分
void process()
{
while(1)
{
//确保buffer里都有值
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{
mBuf.lock();
//删掉小于时间戳cornerLastBuf的，对齐时间戳
while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
odometryBuf.pop();
if (odometryBuf.empty())
{
mBuf.unlock();
break;
}
while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
surfLastBuf.pop();
if (surfLastBuf.empty())
{
mBuf.unlock();
break;
}
while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
fullResBuf.pop();
if (fullResBuf.empty())
{
mBuf.unlock();
break;
}
//取出时间戳
timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();
timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();
timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
//如果时间戳对不齐，打印消息
if (timeLaserCloudCornerLast != timeLaserOdometry ||
timeLaserCloudSurfLast != timeLaserOdometry ||
timeLaserCloudFullRes != timeLaserOdometry)
{
printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);
printf("unsync messeage!");
mBuf.unlock();
break;
}
//ros-》pcl
laserCloudCornerLast->clear();
pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);
cornerLastBuf.pop();
laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
surfLastBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);
fullResBuf.pop();
//ros->eigen
q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();
//为了实时性，清空cornerLastBuf
while(!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}
mBuf.unlock();这部分函数的主要作用如下：
确保数据缓冲区不为空：
- 函数首先检查四个数据缓冲区（角点、面点、完整分辨率点云、里程计数据）是否都非空，这是后续处理的前提条件。
时间戳对齐：
- 通过比较不同类型数据的时间戳，确保它们是同步的。这是数据融合过程中常见的步骤，确保所有数据都是来自相同的时间点。
- 如果发现时间戳不匹配，会弹出（丢弃）那些时间戳较早的数据，直到时间戳相同。
数据转换：
- 从 ROS 的消息格式转换为 PCL（Point Cloud Library）的点云格式，以便进行进一步处理。
- 同时，将里程计的姿态和位置数据从 ROS 消息转换为 Eigen 类型，以方便进行数学计算。
清空缓冲区：
- 为了保持实时性，清空
cornerLastBuf缓冲区中的剩余数据，并输出提示信息。线程安全：
- 使用互斥锁
mBuf来保证多线程环境下数据的一致性和线程安全。
2.2坐标转换
TicToc t_whole;
//给后段一个初始估计，如果有Imu，这个函数可以使用imu
transformAssociateToMap();
TicToc t_shift;// set initial guess
//里程计位姿转化为地图位姿
void transformAssociateToMap()
{
q_w_curr = q_wmap_wodom * q_wodom_curr;
t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}T_wodom_curr是里程计的位姿，是里程计线程传输进来的，T_wmap_wodom是两者的相对位姿变换，初值是单位矩阵和0位移，之后会在函数末尾进行不断更新。T_w_curr则代表此雷达帧在地图中的位姿
2.3建立和调整珊格地图
//设置21*21*11个边长为50m的珊格
//形成一个以当前点为中心的珊格地图
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;
//避免负值向上取整
if (t_w_curr.x() + 25.0 < 0)
centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
centerCubeK--;
//如果索引小于3，接近边界，对珊格进行调整
while (centerCubeI < 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
//从x最大值20开始
int i = laserCloudWidth - 1;
//使用一个数组涵盖所有点
//取出角点数组最大的指针
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
//取出面点数组最大的指针
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
//整体向右移动
for (; i >= 1; i--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
//i=0,laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] 代表最左边的格子，赋值之前最右侧的值
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
//指针操作，最左边格子清空了
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
//索引向右移动
centerCubeI++;
laserCloudCenWidth++;
}
//如果到达右边界，则整体左移
while (centerCubeI >= laserCloudWidth - 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
//左移
for (; i < laserCloudWidth - 1; i++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI--;
laserCloudCenWidth--;
}
//同理x
while (centerCubeJ < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = laserCloudHeight - 1;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
//整体上移
for (; j >= 1; j--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ++;
laserCloudCenHeight++;
}
//同理
while (centerCubeJ >= laserCloudHeight - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j < laserCloudHeight - 1; j++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ--;
laserCloudCenHeight--;
}
while (centerCubeK < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = laserCloudDepth - 1;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k >= 1; k--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK++;
laserCloudCenDepth++;
}
while (centerCubeK >= laserCloudDepth - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k < laserCloudDepth - 1; k++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK--;
laserCloudCenDepth--;
}这里设置了一个以当前点为中心的，21*21*11的，每格边长为50m的珊格地图，在当前点偏离中心过多后对珊格位置进行调整。依次处理XYZ方向，代码雷同度很高只注释了X方向，这里珊格上的所有点都是按照珊格位置的索引放在角点和面点两个个一维数组里的，laserCloudCornerArray和laserCloudSurfArray，里面存放的是指针，数目就是4851，珊格的总数量
?
?2.4创建局部优化地图
//维护里一个局部地图，保证当前帧不再这个局部地图的边缘，这样才可以获取足够的约束
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
//从当前格子为中心，选5*5*3=75珊格点
//即250m*250m*150m的约束范围
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{
for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
{
for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
{
//确保没有越界
if (i >= 0 && i < laserCloudWidth &&
j >= 0 && j < laserCloudHeight &&
k >= 0 && k < laserCloudDepth)
{
//记录索引
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
//开始构建用于这一帧优化的局部地图
for (int i = 0; i < laserCloudValidNum; i++)
{
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}
int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();根据当前机器人的位置选择周围的立方体网格（5x5x3），构建局部地图。?
?2.5创建线特征和面特征约束，进行CERES优化
//当前帧角点和面点点云下采样，体素滤波
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerStack);
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfStack);
int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
printf("map prepare time %f ms\n", t_shift.toc());
printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);
//局部地图内保证角点大于10且面点大于50，否则输出警告信息
if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
{
TicToc t_opt;
TicToc t_tree;
//局部地图的角点和面点送入KDTREE
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
printf("build tree time %f ms \n", t_tree.toc());
//迭代2次
for (int iterCount = 0; iterCount < 2; iterCount++)
{
//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);
//使用自定义四元素加法添加参数块，parameters是指针，前四位是旋转，后三位平移
problem.AddParameterBlock(parameters, 4, q_parameterization);
problem.AddParameterBlock(parameters + 4, 3);
TicToc t_data;
int corner_num = 0;
//构建角点约束
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
pointOri = laserCloudCornerStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
//把当前点投到地图坐标系
pointAssociateToMap(&pointOri, &pointSel);
//寻找最近的5个点，id记录到pointSearchInd，距离记录到pointSearchSqDis
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
//如果最远的小于1m
if (pointSearchSqDis[4] < 1.0)
{
std::vector<Eigen::Vector3d> nearCorners;
Eigen::Vector3d center(0, 0, 0);
//作加法
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
nearCorners.push_back(tmp);
}
//取均值
center = center / 5.0;
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
//构建协方差矩阵
for (int j = 0; j < 5; j++)
{
Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
//特征值分解
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
// if is indeed line feature
// note Eigen library sort eigenvalues in increasing order
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
//最大特征值大于3倍次大特征值
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
//中心点加减上0.1倍的线特征，作为两个端点，类似于前端
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
//构建约束，和前段COST一致，迭代四次
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
corner_num++;
}
}
}
int surf_num = 0;
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
//同上
pointOri = laserCloudSurfStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
//构建超定方程
Eigen::Matrix<double, 5, 3> matA0;
//-1行向量
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
//如果最远的距离小于1
if (pointSearchSqDis[4] < 1.0)
{
//构建5*3矩阵
for (int j = 0; j < 5; j++)
{
matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
//printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));
}
// find the norm of plane
//调用eigen接口，求解平面法向量
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
//求模
double negative_OA_dot_norm = 1 / norm.norm();
//归一化
norm.normalize();
// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
//判断每一个点到平面的距离是否足够小，即大于0.2m，break掉，置为false
for (int j = 0; j < 5; j++)
{
// if OX * n > 0.2, then plane is not fit well
if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
{
planeValid = false;
break;
}
}
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
//如果有效，加到CERES里优化
if (planeValid)
{
ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
surf_num++;
}
}
}
//printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);
//printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);
printf("mapping data assosiation time %f ms \n", t_data.toc());
//调用CERES求解
TicToc t_solver;
ceres::Solver::Options options;
//稠密
options.linear_solver_type = ceres::DENSE_QR;
//4次迭代
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("mapping solver time %f ms \n", t_solver.toc());
//printf("time %f \n", timeLaserOdometry);
//printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);
//printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],
// parameters[4], parameters[5], parameters[6]);
}
printf("mapping optimization time %f \n", t_opt.toc());
}
else
{
ROS_WARN("time Map corner and surf num are not enough");
}主要流程如下：
点云下采样：使用体素滤波器对当前帧的角点和面点点云进行下采样，减少计算量。
建立KD树：为局部地图的角点和面点创建KD树，以便快速寻找最近的点。
角点处理：
- 对于每个角点，找到最近的5个点。
- 如果满足条件（例如距离小于1米），则使用这些点构建协方差矩阵，并计算其特征值和特征向量。
- 使用最大特征值对应的特征向量作为线的方向，构建角点约束。
面点处理：
- 对于每个面点，同样找到最近的5个点。
- 如果满足条件，通过这5个点求解平面的法向量。
- 对于有效的平面，添加面点约束。
优化：在每次迭代中，使用Ceres Solver对角点和面点的约束进行优化，调整机器人的位姿。
核函数：使用Huber核函数来减少离群点的影响。
参数块：设置四元数和平移的参数块，并指定局部参数化（四元数的参数化）。
?首先以当前点为中心建立了一个5*5*3即75个珊格的局部地图，然后检验centerCube，确保其没有越界，将局部地图的角点，面点取出。
对当前帧的点和面点点云下采样，进行体素滤波，具体参数在主函数里设置好了。
当前雷达帧在局部地图内保证角点大于10且面点大于50，否则输出警告信息。
将局部地图的角点和面点送入KDTREE，进入优化迭代，优化目的是求出准确的地图坐标。
对角点和面点优化是，简单的点到线、面距离数学问题，原理参考：SLAM前端入门框架-A_LOAM源码解析 - 知乎个人看完A_LOAM的唯一感觉就是，应该是比较好的上手框架了，虽然说在此框架诞生的时候并没有非常好的考虑设计，但是省略了LOAM中作者手动推倒雅可比的那部分，整体结构也是十分清楚，是非常适合一般初学者进入这个…
https://zhuanlan.zhihu.com/p/400014744使用ceres进行优化，这里用点到直线的距离举例：
2.4.1 struct LidarEdgeFactor
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
{
//写成模板的形式，类似于vector，定义当前点和两个端点
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;
};给出残差定义方式即可，最后的residual的模即是点到直线的距离。
2.4.2优化的几个核心点
Ceres Solver 通过调整位姿参数来最小化所有角点和面点到对应线特征的距离总和。
Huber Loss 函数被用作损失函数，减小外点的影响。
优化的对象是四元数和平移double parameters[7] = {0, 0, 0, 1, 0, 0, 0};也就是当前帧在世界坐标系下的位姿
通过 Eigen::Map 对象修改数据时，实际上是在直接修改原始数据数组。所以第一次优化后，位姿发生改变
//帧坐标系转化到map坐标系
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
po->x = point_w.x();
po->y = point_w.y();
po->z = point_w.z();
po->intensity = pi->intensity;
//po->intensity = 1.0;
}本身的位置一定发生变化，最近的5个点也可能发生变化，然后进行第二次优化，得到更准确的T_w_curr
2.6 更新T_wmap_wodom
//T_w_curr经过优化后更新T_wmap_wodom
void transformUpdate()
{
q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}我们得到了准确的T_w_curr，更新里程计坐标系和地图坐标系的相对位姿T_wmap_wodom
2.7将本帧的角点面点存入珊格地图
//将优化后的当前帧角点加到局部地图中
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
//下采样后的点投到地图坐标系
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
//求出此点的索引
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
//求出数列中的索引
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}
//同上
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudSurfArray[cubeInd]->push_back(pointSel);
}
}
printf("add points time %f ms\n", t_add.toc());
?2.8下采样发布点云
for (int i = 0; i < laserCloudValidNum; i++)
{
int ind = laserCloudValidInd[i];
pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
downSizeFilterCorner.filter(*tmpCorner);
laserCloudCornerArray[ind] = tmpCorner;
pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
downSizeFilterSurf.filter(*tmpSurf);
laserCloudSurfArray[ind] = tmpSurf;
}
//publish surround map for every 5 frame
//每5帧发布
if (frameCount % 5 == 0)
{
laserCloudSurround->clear();
//把当前帧局部地图发布（包括角点和面点）
for (int i = 0; i < laserCloudSurroundNum; i++)
{
int ind = laserCloudSurroundInd[i];
*laserCloudSurround += *laserCloudCornerArray[ind];
*laserCloudSurround += *laserCloudSurfArray[ind];
}
//以ros格式发布给RVIZ
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "/camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);
}
//每20帧发布
if (frameCount % 20 == 0)
{
//所有的点
pcl::PointCloud<PointType> laserCloudMap;
for (int i = 0; i < 4851; i++)
{
laserCloudMap += *laserCloudCornerArray[i];
laserCloudMap += *laserCloudSurfArray[i];
}
sensor_msgs::PointCloud2 laserCloudMsg;
pcl::toROSMsg(laserCloudMap, laserCloudMsg);
laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudMsg.header.frame_id = "/camera_init";
pubLaserCloudMap.publish(laserCloudMsg);
}
//发布当前帧
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3.header.frame_id = "/camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
printf("mapping pub time %f ms \n", t_pub.toc());
printf("whole mapping time %f ms +++++\n", t_whole.toc());
//发布当前位姿
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
//轨迹
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header = odomAftMapped.header;
laserAfterMappedPose.pose = odomAftMapped.pose.pose;
laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id = "/camera_init";
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);
//tf
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(t_w_curr(0),
t_w_curr(1),
t_w_curr(2)));
q.setW(q_w_curr.w());
q.setX(q_w_curr.x());
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));
frameCount++;
}
//通常用于减少 CPU 使用率，通过在循环中插入短暂的休眠来防止过度占用 CPU 时间。
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
}至此，结束！我们再看一下线程之间的传递
?
文章来源:https://blog.csdn.net/weixin_44760904/article/details/135328765
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：chenni525@qq.com进行投诉反馈，一经查实，立即删除！
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：chenni525@qq.com进行投诉反馈，一经查实，立即删除！
最新文章
- Python教程
- 深入理解 MySQL 中的 HAVING 关键字和聚合函数
- Qt之QChar编码（1）
- MyBatis入门基础篇
- 用Python脚本实现FFmpeg批量转换
- 【蓝桥杯选拔赛真题85】Scratch不重复的子串 第十五届蓝桥杯scratch图形化编程 少儿编程创意编程选拔赛真题解析
- python基数排序
- RHCE chrony服务器
- 力扣 面试经典150算法题
- 堆能高效解决的经典问题（算法村第十四关白银挑战）
- 【LeetCode刷题笔记】动态规划（四）
- ros2 humble安装joint_state_publisher功能包
- 第四章 处理机调度【操作系统】
- nginx的location与rewrite
- 分库分表之Mycat应用学习三