解决异常，代码已正常，ros 消息time赋值不对会异常

rviz_cfg/loam_livox.rviz

@@ -110,7 +110,7 @@ Visualization Manager:
           Color: 115; 210; 22
           Color Transformer: Intensity
           Decay Time: 1000
-          Enabled: false
+          Enabled: true
           Invert Rainbow: true
           Max Color: 255; 255; 255
           Min Color: 0; 0; 0
@@ -125,7 +125,7 @@ Visualization Manager:
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
-          Value: false
+          Value: true
       Enabled: true
       Name: mapping
     - Class: rviz/Group
@@ -382,5 +382,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1359
-  X: -273
-  Y: 58
+  X: 0
+  Y: 27

src/keyFramesManager.cpp

@@ -12,6 +12,7 @@ keyFrame::keyFrame(PointCloudXYZI::Ptr cloud) {
 keyFrame::keyFrame(PointTypePose pose, PointCloudXYZI::Ptr cloud) {
   cloud_=cloud;
   cloud_world_.reset(new PointCloudXYZI());
+  pose_=pose;
   pose_changed_=true;
 
 }

src/laserMapping.cpp

@@ -504,30 +504,33 @@ void visualizeLoopClosure()
 /**
  * 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
  */
-bool saveFrame()
-{
-    if (cloudKeyPoses3D->points.empty())
-        return true;
-
-    // 前一帧位姿
-    Eigen::Affine3f transStart = pclPointToAffine3f(keyFrames_.back().Pose());
-    // 当前帧位姿
-    Eigen::Affine3f transFinal = trans2Affine3f(transformTobeMapped);
-    // Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
-    //                                                     transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
-                    
-    // 位姿变换增量
-    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) < surroundingkeyframeAddingAngleThreshold &&
-        abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
-        abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
-        sqrt(x * x + y * y + z * z) < surroundingkeyframeAddingDistThreshold)
-        return false;
+bool saveFrame() {
+  if (cloudKeyPoses3D->points.empty()) {
+    printf(" 3d empty add keyframe\n");
     return true;
+  }
+
+  // 前一帧位姿
+  Eigen::Affine3f transStart = pclPointToAffine3f(keyFrames_.back().Pose());
+  // 当前帧位姿
+  Eigen::Affine3f transFinal = trans2Affine3f(transformTobeMapped);
+  // Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
+  //                                                     transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
+
+  // 位姿变换增量
+  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) < surroundingkeyframeAddingAngleThreshold &&
+      abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
+      abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
+      sqrt(x * x + y * y + z * z) < surroundingkeyframeAddingDistThreshold)
+    return false;
+  PointTypePose last = keyFrames_.back().Pose();
+
+  return true;
 }
 
 /**
@@ -543,8 +546,7 @@ void addOdomFactor()
         // 变量节点设置初始值
         initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
         gtsam::Pose3 init=trans2gtsamPose(transformTobeMapped);
-        printf("  add 0  init 0 :%f %f %f %f %f %f\n",init.x(),init.y(),init.z(),
-               init.rotation().roll(),init.rotation().pitch(),init.rotation().yaw());
+
     }
     else
     {
@@ -557,11 +559,6 @@ void addOdomFactor()
         gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
         // 变量节点设置初始值
         initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
-      printf(" add between :%d to  %d  init %d:%f %f %f %f %f %f\n",
-             cloudKeyPoses3D->size()-1,cloudKeyPoses3D->size(),cloudKeyPoses3D->size(),
-             poseTo.x(),poseTo.y(),poseTo.z(),
-             poseTo.rotation().roll(),poseTo.rotation().pitch(),poseTo.rotation().yaw());
-
     }
 }
 
@@ -921,40 +918,38 @@ void recontructIKdTree(){
 /**
  * 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
  */
-void correctPoses()
-{
-    if (cloudKeyPoses3D->points.empty())
-        return;
+void correctPoses() {
+  if (cloudKeyPoses3D->points.empty())
+    return;
 
-    if (aLoopIsClosed == true)
-    {
-        // 清空里程计轨迹
-        globalPath.poses.clear();
-        // 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿
-        int numPoses = isamCurrentEstimate.size();
-        for (int i = 0; i < numPoses; ++i)
-        {
-            cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().x();
-            cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().y();
-            cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().z();
-
-            PointTypePose new_pose;
-            new_pose.x = cloudKeyPoses3D->points[i].x;
-            new_pose.y = cloudKeyPoses3D->points[i].y;
-            new_pose.z = cloudKeyPoses3D->points[i].z;
-            new_pose.roll = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().roll();
-            new_pose.pitch = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().pitch();
-            new_pose.yaw = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().yaw();
-            keyFrames_[i].ResetPose(new_pose);
-
-            // 更新里程计轨迹
-            updatePath(keyFrames_[i].Pose());
-        }
-        // 清空局部map， reconstruct  ikdtree submap
-        recontructIKdTree();
-        ROS_INFO("ISMA2 Update");
-        aLoopIsClosed = false;
+  if (aLoopIsClosed == true) {
+    // 清空里程计轨迹
+    globalPath.poses.clear();
+    // 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿
+    int numPoses = isamCurrentEstimate.size();
+    for (int i = 0; i < numPoses; ++i) {
+      cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().x();
+      cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().y();
+      cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().z();
+
+      PointTypePose new_pose = keyFrames_[i].Pose();
+      new_pose.x = cloudKeyPoses3D->points[i].x;
+      new_pose.y = cloudKeyPoses3D->points[i].y;
+      new_pose.z = cloudKeyPoses3D->points[i].z;
+      new_pose.roll = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().roll();
+      new_pose.pitch = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().pitch();
+      new_pose.yaw = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().yaw();
+      keyFrames_[i].ResetPose(new_pose);
+
+      // 更新里程计轨迹
+      updatePath(keyFrames_[i].Pose());
     }
+    printf(" \n");
+    // 清空局部map， reconstruct  ikdtree submap
+    recontructIKdTree();
+    ROS_INFO("ISMA2 Update");
+    aLoopIsClosed = false;
+  }
 }
 
 //回环检测三大要素