小的修改

[PixelVoyager-30]

if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) {
            std::cout << "ICP fitness test failed (" << icp.getFitnessScore() << " > " << historyKeyframeFitnessScore << "). Reject this SC loop." << std::endl;
            return;
        } else {
            std::cout << "ICP fitness test passed (" << icp.getFitnessScore() << " < " << historyKeyframeFitnessScore << "). Add this SC loop." << std::endl;
 }

当我遇到重定位失败的时候终端卡在Reject this SC loop，也是看完线程代码才知道，可以通过rviz提供初始位姿使程序继续执行。
建议注释掉这里的return，否则当sc方法得到的初始位姿较差时，终端不会提示通过rviz重新提供初始位姿。

[shallowlife]

注释掉这个return就可以实现，SC不成功的时候，提示通过rviz重新提供初始位姿吗？这个暂时我没机会测试，如果可以的话为就修改这个部分