Update laserPosegraphOptimization.cpp

error fixed in loopFindNearKeyframesCloud
gisbi-kim · Sep 23, 2021 · 7ee4ac9 · 7ee4ac9
1 parent a8b724a
commit 7ee4ac9
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/SC-PGO/src/laserPosegraphOptimization.cpp b/SC-PGO/src/laserPosegraphOptimization.cpp
@@ -404,12 +404,12 @@ void loopFindNearKeyframesCloud( pcl::PointCloud<PointType>::Ptr& nearKeyframes,
     // extract and stacking near keyframes (in global coord)
     nearKeyframes->clear();
     for (int i = -submap_size; i <= submap_size; ++i) {
-        int keyNear = root_idx + i;
+        int keyNear = key + i;
         if (keyNear < 0 || keyNear >= int(keyframeLaserClouds.size()) )
             continue;
 
         mKF.lock(); 
-        *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[keyNear]);
+        *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[root_idx]);
         mKF.unlock(); 
     }
 
@@ -828,4 +828,4 @@ int main(int argc, char **argv)
  	ros::spin();
 
 	return 0;
-}
+}