Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Load map but with no point showed in the viewer windows,just KeyFrames #737

Open
LightInDust opened this issue Mar 27, 2023 · 6 comments
Open

Comments

@LightInDust
Copy link

          Applying the changes from [this fork](https://github.com/dz306271098/ORB_SLAM3) seems to solve my problem. 

Specifically, replace the Map::PreSave function and MapPoint::PreSave function with the following:

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;
    }

}

and

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;
    }
}

Originally posted by @heiwang1997 in #443 (comment)

@LightInDust LightInDust changed the title Applying the changes from [this fork](https://github.com/dz306271098/ORB_SLAM3) seems to solve my problem. Load map but with no point showed in the viewer windows,just KeyFrames Mar 27, 2023
@LightInDust
Copy link
Author

I've replace my replace the Map::PreSave function and MapPoint::PreSave function as #443 showed.And I could save map without seg fault anymore.
But when I load the map in order to use the localization mode,there is no map point in the viewer.
First I guess it just some bugs in viewer.cc and has no influence on the localization mode.However,the localization will show no match and loop in reseting map.

@LightInDust
Copy link
Author

There are part of the message out
`ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
This program comes with ABSOLUTELY NO WARRANTY;
This is free software, and you are welcome to redistribute it
under certain conditions. See LICENSE.txt.

Input sensor was set to: Stereo-Inertial
Loading settings from /home/sp-lab/datasets/RealSense_D435i_Stereo.yaml
-Loaded camera 1
-Loaded camera 2
-Loaded image info
-Loaded IMU calibration
-Loaded ORB settings
-Loaded viewer settings
-Loaded Atlas settings
-Loaded misc parameters

SLAM settings:
-Camera 1 parameters (Pinhole): [ 382.507 382.507 319.629 236.036 ]
): [ 382.507 382.507 319.629 236.036 ]
-Original image size: [ 640 , 480 ]
-Current image size: [ 640 , 480 ]
-Sequence FPS: 30
-Stereo baseline: 0.0498859
-Stereo depth threshold : 40
-Gyro noise: 0.001
-Accelerometer noise: 0.01
-Gyro walk: 1e-06
-Accelerometer walk: 0.0001
-IMU frequency: 250
-Features per image: 1250
-ORB scale factor: 1.2
-ORB number of scales: 8
-Initial FAST threshold: 20
-Min FAST threshold: 7

Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!

Load File
Initialization of Atlas from file: loadmap
Starting to read the save binary file
End to load the save binary file
Creation of new map with id: 1
Creation of new map with last KF id: 1290
Seq. Name:
There are 1 cameras in the atlas
Camera 0 is pinhole
Starting the Viewer
not IMU meas
not IMU meas
not IMU meas
First KF:1290; Map init KF:1290
New Map created with 816 points
seting Localization mode
waiting for LM stopped
Local Mapping STOP
waiting for LM stopped
pCurrentMap->isImuInitialized: 0
pCurrentMap->GetIniertialBA2(): 0
IMU is not or recently initialized. Reseting active map...
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 0
mnInitialFrameId = 0
786 Frames set to lost
First KF:1296; Map init KF:1290
New Map created with 284 points
pCurrentMap->isImuInitialized: 0
pCurrentMap->GetIniertialBA2(): 0
IMU is not or recently initialized. Reseting active map...
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 0
mnInitialFrameId = 6645
0 Frames set to lost
not IMU meas
First KF:1297; Map init KF:1296
New Map created with 300 points
pCurrentMap->isImuInitialized: 0
pCurrentMap->GetIniertialBA2(): 0
IMU is not or recently initialized. Reseting active map...
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 0
mnInitialFrameId = 6678
0 Frames set to lost
not IMU meas
First KF:1298; Map init KF:1297
New Map created with 341 points
pCurrentMap->isImuInitialized: 0
pCurrentMap->GetIniertialBA2(): 0
IMU is not or recently initialized. Reseting active map...
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!`

@LightInDust
Copy link
Author

2023-03-22 21-25-01
2023-03-27 14-48-35屏幕截图

@dionkrith
Copy link

I have the same problem. Is there any solution?

@Sadegh-Kalami
Copy link

I did what the repo said to build orbslam3 but I have no visualization like u have shared with us. would you pls tell me how did you do this?

@LightInDust
Copy link
Author

Actually I did not solve the problem. As I said before,, there are some bugs in viewer.cc and localization mode. I tried in the code in the project:
https://github.com/lturing/ORB_SLAM3_ROS,and it uses the RViz to achieve the visualization of ORB3.When I loaded the map ,RViz would show the point.
However,the loop in the localization mode still have some bugs.It seems that when you enter the localization mode,you will loop in somewhere I can't figure out.
Because I have left the work for almost 4 months, these are what I can do.Hope that someone will solve the problem for us in the future!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants