Bug in laserPosegraphOptimization.cpp?

[PaulKemppi]

In the function loopFindNearKeyframesCloud (line 412), key frames are being combined into a single point cloud to be used as the target in the ICP registration (doICPVirtualRelative).

This is how it is:

*nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[root_idx]);

This is how it should be (I guess):

*nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[keyNear]);

This way, the correct pose for each key frame cloud is used when converting them to the global coordinate frame. The original line results a cluttered point cloud, and thus the ICP registration results often a poor fitness score. Because the best matching key frame with index 'key' (predicted by the scan context matching) is still included in the cluttered cloud, the registration may still result a correct transform. But not always. At least not with my LiDAR data collected with Ouster OS1-32.

[mingloo]

@PaulKemppi I've same doubt with you.

One same issue mentioned in #11 as well. Also, it relates to #7.

@gisbi-kim Could you please help double confirm this issue.

[huawei-sai]

Yes, I confirm this - I faced the same issue and changing the root_idx to keyNear solved the issue! @mingloo @PaulKemppi @gisbi-kim

[YZH-bot]

Yes, I also think it should be like this one:

*nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[keyNear]);