Update laserPosegraphOptimization.cpp

error fixed in loopFindNearKeyframesCloud

Author: Giseop Kim

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;
-}
+}