minor bug fix

Tconcept · Dec 9, 2020 · a53730e · a53730e
1 parent 18ebdbe
commit a53730e
Showing 1 changed file with 6 additions and 4 deletions.
diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp
@@ -128,7 +128,7 @@ class mapOptimization : public ParamServer
     std::mutex mtxLoopInfo;
 
     bool isDegenerate = false;
-    Eigen::Matrix<float, 6, 6> matP;
+    cv::Mat matP;
 
     int laserCloudCornerFromMapDSNum = 0;
     int laserCloudSurfFromMapDSNum = 0;
@@ -222,7 +222,7 @@ class mapOptimization : public ParamServer
             transformTobeMapped[i] = 0;
         }
 
-        matP.setZero();
+        matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
     }
 
     void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
@@ -671,6 +671,9 @@ class mapOptimization : public ParamServer
 
     void visualizeLoopClosure()
     {
+        if (loopIndexContainer.empty())
+            return;
+
         visualization_msgs::MarkerArray markerArray;
         // loop nodes
         visualization_msgs::Marker markerNode;
@@ -693,7 +696,7 @@ class mapOptimization : public ParamServer
         markerEdge.ns = "loop_edges";
         markerEdge.id = 1;
         markerEdge.pose.orientation.w = 1;
-        markerEdge.scale.x = 0.1; markerEdge.scale.y = 0.1; markerEdge.scale.z = 0.1;
+        markerEdge.scale.x = 0.1;
         markerEdge.color.r = 0.9; markerEdge.color.g = 0.9; markerEdge.color.b = 0;
         markerEdge.color.a = 1;
 
@@ -1126,7 +1129,6 @@ class mapOptimization : public ParamServer
         cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
         cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
         cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
-        cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));
 
         PointType pointOri, coeff;