解决 orbslam3 保存地图时概率性崩溃保存失败的问题，段错误（核心已转储）Segmentation fault(core dumped)

我才是一卓

4364人浏览 · 2023-05-31 19:06:46

我才是一卓 · 2023-05-31 19:06:46 发布

1、问题

orbslam3 新增了地图保存、加载复用的功能。可以对环境慢慢建图，后续任务直接使用旧地图定位减少算力消耗。
- ORB_SLAM3地图保存与加载：https://blog.csdn.net/weixin_44675820/article/details/125076927
但是保存地图时高概率出现 段错误（核心已转储）Segmentation fault(core dumped) 问题，导致辛辛苦苦建的图直接丢失。

2、排查

通过 gdb 调试，输出堆栈信息如下：
- gdb 带参数调试可执行文件程序：https://blog.csdn.net/weixin_43667077/article/details/130974021

Thread 1 "Mono" received signal SIGSEGV, Segmentation fault.
__GI___pthread_mutex_lock (mutex=0x707070707070957) at ../nptl/pthread_mutex_lock.c:67
67	../nptl/pthread_mutex_lock.c: 没有那个文件或目录.
(gdb) bt
#0  0x00007ffff37ecfd0 in __GI___pthread_mutex_lock (mutex=0x707070707070957)
    at ../nptl/pthread_mutex_lock.c:67
#1  0x00007ffff668580d in void std::lock<std::unique_lock<std::mutex>, std::unique_lock<std::mutex>>(std::unique_lock<std::mutex>&, std::unique_lock<std::mutex>&) ()
    at /home/nav/catkin_ws/src/orbslam3/orbslam3/../lib/libORB_SLAM3.so
#2  0x00007ffff668174f in ORB_SLAM3::MapPoint::isBad() ()
    at /home/nav/catkin_ws/src/orbslam3/orbslam3/../lib/libORB_SLAM3.so
#3  0x00007ffff6697f57 in ORB_SLAM3::Map::PreSave(std::set<ORB_SLAM3::GeometricCamera*, std::less<ORB_SLAM3::GeometricCamera*>, std::allocator<ORB_SLAM3::GeometricCamera*> >&) ()
    at /home/nav/catkin_ws/src/orbslam3/orbslam3/../lib/libORB_SLAM3.so
#4  0x00007ffff66951f5 in ORB_SLAM3::Atlas::PreSave() ()
    at /home/nav/catkin_ws/src/orbslam3/orbslam3/../lib/libORB_SLAM3.so
#5  0x00007ffff65f54f2 in ORB_SLAM3::System::SaveAtlas(int, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) ()
    at /home/nav/catkin_ws/src/orbslam3/orbslam3/../lib/libORB_SLAM3.so
#6  0x0000555555567ebf in main ()

可以大概看出是 System::SaveAtlas() 这个保存地图过程的中，在 MapPoint::isBad() 函数里发生了互斥锁（pthread_mutex_lock）导致的错误。
进一步分析配合 github 上开发者们的讨论，个人理解问题根因是Map::PreSave 和 MapPoint::PreSave 函数中的 EraseObservation 操作会在遍历时删除元素，导致崩溃。
- github 讨论：https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/443

3、解决

个人理解的大致解决思路是在Map::PreSave 和 MapPoint::PreSave 函数 EraseObservation 操作之前，把 mspMapPoints 备份在临时 set 里，然后遍历临时 set，对真实 set 进行 EraseObservation 操作。
根据 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/443#issuecomment-1154814254 的解决方案，替换 Map.cc 的 Map::PreSave 和 MapPoint.cc 的 MapPoint::PreSave 函数即可。
搬运过来的、不会崩溃的函数内容如下:

void Map::PreSave(std::set<GeometricCamera*> &spCams)
{
    int nMPWithoutObs = 0;

    std::set<MapPoint*> tmp_mspMapPoints1;
    tmp_mspMapPoints1.insert(mspMapPoints.begin(), mspMapPoints.end());

    for(MapPoint* pMPi : tmp_mspMapPoints1)
    {
        if(!pMPi || pMPi->isBad())
            continue;

        if(pMPi->GetObservations().size() == 0)
        {
            nMPWithoutObs++;
        }
        map<KeyFrame*, std::tuple<int,int>> mpObs = pMPi->GetObservations();
        for(map<KeyFrame*, std::tuple<int,int>>::iterator it= mpObs.begin(), end=mpObs.end(); it!=end; ++it)
        {
            if(it->first->GetMap() != this || it->first->isBad())
            {
                pMPi->EraseObservation(it->first);
            }

        }
    }

    // Saves the id of KF origins
    mvBackupKeyFrameOriginsId.clear();
    mvBackupKeyFrameOriginsId.reserve(mvpKeyFrameOrigins.size());
    for(int i = 0, numEl = mvpKeyFrameOrigins.size(); i < numEl; ++i)
    {
        mvBackupKeyFrameOriginsId.push_back(mvpKeyFrameOrigins[i]->mnId);
    }


    // Backup of MapPoints
    mvpBackupMapPoints.clear();

    std::set<MapPoint*> tmp_mspMapPoints2;
    tmp_mspMapPoints2.insert(mspMapPoints.begin(), mspMapPoints.end());

    for(MapPoint* pMPi : tmp_mspMapPoints2)
    {
        if(!pMPi || pMPi->isBad())
            continue;

        mvpBackupMapPoints.push_back(pMPi);
        pMPi->PreSave(mspKeyFrames,mspMapPoints);
    }

    // Backup of KeyFrames
    mvpBackupKeyFrames.clear();
    for(KeyFrame* pKFi : mspKeyFrames)
    {
        if(!pKFi || pKFi->isBad())
            continue;

        mvpBackupKeyFrames.push_back(pKFi);
        pKFi->PreSave(mspKeyFrames,mspMapPoints, spCams);
    }

    mnBackupKFinitialID = -1;
    if(mpKFinitial)
    {
        mnBackupKFinitialID = mpKFinitial->mnId;
    }

    mnBackupKFlowerID = -1;
    if(mpKFlowerID)
    {
        mnBackupKFlowerID = mpKFlowerID->mnId;
    }

}

void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
{
    mBackupReplacedId = -1;
    if(mpReplaced && spMP.find(mpReplaced) != spMP.end())
        mBackupReplacedId = mpReplaced->mnId;

    mBackupObservationsId1.clear();
    mBackupObservationsId2.clear();
    // Save the id and position in each KF who view it
    std::map<KeyFrame*,std::tuple<int,int> > tmp_mObservations;
    tmp_mObservations.insert(mObservations.begin(), mObservations.end());

    for(std::map<KeyFrame*,std::tuple<int,int> >::const_iterator it = tmp_mObservations.begin(), end = tmp_mObservations.end(); it != end; ++it)
    {
        KeyFrame* pKFi = it->first;
        if(spKF.find(pKFi) != spKF.end())
        {
            mBackupObservationsId1[it->first->mnId] = get<0>(it->second);
            mBackupObservationsId2[it->first->mnId] = get<1>(it->second);
        }
        else
        {
            EraseObservation(pKFi);
        }
    }

    // Save the id of the reference KF
    if(spKF.find(mpRefKF) != spKF.end())
    {
        mBackupRefKFId = mpRefKF->mnId;
    }
}