saving/loading maps about orb_slam HOT 14 CLOSED

@jingpang, Hello. I have worked on saving data, i.e., map points, and I believe I made some progress on ORB_SLAM version 2 (https://github.com/raulmur/ORB_SLAM2). :-)
I made some change to src/System.cc, include/System.h, and Example/ROS/ORB_SLAM2/ros_mono.cc.

For System.cc, I added the following code:

void System::SaveMapPoints(const string &filename) {
    cout << endl << "Saving map points to " << filename << " ..." << endl;

    vector<MapPoint*> vpMPs = mpMap->GetAllMapPoints();

    // Transform all keyframes so that the first keyframe is at the origin.
    // After a loop closure the first keyframe might not be at the origin.
    ofstream f;
    f.open(filename.c_str());
    f << fixed;

    for(size_t i=0; i<vpMPs.size(); i++) {
        MapPoint* pMP = vpMPs[i];

        if(pMP->isBad())
            continue;

        cv::Mat MPPositions = pMP->GetWorldPos();

        f << setprecision(7) << " " << MPPositions.at<float>(0) << " " << MPPositions.at<float>(1) << " " << MPPositions.at<float>(2) << endl;
    }

    f.close();
    cout << endl << "Map Points saved!" << endl;

}

For System.h, I added the following code:

void SaveMapPoints(const string &filename);

For ros_mono.cc, I added the following code:

SLAM.SaveMapPoints("MapPointsSave.txt");

The MapPointsSave.txt contains all the map points in terms of global locations, i.e., xyz coordinate.
Thus I converted the saved map points in .txt file into .pcd file (http://pointclouds.org/documentation/tutorials/writing_pcd.php), which I could convert into .ply file to view in Meshlab.
As I viewed in Meshlab, I could see that the saved map points are similar to the ones constructed in ORB_SLAM2 view.

For saving the keyframe trajectory, I believe authors of ORB_SLAM2 already built the functions!

Sincerely,

from orb_slam.

@jingpang

Hi. Thanks for your save and load functions. Excellent work!!!

I have found the bugs and fixed them. After the system reloaded the saved world, the program has a very large chance to stop since a deadlock happens.

The system keeps a tree to store keyframes. Each node in the tree has a parent node and several children. Sometimes a node may add its parent to the list of children or select one of its children as the parent. To fix the problem, it's necessary to check the pointers (or ID) of keyframes and avoid those troublesome situations.

Here are the modifications to fix the bug.
In the file: https://github.com/jingpang/ORBSLAM_jpMiniPC/blob/master/src/KeyFrame.cc

1 Add two lines in the function AddChild

void KeyFrame::AddChild(KeyFrame *pKF)
{
    if ((void *)this == (void *)pKF || mspChildrens.count(pKF)>0)
        return;
    boost::mutex::scoped_lock lockCon(mMutexConnections);
    mspChildrens.insert(pKF);
}

2 Add one line before line 581 in the function SetBadFlag like this

        if((void *)mpParent!=(void *)this)
            mpParent->EraseChild(this);
        mbBad = true;

Sincerely

from orb_slam.

We have not implemented that functionality yet.

from orb_slam.

@NikolausDemmel Have you ever tried to save and load the map?
I tried to save the information (of keyframes and mappoints) which I thought useful in files, and relocalized for tracking successfully after reloading the files.
But the program stopped later and I can't find the reason.

@raulmur And could you please tell me if i'd like to save and reload the world, what should I pay attention to? (For the moment i save almost all the data of good KeyFrames and good MapPoints)
Thank you!

from orb_slam.

No, i haven't investigated further.

from orb_slam.

No, i haven't investigated further.

from orb_slam.

@jingpang Hello. Would it be possible to share the code for saving and loading the map (keyframes and mappoints)? Thank you!

from orb_slam.

@dk683 Hello, i added a file to save and load the data, but there're some unknown bugs. After i reload the data, it may crash and i haven't found why.
The file link: https://github.com/jingpang/ORBSLAM_jpMiniPC/blob/master/include/SaveLoadWorld.h
(Tried text / binary file, text part commented. a little different between them. )

If you can make it to reuse the map, please offer some help and i'll really appreciate it.

from orb_slam.

Thank you for sharing the code with me! Yes, I will debug your code and if I found a reason for the bug, I will post here right away! Thank you.

from orb_slam.

Thanks! @dk683

from orb_slam.

@WangXiaoLongNWPU
Thanks!
I'll try your suggestions.
And i also send an email to [email protected] .
Looking forward to more discussion.

Thanks again.

from orb_slam.

@jingpang
Hi. Thanks for your save and load functions. Excellent work!!!

I tried to make it work on ORB_SLAM version 2, but I get this error:
scnt,pKFi->GetMapPoints.size,nMPcnt = 436,432,436scnt!=pKFi->GetMapPoints().size(), shouldn't

code line 1496 in SaveLoadWorld.h

if(scnt!=pKFi->GetMapPoints().size() || scnt!=nMPcnt)
cerr<<"scnt,pKFi->GetMapPoints.size,nMPcnt = "<<scnt<<","<GetMapPoints().size()<<","<<nMPcnt<<"scnt!=pKFi->GetMapPoints().size(), shouldn't"<<endl;
}

Please offer some help and I'll really appreciate it.

from orb_slam.

@ygx2011
Sorry, I'm not so sure. I've just tested it on ORBSLAM1.
It seems that mvpMappoint has changed? (I didn't add mutex so the vector may be modified?)
Have all the other threads (LocalMapping&LoopClosing&GlobalBundleAdjustment) stopped when you try to save the data?

Perhaps you need debug it...

PS. Someone added save/load function for ORBSLAM2 (also find it in the issues of ORB2). You can try it. (the link below, or maybe other issues)
raulmur/ORB_SLAM2#19

from orb_slam.

Hello,
Dear @ghost , @jingpang, @WangXiaoLongNWPU, @ygx2011
I have performed the changes in ORBSLAM2 (I have made some change to src/System.cc, include/System.h, and Example/ROS/ORB_SLAM2/ros_mono.cc.).
But I could not save a map and load data. How can I save and load data? Do I need to SaveLoadWorld.h ? I do not it. I need to SaveLoadWorld.h. Please share this code, completely.

Please offer help.
Thanks.

from orb_slam.