再来看看第三部分，这一节是建图部分，与原有LOAM不同的是增加了回环检测，我们可以用经典的graphSLAM的思想来看它的过程。
定位到主函数，它与上一个节点类似，也是一个run函数不停轮转，它对实时性要求较高。
void run(){
if (newLaserCloudCornerLast && std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
newLaserCloudSurfLast && std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
newLaserOdometry)
{
newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;
std::lock_guard<std::mutex> lock(mtx);
//mappingProcessInterval是0.3秒，以相对较慢的速度进行建图
if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
timeLastProcessing = timeLaserOdometry;
//把点云坐标均转换到世界坐标系下
transformAssociateToMap();
//由于帧数的频率大于建图的频率，因此需要提取关键帧进行匹配
extractSurroundingKeyFrames();
//降采样匹配以及增加地图点云，回环检测
downsampleCurrentScan();
scan2MapOptimization();
saveKeyFramesAndFactor();
//发送TF变换
correctPoses();
publishTF();
publishKeyPosesAndFrames();
clearCloud();
}
}
}1.transformAssociateToMap函数，我们可以看到是围绕transformTobeMapped数组进行的变换，这就是从雷达坐标系转换到世界坐标系下的变换。
2.extractSurroundingKeyFrames函数作为关键帧的匹配，首先就要对回环检测进行判断：
2.1 先看看没有回环的情况，在这里是寻找附近点之后进行降维处理，目的是让地图点云不要太过密集，最终将合适的点云累加起来，这是一个将新点云在线调整并叠加的过程。
else
{
surroundingKeyPoses->clear();
surroundingKeyPosesDS->clear();
//cloudKeyPoses3D虽说是点云，但是是为了保存机器人在建图过程中的轨迹，其中的点就是定周期采样的轨迹点，这一点是在saveKeyFramesAndFactor中计算出的，在第一帧时必然是空的
//surroundingKeyframeSearchRadius是50米，也就是说是在当前位置进行半径查找，得到附近的轨迹点
//距离数据保存在pointSearchSqDis中
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
kdtreeSurroundingKeyPoses->radiusSearch(currentRobotPosPoint, (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis, 0);
for (int i = 0; i < pointSearchInd.size(); ++i)
surroundingKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchInd[i]]);
//对附近轨迹点的点云进行降采样，轨迹具有一定间隔
downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
int numSurroundingPosesDS = surroundingKeyPosesDS->points.size();
for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i){
bool existingFlag = false;
for (int j = 0; j < numSurroundingPosesDS; ++j){
//在这里我们可以看到points的intensity参数在saveKeyFramesAndFactor函数中计算，它代表cloudKeyPoses3D这个点云的点数量，点云intensity参数在代码里真心被玩坏了
//也就是说，这个等式的意义是判断附近某一个关键帧等于降维后点云的第j个关键帧
if (surroundingExistingKeyPosesID[i] == (int)surroundingKeyPosesDS->points[j].intensity){
existingFlag = true;
break;
}
}
//这也是一个变相的降维处理
if (existingFlag == false){
surroundingExistingKeyPosesID. erase(surroundingExistingKeyPosesID. begin() + i);
//这三个都是双端队列
surroundingCornerCloudKeyFrames. erase(surroundingCornerCloudKeyFrames. begin() + i);
surroundingSurfCloudKeyFrames. erase(surroundingSurfCloudKeyFrames. begin() + i);
surroundingOutlierCloudKeyFrames.erase(surroundingOutlierCloudKeyFrames.begin() + i);
--i;
}
}
for (int i = 0; i < numSurroundingPosesDS; ++i) {
bool existingFlag = false;
for (auto iter = surroundingExistingKeyPosesID.begin(); iter != surroundingExistingKeyPosesID.end(); ++iter){
if ((*iter) == (int)surroundingKeyPosesDS->points[i].intensity){
existingFlag = true;
break;
}
}
if (existingFlag == true){
continue;
}else{
//这类情况是初次处理时将点云变换到当前坐标系下
//cloudKeyPoses6D中的数据来源自thisPose6D，我们可以看到thisPose6D根据isam库进行先验后得到，所以在下面容器中存放的都是已经粗配准完毕的点云下标
int thisKeyInd = (int)surroundingKeyPosesDS->points[i].intensity;
PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
updateTransformPointCloudSinCos(&thisTransformation);
surroundingExistingKeyPosesID. push_back(thisKeyInd);
surroundingCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
surroundingSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
surroundingOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
}
}
//累加点云
for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) {
*laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingOutlierCloudKeyFrames[i];
}
}2.2 回环检测出现的情况
if (loopClosureEnableFlag == true)
{
//surroundingKeyframeSearchNum是50，我的理解是将角点、平面点等均填充至合格数量
if (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum)
{
recentCornerCloudKeyFrames. clear();
recentSurfCloudKeyFrames. clear();
recentOutlierCloudKeyFrames.clear();
int numPoses = cloudKeyPoses3D->points.size();
for (int i = numPoses-1; i >= 0; --i)
{
int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity;
PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
updateTransformPointCloudSinCos(&thisTransformation);
recentCornerCloudKeyFrames. push_front(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
recentSurfCloudKeyFrames. push_front(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
recentOutlierCloudKeyFrames.push_front(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
if (recentCornerCloudKeyFrames.size() >= surroundingKeyframeSearchNum)
break;
}
}
//将最新的点云叠加
else
{
if (latestFrameID != cloudKeyPoses3D->points.size() - 1)
{
recentCornerCloudKeyFrames. pop_front();
recentSurfCloudKeyFrames. pop_front();
recentOutlierCloudKeyFrames.pop_front();
latestFrameID = cloudKeyPoses3D->points.size() - 1;
PointTypePose thisTransformation = cloudKeyPoses6D->points[latestFrameID];
updateTransformPointCloudSinCos(&thisTransformation);
recentCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[latestFrameID]));
recentSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[latestFrameID]));
recentOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[latestFrameID]));
}
}
for (int i = 0; i < recentCornerCloudKeyFrames.size(); ++i)
{
*laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentOutlierCloudKeyFrames[i];
}
}3.downsampleCurrentScan函数是将当前各类点云降采样，其中laserCloudSurfTotalLast是平面部分与异常部分的叠加。
4.scan2MapOptimization函数是根据现有地图与最新点云数据进行配准从而更新机器人精确位姿与融合建图，它分为角点优化、平面点优化、配准与更新等部分。
优化的过程与里程计的计算类似，是通过计算点到直线或平面的距离，构建优化公式再用LM法求解。这是角点优化cornerOptimization的函数：
void cornerOptimization(int iterCount){
updatePointAssociateToMapSinCos();
for (int i = 0; i < laserCloudCornerLastDSNum; i++) {
pointOri = laserCloudCornerLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
//利用kd树查找最近的5个点，接下来需要计算这五个点的协方差
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
//只有最近的点都在一定阈值内（1米）才进行计算
if (pointSearchSqDis[4] < 1.0) {
float cx = 0, cy = 0, cz = 0;
for (int j = 0; j < 5; j++) {
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
//计算算术平均值
cx /= 5; cy /= 5; cz /= 5;
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
//计算协方差矩阵
for (int j = 0; j < 5; j++) {
float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
a22 += ay * ay; a23 += ay * az;
a33 += az * az;
}
a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
//求协方差矩阵的特征值和特征向量
cv::eigen(matA1, matD1, matV1);
//与里程计的计算类似，计算到直线的距离
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = cx + 0.1 * matV1.at<float>(0, 0);
float y1 = cy + 0.1 * matV1.at<float>(0, 1);
float z1 = cz + 0.1 * matV1.at<float>(0, 2);
float x2 = cx - 0.1 * matV1.at<float>(0, 0);
float y2 = cy - 0.1 * matV1.at<float>(0, 1);
float z2 = cz - 0.1 * matV1.at<float>(0, 2);
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
* ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
* ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
* ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float ld2 = a012 / l12;
float s = 1 - 0.9 * fabs(ld2);
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
if (s > 0.1) {
laserCloudOri->push_back(pointOri);
coeffSel->push_back(coeff);
}
}
}
}
}5. 优化完毕后更新姿态的transformUpdate函数是利用IMU的数据校正Pitch与Roll角，接着轮转更新姿态数据transformBefMapped与transformAftMapped，等待下次更新。
transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast;
transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast;
for (int i = 0; i < 6; i++) {
transformBefMapped[i] = transformSum[i];
transformAftMapped[i] = transformTobeMapped[i];
}6.saveKeyFramesAndFactor函数，接下来的过程都是LOAM不具备的，保存轨迹与位姿图就是为了回环检测。
void saveKeyFramesAndFactor(){
currentRobotPosPoint.x = transformAftMapped[3];
currentRobotPosPoint.y = transformAftMapped[4];
currentRobotPosPoint.z = transformAftMapped[5];
bool saveThisKeyFrame = true;
//前后两个位姿之间不要差距太近
if (sqrt((previousRobotPosPoint.x-currentRobotPosPoint.x)*(previousRobotPosPoint.x-currentRobotPosPoint.x)
+(previousRobotPosPoint.y-currentRobotPosPoint.y)*(previousRobotPosPoint.y-currentRobotPosPoint.y)
+(previousRobotPosPoint.z-currentRobotPosPoint.z)*(previousRobotPosPoint.z-currentRobotPosPoint.z)) < 0.3){
saveThisKeyFrame = false;
}
if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
return;
previousRobotPosPoint = currentRobotPosPoint;
//如果是第一帧数据则直接加入到位姿图中
if (cloudKeyPoses3D->points.empty()){
gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));
for (int i = 0; i < 6; ++i)
transformLast[i] = transformTobeMapped[i];
}
//否则按更新后的位姿对位姿图进行更新
else{
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),
Point3(transformLast[5], transformLast[3], transformLast[4]));
gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise));
initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])));
}
isam->update(gtSAMgraph, initialEstimate);
isam->update();
gtSAMgraph.resize(0);
initialEstimate.clear();
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
thisPose3D.x = latestEstimate.translation().y();
thisPose3D.y = latestEstimate.translation().z();
thisPose3D.z = latestEstimate.translation().x();
thisPose3D.intensity = cloudKeyPoses3D->points.size();
cloudKeyPoses3D->push_back(thisPose3D);
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity;
thisPose6D.roll = latestEstimate.rotation().pitch();
thisPose6D.pitch = latestEstimate.rotation().yaw();
thisPose6D.yaw = latestEstimate.rotation().roll();
thisPose6D.time = timeLaserOdometry;
cloudKeyPoses6D->push_back(thisPose6D);
if (cloudKeyPoses3D->points.size() > 1){
transformAftMapped[0] = latestEstimate.rotation().pitch();
transformAftMapped[1] = latestEstimate.rotation().yaw();
transformAftMapped[2] = latestEstimate.rotation().roll();
transformAftMapped[3] = latestEstimate.translation().y();
transformAftMapped[4] = latestEstimate.translation().z();
transformAftMapped[5] = latestEstimate.translation().x();
//轮转更新位姿
for (int i = 0; i < 6; ++i){
transformLast[i] = transformAftMapped[i];
transformTobeMapped[i] = transformAftMapped[i];
}
}
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);
//叠加点云
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);
}7.correctPoses是回环检测成功后将位姿图的数据依次更新。之后是发布TF变换等一些善后工作。
这就是主线程的大致流程，接下来看看其他线程的工作：
①visualizeGlobalMapThread是以0.2Hz的频率发布地图点云，它对地图点云进行了两次降维，并且将角点、平面点、异常点均加入到最终的地图点云中发布。
②loopClosureThread是1HZ的频率在回环检测，我们先来看看是怎么检测是否成功的：
bool detectLoopClosure()
{
latestSurfKeyFrameCloud->clear();
nearHistorySurfKeyFrameCloud->clear();
nearHistorySurfKeyFrameCloudDS->clear();
std::lock_guard<std::mutex> lock(mtx);
std::vector<int> pointSearchIndLoop;
std::vector<float> pointSearchSqDisLoop;
//寻找附近5m内的历史轨迹
kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
closestHistoryFrameID = -1;
for (int i = 0; i < pointSearchIndLoop.size(); ++i)
{
int id = pointSearchIndLoop[i];
//需要有一定的时间间隔
if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0)
{
closestHistoryFrameID = id;
break;
}
}
if (closestHistoryFrameID == -1)
{
return false;
}
//最新的一帧点云
latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1;
*latestSurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
*latestSurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
pcl::PointCloud<PointType>::Ptr hahaCloud(new pcl::PointCloud<PointType>());
int cloudSize = latestSurfKeyFrameCloud->points.size();
for (int i = 0; i < cloudSize; ++i)
{
if ((int)latestSurfKeyFrameCloud->points[i].intensity >= 0)
{
hahaCloud->push_back(latestSurfKeyFrameCloud->points[i]);
}
}
latestSurfKeyFrameCloud->clear();
*latestSurfKeyFrameCloud = *hahaCloud;
//historyKeyframeSearchNum是25,
for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j)
{
//如果超出搜索边界则无视
if (closestHistoryFrameID + j < 0 || closestHistoryFrameID + j > latestFrameIDLoopCloure)
continue;
//将搜索范围内的角点点云与平面点点云均叠加至nearHistorySurfKeyFrameCloud中
*nearHistorySurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
*nearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
}
downSizeFilterHistoryKeyFrames.setInputCloud(nearHistorySurfKeyFrameCloud);
downSizeFilterHistoryKeyFrames.filter(*nearHistorySurfKeyFrameCloudDS);
if (pubHistoryKeyFrames.getNumSubscribers() != 0)
{
sensor_msgs::PointCloud2 cloudMsgTemp;
pcl::toROSMsg(*nearHistorySurfKeyFrameCloudDS, cloudMsgTemp);
cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
cloudMsgTemp.header.frame_id = "/camera_init";
pubHistoryKeyFrames.publish(cloudMsgTemp);
}
return true;
}因此LeGO-LOAM的回环检测是基于位置也就是里程计的几何关系来实现的，这一点似乎值得商榷，虽然根据里程计的不断匹配算是比较精准，但是当地图本身尺寸导致不可避免的误差时，基于几何关系的里程计会有些力不从心。
最终得到的nearHistorySurfKeyFrameCloudDS点云将用于ICP匹配正确位姿：
void performLoopClosure()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
if (potentialLoopFlag == false)
{
if (detectLoopClosure() == true)
{
potentialLoopFlag = true;
timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry;
}
if (potentialLoopFlag == false)
return;
}
potentialLoopFlag = false;
//经典的ICP匹配流程
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(100);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
icp.setInputSource(latestSurfKeyFrameCloud);
icp.setInputTarget(nearHistorySurfKeyFrameCloudDS);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result);
//historyKeyframeFitnessScore是0.3
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
if (pubIcpKeyFrames.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud (*latestSurfKeyFrameCloud, *closed_cloud, icp.getFinalTransformation());
sensor_msgs::PointCloud2 cloudMsgTemp;
pcl::toROSMsg(*closed_cloud, cloudMsgTemp);
cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
cloudMsgTemp.header.frame_id = "/camera_init";
pubIcpKeyFrames.publish(cloudMsgTemp);
}
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionCameraFrame;
//最终匹配得到的坐标转换
correctionCameraFrame = icp.getFinalTransformation();
//转换IMU坐标系与雷达坐标系
pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch);
Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]);
gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
constraintNoise = noiseModel::Diagonal::Variances(Vector6);
std::lock_guard<std::mutex> lock(mtx);
//在位姿图中加入新匹配得到的位姿约束
gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));
isam->update(gtSAMgraph);
isam->update();
gtSAMgraph.resize(0);
aLoopIsClosed = true;
}如之前所说在回环检测成功后，位姿图会在saveKeyFramesAndFactor函数中依次更新，这样就实现了回环检测的校正功能，而地图点云也会再次更新，这样我们就实现了建图功能。
2659
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
