gisbi-kim / SC-A-LOAM

Robust LiDAR SLAM with a versatile plug-and-play loop closing and pose-graph optimization.
438 stars 95 forks source link

Question about "loopFindNearKeyframesCloud(cureKeyframeCloud, _curr_kf_idx, 0, _loop_kf_idx) ? #11

Closed Tina1994 closed 2 years ago

Tina1994 commented 2 years ago

Hi, thanks for your contribution. I have same question same as the question https://github.com/gisbi-kim/SC-A-LOAM/issues/7 However, I think the code still have something wrong, the code in the repo is as:

 void loopFindNearKeyframesCloud( pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& submap_size, const int& root_idx)
{
    // extract and stacking near keyframes (in global coord)
    nearKeyframes->clear();
    for (int i = -submap_size; i <= submap_size; ++i) {
        int keyNear = key + i; 
        if (keyNear < 0 || keyNear >= int(keyframeLaserClouds.size()) )
            continue;

        mKF.lock(); 
        *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[root_idx]);
        mKF.unlock(); 
    }
    if (nearKeyframes->empty())
        return;
    pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
    downSizeFilterICP.setInputCloud(nearKeyframes);
    downSizeFilterICP.filter(*cloud_temp);
    *nearKeyframes = *cloud_temp;
} // loopFindNearKeyframesCloud

where nearKeyframes += local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[root_idx]) should be:

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

The target of the function “loopFindNearKeyframesCloud ” is transforming the nearby keyframe point cloud stored in keyframeLaserClouds from thr Lidar coordinate to the world coordinate, to form a submap in _loop_kf_idx, so I think local2global should be the tranformation stored in keyframePosesUpdated[keyNear].

微信图片_20220309111635

harveybia commented 2 years ago

Can confirm this is correct.

Tina1994 commented 2 years ago

Can confirm this is correct.

Thanks for you reply, are you mean my explain is correct? I am a beginner, so if you mean the source code is correct, please explain the reason. Thanks again.

harveybia commented 2 years ago

Sorry for not being clear, you are correct. The keyframes need to be transformed into the same global frame. *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[keyNear]);

Thanks for you reply, are you mean my explain is correct? I am a beginner, so if you mean the source code is correct, please explain the reason. Thanks again.

Tina1994 commented 2 years ago

Sorry for not being clear, you are correct. The keyframes need to be transformed into the same global frame. *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[keyNear]);

Thanks for you reply, are you mean my explain is correct? I am a beginner, so if you mean the source code is correct, please explain the reason. Thanks again.

Thanks you very much! I 'll modify the error in downloaded source code and close this question.