里程计,是通过累计帧间位姿变换得来的,因此会累积帧间误差。
如果想要纠正此累积误差,我们需要通过另一种方法得到可信位姿,以此校正相同时刻里程计位姿。
SLAM图优化,是一种记录各帧时刻里程计,并通过可信位姿校正累积误差的工具。
闭环检测,利用“故地重游”的思想,通过当前帧与历史帧的配准(不是相邻时间帧),提供另一种位姿变换的关系,用来校正闭环内的累积误差。
mapOptmization.cpp
代码注释较详细,如下所示。
/**
* Created by haocaichao on 2021/8/29
*
*里程计图优化
* (1)接收特征点云和待优化的里程计
* (2)关键帧判断与信息记录(关键位置、位姿、轨迹、帧数据)
* (3)添加里程计因子
* (4)添加闭环因子(闭环帧判断、闭环局部地图提取、闭环配准)
* (5)里程计图优化与历史更新
* (6)发布优化校正结果
*/
#include "header.h"
typedef PointXYZIRPYT PointTypePose6D; //x,y,z,roll,pitch,yaw
typedef pcl::PointXYZI PointTypePose3D; //x,y,z
typedef pcl::PointXYZI PointType; //x,y,z 同上
ros::Subscriber sub_plane_frame_cloud; //接收平面特征点云
ros::Subscriber sub_frame_Odometry; //接收激光里程计
ros::Publisher pub_sum_map_cloud; //发布里程计优化后的点云地图
ros::Publisher pub_map_frame; //发布里程计优化后的当前帧(坐标转换后)
ros::Publisher pub_map_odometry; //发布优化后的里程计
ros::Publisher pub_map_path; //发布优化后的轨迹
nav_msgs::Path globalPath;
pcl::PointCloud<PointType>::Ptr currFramePlanePtr(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr mapCloudPtr(new pcl::PointCloud<PointType>());
//关键帧空间位置点云对象(将各关键位置点存储在一个点云对象中,方便位置查找)
pcl::PointCloud<PointTypePose3D>::Ptr keyPose3DCloud(new pcl::PointCloud<PointTypePose3D>());
//关键帧位姿点云对象(包含位置和方向)
pcl::PointCloud<PointTypePose6D>::Ptr keyPose6DCloud(new pcl::PointCloud<PointTypePose6D>());
//关键帧数据数组
std::vector<pcl::PointCloud<PointType>> keyFrameVector;
//关键帧位置查找树kdtree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses(new pcl::KdTreeFLANN<PointType>());
pcl::VoxelGrid<PointType> downSizeFilterICP;
pcl::VoxelGrid<PointType> downSizeFilterMap;
std_msgs::Header currHead;
std::queue<sensor_msgs::PointCloud2> planeQueue; //定义点云消息队列
std::queue<nav_msgs::Odometry> odometryQueue; //定义里程计消息队列
std::mutex mLock;
double timePlane=0;
double timeOdom=0;
double arr6d[6]={0,0,0,0,0,0}; //数组存储6D位姿信息(x,y,z,roll,pitch,yaw)
Eigen::Quaterniond q_fodom_0_curr(1,0,0,0);
Eigen::Vector3d t_fodom_0_curr(0,0,0);
Eigen::Affine3d T_fodom_0_curr=Eigen::Affine3d::Identity(); //变换,帧帧里程计中,当前帧到起点
Eigen::Affine3d T_map_0_curr=Eigen::Affine3d::Identity(); //变换,优化里程计中,当前帧到起点
Eigen::Affine3d trans_map_0_curr_i=Eigen::Affine3d::Identity(); //变换,优化处理后,当前帧到起点
Eigen::Affine3d trans_loop_adjust=Eigen::Affine3d::Identity(); //变换,累计闭环校正量,当前帧到闭环帧
int isDone=1;
int numFrame=0;
bool aLoopIsClosed = false; //是否闭环成功
size_t loopRecordIndex=0;
Eigen::Affine3d correctionLidarFrame; //变换,本次闭环优化校正量,当前帧到闭环帧
std::map<int, int> loopIndexContainer;
gtsam::NonlinearFactorGraph gtSAMgraph; //定义图优化对象
gtsam::Values initialEstimate;
gtsam::ISAM2 *isam;
gtsam::Values isamCurrentEstimate; //图优化结果
gtsam::ISAM2Params parameters;
void planeCloudHandler(const sensor_msgs::PointCloud2ConstPtr &planeCloudMsg) {
mLock.lock();
planeQueue.push(*planeCloudMsg);
mLock.unlock();
}
void odomHandler(const nav_msgs::Odometry::ConstPtr &odomMsg){
mLock.lock();
odometryQueue.push(*odomMsg);
mLock.unlock();
}
//将6D位姿信息与时间戳存入位置对象、位姿对象、轨迹对象
void addPose3D6D(double x,double y,double z,double roll,double pitch,double yaw,double time){
PointTypePose3D p3d;
PointTypePose6D p6d;
p3d.x=x;
p3d.y=y;
p3d.z=z;
p3d.intensity=keyPose3DCloud->size(); // index ,now size==0, before push_back
p6d.x=p3d.x;
p6d.y=p3d.y;
p6d.z=p3d.z;
p6d.intensity=p3d.intensity;
p6d.roll=roll;
p6d.pitch=pitch;
p6d.yaw=yaw;
p6d.time=time;
keyPose3DCloud->push_back(p3d);
keyPose6DCloud->push_back(p6d);
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time().fromSec(time);
pose_stamped.header.frame_id = "map";
pose_stamped.pose.position.x = x;
pose_stamped.pose.position.y = y;
pose_stamped.pose.position.z = z;
tf::Quaternion q = tf::createQuaternionFromRPY(roll, pitch, yaw);
pose_stamped.pose.orientation.x = q.x();
pose_stamped.pose.orientation.y = q.y();
pose_stamped.pose.orientation.z = q.z();
pose_stamped.pose.orientation.w = q.w();
globalPath.poses.push_back(pose_stamped);
}
//更新里程计轨迹
void updatePath(const PointTypePose6D& pose_in,int indx)
{
globalPath.poses[indx].pose.position.x = pose_in.x;
globalPath.poses[indx].pose.position.y = pose_in.y;
globalPath.poses[indx].pose.position.z = pose_in.z;
tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
globalPath.poses[indx].pose.orientation.x = q.x();
globalPath.poses[indx].pose.orientation.y = q.y();
globalPath.poses[indx].pose.orientation.z = q.z();
globalPath.poses[indx].pose.orientation.w = q.w();
}
//计算当前帧与前一帧位姿变换,如果变化太小,不设为关键帧,反之设为关键帧
bool isKeyFrame()
{
if (keyPose3DCloud->points.empty())
return true;
PointTypePose6D thisPoint=keyPose6DCloud->back(); // 前一帧位姿
Eigen::Affine3d T_key_0_last;
pcl::getTransformation(thisPoint.x,thisPoint.y,thisPoint.z,thisPoint.roll,thisPoint.pitch,thisPoint.yaw,T_key_0_last);
//当前帧与前一帧位姿变换增量
Eigen::Affine3d transBetween = T_key_0_last.inverse() * T_map_0_curr;
double x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);
// 旋转和平移量都较小,当前帧不设为关键帧
if (abs(roll) < 0.01 && abs(pitch) < 0.01 && abs(yaw) < 0.01 &&
sqrt(x*x + y*y + z*z) < 1)
return false;
return true;
}
//添加激光里程计因子
void addOdomFactor()
{
gtsam::Pose3 currPose(gtsam::Rot3::RzRyRx(arr6d[3], arr6d[4], arr6d[5]),gtsam::Point3(arr6d[0], arr6d[1], arr6d[2]));
if (keyPose3DCloud->points.size()==1){
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
gtSAMgraph.add(gtsam::PriorFactor<gtsam::Pose3>(0, currPose, priorNoise)); // 第一帧初始化先验因子
initialEstimate.insert(0, currPose); // 变量节点设置初始值
}
if (keyPose3DCloud->points.size()>1)
{
PointTypePose6D thisPoint=keyPose6DCloud->points[keyPose3DCloud->points.size()-2];
gtsam::Pose3 lastPost(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
gtsam::noiseModel::Diagonal::shared_ptr odometryNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
// 添加激光里程计因子 参数:前一帧id,当前帧id,前一帧与当前帧的位姿变换(作为观测值),噪声协方差
gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(keyPose3DCloud->size()-2, keyPose3DCloud->size()-1, lastPost.between(currPose), odometryNoise));
initialEstimate.insert(keyPose3DCloud->size()-1, currPose); // 变量节点设置初始值
}
}
// 闭环检测,找出闭环关键帧序号用于后续闭环判断;候选帧选择依据是此帧距离最近,时间久远超过阈值
bool detectLoopFrameID(int *latestID, int *closestID)
{
int loopKeyCur = - 1; // 当前关键帧序号
loopKeyCur = keyPose3DCloud->size() - 1; // 当前关键帧序号
int loopKeyPre = -1;
// 当前帧已经添加过闭环对应关系,不再继续添加
auto it = loopIndexContainer.find(loopKeyCur);
if (it != loopIndexContainer.end())
return false;
// 在历史关键帧中查找与当前关键帧距离最近的关键帧集合
std::vector<int> pointSearchIndLoop;
std::vector<float> pointSearchSqDisLoop;
kdtreeHistoryKeyPoses->setInputCloud(keyPose3DCloud);
kdtreeHistoryKeyPoses->radiusSearch(keyPose3DCloud->back(), 15, pointSearchIndLoop, pointSearchSqDisLoop, 0);
// 在候选关键帧集合中,找到与当前帧时间相隔较远的帧,设为候选匹配帧
for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
{
int id = pointSearchIndLoop[i];
if (abs(keyPose6DCloud->points[id].time - keyPose6DCloud->points[loopKeyCur].time) > 20)
{
loopKeyPre = id;
break;
}
}
if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
return false;
*latestID = loopKeyCur;
*closestID = loopKeyPre;
loopRecordIndex=loopKeyCur+2; //跳过2帧
return true;
}
// 提取闭环关键帧周围的点云组成局部地图
void getLoopLocalMap(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
{
nearKeyframes->clear();
int cloudSize = keyPose6DCloud->size();
for (int i = -searchNum; i <= searchNum; ++i)
{
int keyNear = key + i;
if (keyNear < 0 || keyNear >= cloudSize )
continue;
Eigen::Affine3d trans;
PointTypePose6D pt6d=keyPose6DCloud->points[keyNear];
pcl::getTransformation(pt6d.x, pt6d.y, pt6d.z, pt6d.roll, pt6d.pitch, pt6d.yaw,trans);
pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(keyFrameVector[keyNear], *cloud_temp, trans);
*nearKeyframes += *cloud_temp;
}
if (nearKeyframes->empty())
return;
pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
downSizeFilterICP.setInputCloud(nearKeyframes);
downSizeFilterICP.filter(*cloud_temp);
*nearKeyframes = *cloud_temp;
}
//闭环判断与闭环因子的添加
void addLoopFactor(){
if (keyPose3DCloud->points.size()<5 || keyPose3DCloud->points.size()-1<=loopRecordIndex)
return;
int loopKeyCur;
int loopKeyPre;
if(detectLoopFrameID(&loopKeyCur, &loopKeyPre)){
pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
{
getLoopLocalMap(cureKeyframeCloud, loopKeyCur, 0); // 提取当前关键帧
getLoopLocalMap(prevKeyframeCloud, loopKeyPre, 10); // 提取闭环匹配关键帧前后相邻若干帧
if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
return;
}
static pcl::IterativeClosestPoint<PointType, PointType> icp; // ICP参数设置
icp.setMaxCorrespondenceDistance(50);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
icp.setInputSource(cureKeyframeCloud);
icp.setInputTarget(prevKeyframeCloud);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result);
if (icp.hasConverged() == false || icp.getFitnessScore() > 0.2)// 未收敛,或者匹配不够好
return;
// 闭环优化得到的当前关键帧与闭环关键帧之间的位姿变换
correctionLidarFrame=icp.getFinalTransformation().cast<double>();
loopRecordIndex=loopRecordIndex+30; //闭环成功后跳过30帧再判断闭环
// 闭环优化前当前帧位姿
Eigen::Affine3d tWrong;
PointTypePose6D pt6d=keyPose6DCloud->points[loopKeyCur];
pcl::getTransformation(pt6d.x, pt6d.y, pt6d.z, pt6d.roll, pt6d.pitch, pt6d.yaw,tWrong);
// 闭环优化后当前帧位姿
Eigen::Affine3d tCorrect = correctionLidarFrame * tWrong;
double x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
gtsam::Pose3 poseFrom = gtsam::Pose3(gtsam::Rot3::RzRyRx(roll, pitch, yaw), gtsam::Point3(x, y, z));
// 闭环匹配帧的位姿
PointTypePose6D thisPoint=keyPose6DCloud->points[loopKeyPre];
gtsam::Pose3 poseTo(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
gtsam::noiseModel::Diagonal::shared_ptr constraintNoise = gtsam::noiseModel::Diagonal::Variances(Vector6);
loopIndexContainer[loopKeyCur] = loopKeyPre;
gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(loopKeyCur, loopKeyPre, poseFrom.between(poseTo), constraintNoise));
aLoopIsClosed = true;
}
}
//执行图优化
void runOptimize(){
isam->update(gtSAMgraph, initialEstimate); // 执行优化
isam->update();
if (aLoopIsClosed == true)
{
for (int j = 0; j < 6; ++j) {
isam->update();
}
}
// update之后要清空一下保存的因子图,注:历史数据不会清掉,ISAM保存起来了
gtSAMgraph.resize(0);
initialEstimate.clear();
isamCurrentEstimate = isam->calculateEstimate(); // 优化结果
}
//更新关键位姿信息(位置、位姿、轨迹、点云变换与拼接)
void updateKeyPose(int i){
keyPose3DCloud->points[i].x = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().x();
keyPose3DCloud->points[i].y = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().y();
keyPose3DCloud->points[i].z = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().z();
keyPose6DCloud->points[i].x = keyPose3DCloud->points[i].x;
keyPose6DCloud->points[i].y = keyPose3DCloud->points[i].y;
keyPose6DCloud->points[i].z = keyPose3DCloud->points[i].z;
keyPose6DCloud->points[i].roll = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().roll();
keyPose6DCloud->points[i].pitch = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().pitch();
keyPose6DCloud->points[i].yaw = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().yaw();
updatePath(keyPose6DCloud->points[i],i); // 更新里程计轨迹
pcl::getTransformation(keyPose6DCloud->points[i].x, keyPose6DCloud->points[i].y, keyPose6DCloud->points[i].z,
keyPose6DCloud->points[i].roll, keyPose6DCloud->points[i].pitch,keyPose6DCloud->points[i].yaw,trans_map_0_curr_i);
pcl::PointCloud<PointType>::Ptr cloud_res_i(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(keyFrameVector[i], *cloud_res_i, trans_map_0_curr_i);
*mapCloudPtr += *cloud_res_i;
}
// 位姿校正,更新因子图中所有变量节点的位姿,也就是所有历史关键帧的位姿,更新里程计轨迹
void correctPoses()
{
int numPoses = isamCurrentEstimate.size();
if (aLoopIsClosed == true)
{
mapCloudPtr->clear();
// 更新因子图中所有变量节点的位姿,即所有历史关键帧的位姿
for (int i = 0; i < numPoses; ++i)
{
updateKeyPose(i);
}
aLoopIsClosed = false;
trans_loop_adjust=trans_loop_adjust*correctionLidarFrame;
}else{
updateKeyPose(numPoses-1);
}
T_map_0_curr=trans_map_0_curr_i;
}
//发布优化校正结果
void publishResult(){
//获取优化后的当前帧里程计(坐标变换)
Eigen::Quaterniond eq;
eq=T_map_0_curr.rotation();
Eigen::Vector3d ev=T_map_0_curr.translation();
//发布优化后的里程计
nav_msgs::Odometry lidarOdometry;
lidarOdometry.header.frame_id = "map";
lidarOdometry.child_frame_id = "map_child";
lidarOdometry.header.stamp = currHead.stamp;
lidarOdometry.pose.pose.orientation.x = eq.x();
lidarOdometry.pose.pose.orientation.y = eq.y();
lidarOdometry.pose.pose.orientation.z = eq.z();
lidarOdometry.pose.pose.orientation.w = eq.w();
lidarOdometry.pose.pose.position.x = ev.x();
lidarOdometry.pose.pose.position.y = ev.y();
lidarOdometry.pose.pose.position.z = ev.z();
pub_map_odometry.publish(lidarOdometry);
//发布优化后的轨迹
globalPath.header.stamp=lidarOdometry.header.stamp;
globalPath.header.frame_id="map";
pub_map_path.publish(globalPath);
//发布优化后的当前帧点云
pcl::PointCloud<PointType>::Ptr cloud_res(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*currFramePlanePtr, *cloud_res, T_map_0_curr);
sensor_msgs::PointCloud2 res_cloud_msgs;
pcl::toROSMsg(*cloud_res, res_cloud_msgs);
res_cloud_msgs.header.stamp = lidarOdometry.header.stamp;
res_cloud_msgs.header.frame_id = "map";
pub_map_frame.publish(res_cloud_msgs);
//发布优化后的点云拼接地图
if(numFrame % 5 == 0){ //每隔5帧发布一次
pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
downSizeFilterMap.setInputCloud(mapCloudPtr);
downSizeFilterMap.filter(*cloud_temp);
sensor_msgs::PointCloud2 res_map_cloud_msgs;
pcl::toROSMsg(*cloud_temp, res_map_cloud_msgs);
res_map_cloud_msgs.header.stamp = currHead.stamp;
res_map_cloud_msgs.header.frame_id = "map";
pub_sum_map_cloud.publish(res_map_cloud_msgs);
}
}
//图优化过程管理
void mapOptimization(){
if(isKeyFrame()){ //判断是否作为关键帧(如果原地变化很小,则不计入关键帧,相当于帧过滤)
// get x, y, z, roll, pitch, yaw
pcl::getTranslationAndEulerAngles(T_map_0_curr,arr6d[0],arr6d[1],arr6d[2],arr6d[3],arr6d[4],arr6d[5]);
addPose3D6D(arr6d[0],arr6d[1],arr6d[2],arr6d[3],arr6d[4],arr6d[5],timeOdom);
keyFrameVector.push_back(*currFramePlanePtr); // save key frame
addOdomFactor(); // 激光里程计因子
addLoopFactor(); // 闭环因子
runOptimize(); // 执行优化
correctPoses(); // 更新里程计
publishResult();
}
}
//点云处理多线程函数
void cloudThread(){
ros::Rate rate(20);
while(ros::ok()){
rate.sleep();
if(isDone==0) continue;
while (!odometryQueue.empty() && !planeQueue.empty()){
if(isDone==0) continue;
mLock.lock();
currHead.stamp=planeQueue.front().header.stamp;
timePlane=planeQueue.front().header.stamp.toSec();
timeOdom=odometryQueue.front().header.stamp.toSec();
if(std::fabs(timePlane-timeOdom)>0.005){ //时间同步判断
printf("frame time unsync messeage! \n");
mLock.unlock();
break;
}
isDone=0;
numFrame++;
currFramePlanePtr->clear();
pcl::fromROSMsg(planeQueue.front(),*currFramePlanePtr);
planeQueue.pop();
q_fodom_0_curr.x()=odometryQueue.front().pose.pose.orientation.x;
q_fodom_0_curr.y()=odometryQueue.front().pose.pose.orientation.y;
q_fodom_0_curr.z()=odometryQueue.front().pose.pose.orientation.z;
q_fodom_0_curr.w()=odometryQueue.front().pose.pose.orientation.w;
t_fodom_0_curr.x()=odometryQueue.front().pose.pose.position.x;
t_fodom_0_curr.y()=odometryQueue.front().pose.pose.position.y;
t_fodom_0_curr.z()=odometryQueue.front().pose.pose.position.z;
odometryQueue.pop();
mLock.unlock();
T_fodom_0_curr=Eigen::Affine3d::Identity();
T_fodom_0_curr.rotate(q_fodom_0_curr);
T_fodom_0_curr.pretranslate(t_fodom_0_curr);
T_map_0_curr=trans_loop_adjust*T_fodom_0_curr; // 闭环累计误差校正
mapOptimization();
isDone=1;
}
}
}
int main(int argc, char **argv) {
parameters.relinearizeThreshold = 0.1;
parameters.relinearizeSkip = 1;
isam = new gtsam::ISAM2(parameters);
downSizeFilterICP.setLeafSize(0.1,0.1,0.1);
downSizeFilterMap.setLeafSize(0.4,0.4,0.4);
ros::init(argc, argv, "MapOptmization");
ros::NodeHandle nh;
sub_plane_frame_cloud = nh.subscribe<sensor_msgs::PointCloud2>("/plane_frame_cloud2", 10, planeCloudHandler);
sub_frame_Odometry = nh.subscribe<nav_msgs::Odometry>("/frame_odom2", 100, odomHandler);
pub_map_frame = nh.advertise<sensor_msgs::PointCloud2>("/map_frame_res3", 10);
pub_sum_map_cloud = nh.advertise<sensor_msgs::PointCloud2>("/sum_map_cloud_res3", 10);
pub_map_odometry = nh.advertise<nav_msgs::Odometry>("/map_odom_res3", 100);
pub_map_path = nh.advertise<nav_msgs::Path>("/map_laser_path_res3", 100);
ROS_INFO("\033[1;32m----> MapOptmization Started.\033[0m");
std::thread t_cloud_thread(cloudThread);
ros::spin();
t_cloud_thread.join();
return 0;
}
代码地址为(GitHub - haocaichao/S-LOAM: S-LOAM(Simple LOAM) 是一种简单易学的激光SLAM算法,整个程序只有几百行代码,十分方便学习与试验分析。)。