mapOptimization函数
transformAssociateToMap函数(细节剖析)
**transformSum:**为当前时刻里程计的估计的姿态变换
**transformBefMapped：**为上一时刻里程计估计的姿态变换
**transformAftMapped：**为上一时刻优化后的全局姿态变换
**transformTobeMapped：**为当前时刻待优化的全局姿态变换
这四个变量都是在世界坐标系下的表示——>下面这页纸是分别运用里程计估计和建图优化推算出的上一时刻lidar pose经过姿态变换得到当前时刻lidar pose的两种不同的推算过程。
通过transformAssociateToMap函数能够估计出一个未经优化的当前时刻的经过建图后的全局姿态变换的估计
参考链接 很重要
参考链接
void transformAssociateToMap()
{//transform delta pose from world frame to current frame
std::cout << "carry out transformAssociateToMap thread"<<std::endl;//yaw T
//using the odometry to estimate the transition in current frame
// (transformSum is the rotation from world to current frame)
float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
- sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
float y1 = transformBefMapped[4] - transformSum[4];
float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
+ cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
float x2 = x1;//pitch T
float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;
// for the estimation of the transition using odometry information0
transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
transformIncre[5] = z2;//roll T
float sbcx = sin(transformSum[0]);
float cbcx = cos(transformSum[0]);
float sbcy = sin(transformSum[1]);
float cbcy = cos(transformSum[1]);
float sbcz = sin(transformSum[2]);
float cbcz = cos(transformSum[2]);//current LO estimated pose
float sblx = sin(transformBefMapped[0]);
float cblx = cos(transformBefMapped[0]);
float sbly = sin(transformBefMapped[1]);
float cbly = cos(transformBefMapped[1]);
float sblz = sin(transformBefMapped[2]);
float cblz = cos(transformBefMapped[2]);//last LO estimated pose
float salx = sin(transformAftMapped[0]);
float calx = cos(transformAftMapped[0]);
float saly = sin(transformAftMapped[1]);
float caly = cos(transformAftMapped[1]);
float salz = sin(transformAftMapped[2]);
float calz = cos(transformAftMapped[2]);
//for the estimation of the current after mapped pose(before updated) store in transformTobeMapped
//first estimate the tobe updated euler angle
float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)
- cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
- cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);
transformTobeMapped[0] = -asin(srx);
float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)
- cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)
- cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)
+ (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)
+ cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)
+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);
float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)
- cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)
+ cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
+ (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)
- cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)
+ (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);
transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]),
crycrx / cos(transformTobeMapped[0]));
float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
+ cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
+ cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]),
crzcrx / cos(transformTobeMapped[0]));
//current to world frame (yaw pitch roll) transformIncre is transition in current frame
x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];
y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4];
z1 = transformIncre[5];
x2 = x1;
y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;
//used the last after maped transition - tobeupdated transition for current tobemapped transition
transformTobeMapped[3] = transformAftMapped[3]
- (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);
transformTobeMapped[4] = transformAftMapped[4] - y2;
transformTobeMapped[5] = transformAftMapped[5]
- (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);
}
TransformToEnd函数
我是这么理解的：先将当前帧的laser point (由于点的扫描是连续的)，这是通过s * transformCur[0],假设匀速运动模型，将点云转换到统一的first lidar (last imu)对应的frame下，然后，在通过transformCur变换，得到当前点云在first lidar下做的变换，得到当前帧点云在first lidar 下得到的姿态更新，然后在通过从first lidar(first)->end lidar(imu)提供的变换，将last end frame 的点云的姿态更新位姿，转到当前时刻的end 姿态更新的位姿
first laser scan map optimization执行内容
- 执行transformAssociateToMap函数，利用通过laserOdometryHandler接收到的经过雷达里程计估计出来的pose信息，对即将被mapping updated的优化位姿做估计预测。并将估计的mapping updated 信息存储到transformTobeMapped中
- 由于当前帧还未提取出关键帧点云，所以extractSurroundingKeyFrames函数不执行，而是执行downsampleCurrentScan对当前帧点云的corner 以及 surf feature point 进行降采样。[降采样的点云分别存储在laserCloudCornerLastDSNum，laserCloudSurfLastDSNum，laserCloudOutlierLastDSNum中，并通过laserCloudSurfTotalLast以及laserCloudSurfTotalLast存储每一帧点云的特征点信息(关注一下后面这个怎么调整)]
- scan2MapOptimization中因为还没有插入到global map中的feature point 因此 不能利用 map 中的关键特征点信息进行scan to map 的优化。
- saveKeyFramesAndFactor 中，首先明确当前帧的初始robot pose is 0.
currentRobotPosPoint.x = transformAftMapped[3];//for first laser scan 0
currentRobotPosPoint.y = transformAftMapped[4];
currentRobotPosPoint.z = transformAftMapped[5];
但是关注这一行代码：
if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
return;
不退出当前的saveKeyFramesAndFactor函数，因为需要从第一帧点云提取关键帧用于后面的scan2mapOptimization.
将第一次得到的优化里程计的估计位姿信息transformAssociateToMap得到的transformTobeMapped估计的位姿预测值加入到factor graph中作为initialEstimate——初始估计，并将初始的mapping后的位姿预测值作为上一次的transform结果传递给transformLast(我推测是用来记录上一次transform 优化后的pose 信息的)，对第一帧点云的估计位姿信息执行isam的优化更新。
isam->update(gtSAMgraph, initialEstimate);
isam->update();
将优化更新后的pose姿态信息作为一个三维点云信息，存储在keypose中作为关键帧输出！！(这里不止存储3Dpose还存储6Dpose)
这里的巧妙之处在于将利用更新的pose信息，作为一个关键帧点信息存储在keypose中
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
//key pose storation
thisPose3D.x = latestEstimate.translation().y();
thisPose3D.y = latestEstimate.translation().z();
thisPose3D.z = latestEstimate.translation().x();
thisPose3D.intensity = cloudKeyPoses3D->points.size(); // this can be used as index
cloudKeyPoses3D->push_back(thisPose3D);
//for the estimation of the 6 pose
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity; // this can be used as index
thisPose6D.roll = latestEstimate.rotation().pitch();
thisPose6D.pitch = latestEstimate.rotation().yaw();
thisPose6D.yaw = latestEstimate.rotation().roll(); // in camera frame
thisPose6D.time = timeLaserOdometry;
cloudKeyPoses6D->push_back(thisPose6D);
将优化更新的位姿进行存储transformAftMapped作为updated后的pose 信息，这样也能明白 transformLast[i] = transformAftMapped[i];中transformLast记录的是前一帧优化更新后的optimized pose,并将下一帧的带估计mapped的pose的预测值初始化为前一帧优化后的optimized pose
pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisOutlierKeyFrame(new pcl::PointCloud<PointType>());
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);//for the last feature point as the key pose
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);//for the storation of the key pose
当前帧被选择为关键帧后，其corner feature 以及surf feature作为关键帧点云，存储在CloudKeyFrames(用来保存所有被选作为关键帧的点云数据信息)中，记录所有作为关键帧的点云。
5. 通过publishTF函数，分别将mapping优化后的pose以及transformBefMapped==这个transformBefMapped代表什么？==的角速度和加速度信息存储到odomAftMapped基于mapping 优化后的odo pose，并进行发布。同时aftMappedTrans发布基于camera到aftermapped 的基于mapping update 的优化位姿变换。
6. 最后通过publishKeyPosesAndFrames函数，发布关键帧点云以及基于优化估计的位姿transformTobeMapped，发布基于里程计的特征点云的数据信息
对于非起始点云
- 通过transformAssociateToMap函数根据当前里程计数据transformSum，transformBefMapped前一时刻里程计数据和transformBefMapped 前一个时刻的优化全局位姿估计出transformTobeMapped 待优化的当前时刻的优化位姿的预测值
- 通过extractSurroundingKeyFrames函数，由于前一个时刻存储了之前所有时刻提取出来的关键帧cloudKeyPoses3D信息，通过kdtree 查找基于上一个优化机器人的pose information currentRobotPosPoint进行最近邻搜索radiusSearch，同时返回所有在上一个优化的机器人关键帧位姿的全部特征关键帧(基于旧关键帧 取提取附近的关键帧 用于提供 scan to map 的匹配),判断基于上一帧优化的关键帧数据其提取出的可用于优化的关键帧的特征信息，是否在surroundingKeyPosesDS中存在，对于不存在的特征 (表示之前存在的老的关键帧特征数据已经不能支持scan 2 map 基于前一帧的优化 采集到的特征 提供新的特征关键帧了 所以 需要删除旧关键帧 以及插入新的关键帧 )这里需要注意surroundingExistingKeyPosesID以及surroundingCornerCloudKeyFrames；surroundingSurfCloudKeyFrames；surroundingOutlierCloudKeyFrames均为基于扫描线的顺序存储，在for 循环插入新的关键帧数据时，通过上一次update 的pose 数据，利用transformPointCloud函数，将插入的新的关键帧数据 transform到world 坐标系，然后在surroundingExistingKeyPoses和Frame中插入，分别将已经经过更新的存在的特征数据插入MaplaserCloudCornerFromMap 以及laserCloudSurfFromMap 和laserCloudSurfFromMap中 用于 构建全局特征地图这个全局地图怎么用？
- scan2MapOptimization基于当前帧的特征数据和kdtreeFeatureFromMap中存储的已经存在的特征数据，执行迭代更新(基于特征)cornerOptimization，surfOptimization
- 大致的更新流程如下：cornerOptimization实则利用updatePointAssociateToMapSinCos函数，根据利用transformAssociateToMap函数基于上一帧的优化位姿和上一帧和当前帧的里程计优化位姿 ，计算得出当前帧未基于mapping 优化前的estimated 位姿计算出当前机器人位姿相对于world 的欧拉角，基于当前帧的特征点信息laserCloudCornerLastDSNum通过pointAssociateToMap函数将当前帧的first imu位置的信息转到world frame,然后在kdtree中搜索**这里注意kdtree 的feature point 信息已经通过transformPointCloud函数，将之前扫描的关键帧特征信息，通过上一次优化的位姿updateTransformPointCloudSinCos函数，更新了最新的一次优化的pose 信息，利用yaw -> pitch ->roll转到了world frame****实现了在global map 以及 current scan中的feature point 均在world坐标系中进行匹配**搜索5个最近邻，计算协方差，然后求出直线的主要的方向向量。这里有一个巧妙的设计(为了在LMOptimization函数中同时使用点面特征进行计算，此时在corneroptimization中两次运用叉乘，计算出surf的法向量 转换成店面特征)
点线特征判断 入1>3入2
- 实现点面特征匹配**surfOptimization：**先投影到world坐标系，然后利用估计的待优化位姿 通过pointAssociateToMap函数将current scan feature point 转到world frame 与exist feature point进行匹配。！！！
这里需要理解一个流程。
先通过cv::solve函数利用neighbour point信息，运用超定方程的解算策略，AX = B的形式 解算出平面surf的法向量信息，然后判断平面的拟合是否有效，在根据有效的平面与面点对应，存储构建的有效的surf 特征的法向量。
参考公式原理
- 利用LMOptimization函数，对yaw -> pitch -> roll的旋转矩阵求偏导数，
对于旋转矩阵求偏导数
关于平移向量的偏导数
float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;
float ary = ((cry*srx*srz - crz*sry)*pointOri.x
+ (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
+ ((-cry*crz - srx*sry*srz)*pointOri.x
+ (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
matA.at<float>(i, 0) = arx;//for the df / drotation
matA.at<float>(i, 1) = ary;
matA.at<float>(i, 2) = arz;
matA.at<float>(i, 3) = coeff.x;//for df /dtransition
matA.at<float>(i, 4) = coeff.y;
matA.at<float>(i, 5) = coeff.z;
matB.at<float>(i, 0) = -coeff.intensity;
A矩阵的构成
高斯牛顿法的优化公式
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);//jtj detax = - jt b
以及判断是否退化，最后更新优化的位姿transformTobeMapped。并判断迭代停止的条件
if (iterCount == 0) {//for the detection of the degenerate
cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[6] = {100, 100, 100, 100, 100, 100};
for (int i = 5; i >= 0; i--) {
if (matE.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;
}
if (isDegenerate) {
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;//using the un distorted estimation to update the delta
}
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);//update the estimated pose using the mapping optimization
float deltaR = sqrt(
pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
float deltaT = sqrt(
pow(matX.at<float>(3, 0) * 100, 2) +
pow(matX.at<float>(4, 0) * 100, 2) +
pow(matX.at<float>(5, 0) * 100, 2));
if (deltaR < 0.05 && deltaT < 0.05) {
return true;//terminate the iteration
}
return false;
- 基于LM高斯牛顿实现的scan2map的迭代优化后，通过transformUpdate函数基于当前里程计的时找出与之关联的imu信息，运用下面这两行信息，将更新的位姿通过调整权重的方式，在优化更新的情况下，考虑IMU数据带来的影响，至此scan2MapOptimization基于扫描scan到局部地图map的匹配完成
transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast;
transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast;//weighted fusion
- saveKeyFramesAndFactor函数基于LM优化结果更新机器人位姿
currentRobotPosPoint.x = transformAftMapped[3];//for first laser scan 0
currentRobotPosPoint.y = transformAftMapped[4];//for not first laser is the scan2map updated pose
currentRobotPosPoint.z = transformAftMapped[5];
判断更新的优化位姿是否满足创建关键帧的条件。如果满足，先将当前帧数据，作为上一个关键帧数据进行赋值，然后利用上一帧的优化信息transformLast和当前帧的优化位姿transformAftMapped创建
参考数学推导链接
//
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),
Point3(transformLast[5], transformLast[3], transformLast[4]));//last updated pose
gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));//current updated pose
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(),
poseFrom.between(poseTo), odometryNoise));//for the between factor id is size - 1 to size （key）
initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),//当前基于约束优化的更新关键帧的pose 信息为基于corner 与surf optimization 经过LM优化后的pose 来作为initial pose 去进行isam的增量图约束优化。
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])));
//add the size which need updated pose in the initialEstimate with the initial guesses is last updated pose
上面这函数poseFrom为上一个时刻更新的pose,poseTo为当前时刻基于LM更新的pose,通过add函数，将两个关键帧之间构建约束。
BetweenFactor用于表示两个状态变量之间的相对约束。描述了这两个变量之间的相对关系。这种因子在优化过程中提供了关于两个状态之间相对位置或姿态的约束。** poseFrom.between(poseTo)为Ra_w * Rw_b计算出Ra_b的相对旋转**，基于isam优化更新的factor graph信息，提取出优化后的transformation信息，存储为cloudKeyPoses3D以及cloudKeyPoses6D，并进行增量更新优化的保存。**transformLast为last isam 增量优化的pose 以及transformTobeMapped也通过上一次的last optimized 进行初始化 **
for (int i = 0; i < 6; ++i){
transformLast[i] = transformAftMapped[i];//last after mapping updated pose store
transformTobeMapped[i] = transformAftMapped[i];
//after mapped but with no next frame pose use the current aftmapped estimation to update the next need mapped estimation
}
- publishTF函数用来发布基于优化后的位姿的转换
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
tf::TransformBroadcaster broadcaster;的作用是创建一个tf::TransformBroadcaster类的对象，用来广播坐标系base_link -> base_laser的变换关系。sendTransform函数用来广播parent frame ->child frame之间的坐标变换关系。
针对于visualizeGlobalMapThread与main线程
visualizeGlobalMapThread线程通过ros::Rate rate(0.2)每5s执行一次global map的构建过程，而基于LO里程计的优化，通过 ros::Rate rate(200)每0.05s执行一次里程计的优化过程，所以针对于map Optimization函数，LO 的里程计优化执行的频率更高，global map的发布则需要积累足够的key pose去发布。
整体main函数
执行是while结束-》threadloopthread结束-》visualizeGlobalMapThre结束
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
for(int i = 0; i<argc; i++)
{
std::cout<<"argv:"<<argv[i]<<std::endl;
}
ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
mapOptimization MO;
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
ros::Rate rate(200);
while (ros::ok())
{
ros::spinOnce();
MO.run();
rate.sleep();
}
loopthread.join();
visualizeMapThread.join();
return 0;
}
扫一扫
937
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
