增加回环距离限制，距离小于10m，角度小于45°

src/laserMapping.cpp

@@ -663,6 +663,25 @@ void performLoopClosure() {
   }else{
     return;
   }
+  //30帧以内，不考虑回环
+  if(loopKeyCur-loopKeyPre<30){
+    return;
+  }
+  //检验两帧位姿是否在附近 距离< 10 角度45°
+  // 前一帧位姿
+  Eigen::Affine3f transStart = pclPointToAffine3f(keyFrames_[loopKeyPre].Pose());
+  // 当前帧位姿
+  Eigen::Affine3f transFinal = pclPointToAffine3f(keyFrames_[loopKeyCur].Pose());
+  // 位姿变换增量
+  Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
+  float x, y, z, roll, pitch, yaw;
+  pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw); //  获取上一帧 相对 当前帧的 位姿
+  // 两帧相距比较远
+  if (abs(roll) > 20*M_PI/180.0 ||
+      abs(pitch) > 20*M_PI/180.0 ||
+      abs(yaw) > 45*M_PI/180.0 ||
+      sqrt(x * x + y * y + z * z) > 10)
+    return ;
 
   // 提取
   pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>()); //  cue keyframe
@@ -684,7 +703,7 @@ void performLoopClosure() {
   double icp_t1 = omp_get_wtime();
   float noiseScore;
   Eigen::Affine3f correctionLidarFrame;
-  float x, y, z, roll, pitch, yaw;
+  //float x, y, z, roll, pitch, yaw;
 
   // ICP Settings
 if(ndt_loop==false) {
@@ -761,7 +780,7 @@ if(ndt_loop==false) {
 
   Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
   gtsam::noiseModel::Diagonal::shared_ptr constraintNoise = gtsam::noiseModel::Diagonal::Variances(Vector6);
-  std::cout << "loopNoiseQueue   =   " << noiseScore << std::endl;
+  std::cout << "loop matched Noise   =   " << noiseScore << std::endl;
 
   // 添加闭环因子需要的数据
   mtx.lock();
@@ -801,7 +820,7 @@ void saveKeyFramesAndFactor()
         isam->update();
         isam->update();
         isam->update();
-        printf(" isam update \n");
+        printf("loop isam update \n");
     }
     // update之后要清空一下保存的因子图，注：历史数据不会清掉，ISAM保存起来了
     gtSAMgraph.resize(0);
@@ -857,8 +876,6 @@ void saveKeyFramesAndFactor()
     // pcl::copyPointCloud(*feats_undistort,  *thisCornerKeyFrame);
     pcl::copyPointCloud(*feats_undistort, *thisSurfKeyFrame); // 存储关键帧,没有降采样的点云
     keyFrames_.push_back(keyFrame(thisPose6D,thisSurfKeyFrame));
-    printf(" keyFrames add %d pose: %f %f %f %f %f %f\n",keyFrames_.size(),thisPose6D.x,thisPose6D.y,thisPose6D.z,
-           thisPose6D.roll,thisPose6D.pitch,thisPose6D.yaw);
     updatePath(thisPose6D); //  可视化update后的path
 }