From 6bc5e3b15ba5752d59cec1a04cf3ca08c97e440c Mon Sep 17 00:00:00 2001
From: huawei-sai <120198289+huawei-sai@users.noreply.github.com>
Date: Mon, 30 Jan 2023 15:57:57 +0100
Subject: [PATCH 1/2] Bug in creating reference pointcloud for ICP registration

This bug has been already highlighted and well explained in these two links.
https://github.com/gisbi-kim/SC-A-LOAM/issues/11
https://github.com/gisbi-kim/SC-A-LOAM/issues/12
To check this - one can visualize the loop submap in RVIZ and can see the cluttered point cloud as it does not have correct registration.
---
 src/laserPosegraphOptimization.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/laserPosegraphOptimization.cpp b/src/laserPosegraphOptimization.cpp
index ce1a896..95765ca 100644
--- a/src/laserPosegraphOptimization.cpp
+++ b/src/laserPosegraphOptimization.cpp
@@ -478,7 +478,7 @@ void loopFindNearKeyframesCloud( pcl::PointCloud<PointType>::Ptr& nearKeyframes,
             continue;
 
         mKF.lock(); 
-        *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[root_idx]);
+        *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[keyNear]);
         mKF.unlock(); 
     }
 

From 95b6b1cc3943fa360d4e9c07cf0689deab359171 Mon Sep 17 00:00:00 2001
From: huawei-sai <120198289+huawei-sai@users.noreply.github.com>
Date: Mon, 30 Jan 2023 18:47:00 +0100
Subject: [PATCH 2/2] change the loopFindNearKeyframesCloud function to be more
 clear

The function is also changed in the author's latest code base
https://github.com/gisbi-kim/lt-mapper/blob/bce4236d97bf0d9d97187f9355380e981ac7e499/ltslam/src/LTslam.cpp#L201

Basically loopFindNearKeyframesCloud function does not require the fourth input parameter that it initially had - it just creates more confusion and it never is used. Also while transforming the pointcloud to global coordinate frame (origin) and to accumulate surrounding frames, one does not need the fourth input parameter. Changed the function accordingly.
---
 src/laserPosegraphOptimization.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/laserPosegraphOptimization.cpp b/src/laserPosegraphOptimization.cpp
index 95765ca..c02a7a8 100644
--- a/src/laserPosegraphOptimization.cpp
+++ b/src/laserPosegraphOptimization.cpp
@@ -468,7 +468,7 @@ pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::
     return cloudOut;
 } // transformPointCloud
 
-void loopFindNearKeyframesCloud( pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& submap_size, const int& root_idx)
+void loopFindNearKeyframesCloud( pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& submap_size)
 {
     // extract and stacking near keyframes (in global coord)
     nearKeyframes->clear();
@@ -499,8 +499,8 @@ std::optional<gtsam::Pose3> doICPVirtualRelative( int _loop_kf_idx, int _curr_kf
     int historyKeyframeSearchNum = 25; // enough. ex. [-25, 25] covers submap length of 50x1 = 50m if every kf gap is 1m
     pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
     pcl::PointCloud<PointType>::Ptr targetKeyframeCloud(new pcl::PointCloud<PointType>());
-    loopFindNearKeyframesCloud(cureKeyframeCloud, _curr_kf_idx, 0, _loop_kf_idx); // use same root of loop kf idx 
-    loopFindNearKeyframesCloud(targetKeyframeCloud, _loop_kf_idx, historyKeyframeSearchNum, _loop_kf_idx); 
+    loopFindNearKeyframesCloud(cureKeyframeCloud, _curr_kf_idx, 0); // use same root of loop kf idx 
+    loopFindNearKeyframesCloud(targetKeyframeCloud, _loop_kf_idx, historyKeyframeSearchNum); 
 
     // loop verification 
     sensor_msgs::PointCloud2 cureKeyframeCloudMsg;