cleanup

kiddhong · Oct 14, 2019 · ff7f173 · ff7f173
1 parent 01afb0b
commit ff7f173
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 25 deletions.
diff --git a/LeGO-LOAM/src/imageProjection.cpp b/LeGO-LOAM/src/imageProjection.cpp
@@ -418,26 +418,25 @@ void ImageProjection::labelComponents(int row, int col) {
 }
 
 void ImageProjection::publishClouds() {
-  const auto& cloudHeader = _seg_msg.header;
 
-  sensor_msgs::PointCloud2 laserCloudTemp;
+  sensor_msgs::PointCloud2 temp;
+  temp.header.stamp = _seg_msg.header.stamp;
+  temp.header.frame_id = "base_link";
 
-  auto PublishCloud = [&](ros::Publisher& pub,
+  auto PublishCloud = [](ros::Publisher& pub, sensor_msgs::PointCloud2& temp,
                           const pcl::PointCloud<PointType>::Ptr& cloud) {
     if (pub.getNumSubscribers() != 0) {
-      pcl::toROSMsg(*cloud, laserCloudTemp);
-      laserCloudTemp.header.stamp = cloudHeader.stamp;
-      laserCloudTemp.header.frame_id = "base_link";
-      pub.publish(laserCloudTemp);
+      pcl::toROSMsg(*cloud, temp);
+      pub.publish(temp);
     }
   };
 
-  PublishCloud(_pub_outlier_cloud, _outlier_cloud);
-  PublishCloud(_pub_segmented_cloud, _segmented_cloud);
-  PublishCloud(_pub_full_cloud, _full_cloud);
-  PublishCloud(_pub_ground_cloud, _ground_cloud);
-  PublishCloud(_pub_segmented_cloud_pure, _segmented_cloud_pure);
-  PublishCloud(_pub_full_info_cloud, _full_info_cloud);
+  PublishCloud(_pub_outlier_cloud, temp, _outlier_cloud);
+  PublishCloud(_pub_segmented_cloud, temp, _segmented_cloud);
+  PublishCloud(_pub_full_cloud, temp, _full_cloud);
+  PublishCloud(_pub_ground_cloud, temp, _ground_cloud);
+  PublishCloud(_pub_segmented_cloud_pure, temp, _segmented_cloud_pure);
+  PublishCloud(_pub_full_info_cloud, temp, _full_info_cloud);
 
   if (_pub_segmented_cloud_info.getNumSubscribers() != 0) {
     _pub_segmented_cloud_info.publish(_seg_msg);

diff --git a/LeGO-LOAM/src/mapOptimization.h b/LeGO-LOAM/src/mapOptimization.h
@@ -48,10 +48,6 @@ class MapOptimization {
   gtsam::ISAM2 *isam;
   gtsam::Values isamCurrentEstimate;
 
-  gtsam::noiseModel::Diagonal::shared_ptr priorNoise;
-  gtsam::noiseModel::Diagonal::shared_ptr odometryNoise;
-  gtsam::noiseModel::Diagonal::shared_ptr constraintNoise;
-
   ros::NodeHandle& nh;
   bool _loop_closure_enabled;
   float _scan_period;

diff --git a/LeGO-LOAM/src/mapOptmization.cpp b/LeGO-LOAM/src/mapOptmization.cpp
@@ -192,10 +192,6 @@ void MapOptimization::allocateMemory() {
     transformAftMapped[i] = 0;
   }
 
-  gtsam::Vector Vector6(6);
-  Vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6;
-  priorNoise = noiseModel::Diagonal::Variances(Vector6);
-  odometryNoise = noiseModel::Diagonal::Variances(Vector6);
 
   matA0.setZero();
   matB0.fill(-1);
@@ -730,7 +726,7 @@ void MapOptimization::performLoopClosure() {
   float noiseScore = icp.getFitnessScore();
   Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore,
       noiseScore;
-  constraintNoise = noiseModel::Diagonal::Variances(Vector6);
+  auto constraintNoise = noiseModel::Diagonal::Variances(Vector6);
   /*
           add constraints
           */
@@ -775,9 +771,9 @@ void MapOptimization::extractSurroundingKeyFrames() {
       }
     } else {  // queue is full, pop the oldest key frame and push the latest
               // key frame
-      if (latestFrameID != cloudKeyPoses3D->points.size() -
-                               1) {  // if the robot is not moving, no need to
-                                     // update recent frames
+      if (latestFrameID != cloudKeyPoses3D->points.size()-1) {
+        // if the robot is not moving, no need to
+        // update recent frames
 
         recentCornerCloudKeyFrames.pop_front();
         recentSurfCloudKeyFrames.pop_front();
@@ -1218,6 +1214,11 @@ void MapOptimization::saveKeyFramesAndFactor() {
   currentRobotPosPoint.y = transformAftMapped[4];
   currentRobotPosPoint.z = transformAftMapped[5];
 
+  gtsam::Vector Vector6(6);
+  Vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6;
+  auto priorNoise = noiseModel::Diagonal::Variances(Vector6);
+  auto odometryNoise = noiseModel::Diagonal::Variances(Vector6);
+
   bool saveThisKeyFrame = true;
   if (sqrt((previousRobotPosPoint.x - currentRobotPosPoint.x) *
                (previousRobotPosPoint.x - currentRobotPosPoint.x) +