Map save load and bin vocabulary

[Alkaid-Benetnash]

Binary vocabulary format is much faster than text. And I am sorry that I can't find out who really contribute related code.
implement #19 with the help of boost serialization library.

[xinyutan17]

I needed this functionality and your code worked like a charm.

There's a couple simple bugs that need to be fixed when you build without -DUSE_MAP_SAVE_LOAD=1.

Also it would be great if you changed your documentation slightly:
The path of mapfile is set in yaml setting file: Map.mapfile: map.bin. See the last few line of Example/settings_with_map.yaml.

Since "Examples/settings_with_map.yaml" didn't exist in the pull request, I was a little confused and had to look into the code to see what it referred to. Perhaps a better way to document the above is:
The path of mapfile can be set by adding Map.mapfile: map.bin to ORB_SLAM2's settings file.

[Alkaid-Benetnash]

@xinyutan17
Thanks for your suggestion. I have changed the conditional compilation and updated the readme.

[xinyutan17]

I'm having trouble with the MapSave function resulting in segfault. I'm fairly certain it's due to multi-threading issues because of not locking the mutex for the "mutex needed variables" during serialization in KeyFrame.cc. Is there a reason why you did not lock the mutex? Otherwise I will try to implement that soon and see whether or not it works.

Also lines 420-425 in MapPoint.cc may be causing a segfault since you initialize mpReplaced to a NULL pointer which will be serialized on save, deserialized on load, and then referenced by the algorithm:

MapPoint::MapPoint():
    nObs(0), mnTrackReferenceForFrame(0),
    mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
    mnCorrectedReference(0), mnBAGlobalForKF(0),mnVisible(1), mnFound(1), mbBad(false),
    mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0)
{}

[Alkaid-Benetnash]

@xinyutan17
I don't lock the mutex because I think that map saving is the last procedure of System::ShutDown and all other threads are finished at that time(guaranteed by other parts of ShutDown).
You can try to lock the mutex, thank you for improving this functionality.

As for the NULL initialized MapPoint:
In my opinion, the default constructor will only be used when boost_serialization loads a MapPoint. And after invoking the default constructor, MapPoint::serialize will load the real value of every class member from saved mapfile. In other words, I define the default constructor only because boost needs one, and I follow the way used in origin non-default constructors to initialize class members.

[xinyutan17]

Okay, thanks! Your explanations for both the mutex and the NULL makes sense.

I was experiencing issues with saving a map that was too large (because the saving operation wouldn't complete before the process was killed by ROS), so I created a SaveMap button in the Pangolin viewer to save the map at any time. This probably caused my issues with the mutexes.

[Alkaid-Benetnash]

mutex is locked now, will it solve your problems?
About the problem of memory usage:
Do you mean it consumes so much memory in the map saving procedure that it always triggers OOM and killed by ROS?
I am curious about that because I think ORB_SLAM2 does occupy too much unnecessary memory because bad keyframes and mappoints aren't deleted from memory. And, the map saving is an easy way to find out how the bad objects are related with other good objects. But it might be a bug if only the map saving procedure triggers OOM.

[xinyutan17]

Yes mutex locking does solve the segfault problem. (I implemented it in a similar fashion the rest of the code does it using for example unique_lock<mutex> lock(mMutexFeatures);.)

It doesn't trigger out of memory. I believe ROS first sends a sigterm, and if the node is taking too long to exit (in this case because it's saving a 400 MB map file), ROS sends a sigkill and I end up with a corrupted map.bin file.

[bmende]

Hi @Alkaid-Benetnash. This code is pretty awesome and does most of what I want! Kudos! However, I am having a problem with segfaults with a loaded map. It seems that if I am using an already created map with a new video, I sometimes get a segfault. What is interesting is that the segfault either happens almost right away, or it happens at the place where it would be able to relocalize into the old map. Any thoughts? Have you encountered this?

[Alkaid-Benetnash]

@bmende I am happy that this code can help you. This code indeed crashes at certain circumstances sometimes. I didn't port later changes to this pull request branch. Can you try the master branch of my fork?
If problem still exists, can you help me reproduce the segfaults? (video sequences, mapfile will be helpful).
I only tested the master branch with several simple cases in TUM dataset and I didn't encounter such problems.
If I have time, I will update this pull request to the master branch later.

[yuyou]

Thanks @Alkaid-Benetnash for the map saving/load feature and it works nicely. Just one question about the Frame::nNextId. After loading the map, the value of "Frame::nNextId" is zero. Should It be the largest value of the KeyFrame's mnFrameId?

In the end of System::LoadMap, I did something like this. Please correct me if it is wrong.

    long unsigned int mnFrameId = 0;
    for (auto it:vpKFS) {

        it->SetORBvocabulary(mpVocabulary);
        it->ComputeBoW();
        if (it->mnFrameId > mnFrameId)
          mnFrameId = it->mnFrameId;
    }
    Frame::nNextId += mnFrameId;

[Alkaid-Benetnash]

@yuyou
Thank you, I am totally convinced. I already add your code to my fork's master branch and rebase patches for this pull request again.

[bmende]

@Alkaid-Benetnash I found the segfault! and how to fix it! In Tracking.cc, during the relocalization function (starts on line 1343), we set the mCurrentFrame.mTcw to an estimated pose and then try to refine it. But if we can't refine it enough, then we exit the relocalization function without emptying the mTcw pose matrix on the current frame. Then, later in Track(..) we try to save the the current pose in order to reconstruct the trajectory later. In order to do this we check if the current frame has a pose, and since it is not empty because of the checks during reloc, we try to add a non-existent reference keyframe. So the solution is in relocalization to set the mTcw matrix back to empty before exiting reloc if we fail to relocalize.

line: 1494 in Tracking.cc

if(!bMatch)
{
        mCurrentFrame.mTcw = cv::Mat::zeros(0, 0, CV_32F); // this prevents a segfault later
        return false;
}

[finphin]

@xinyutan17
I also have trouble with the SaveMap function resulting in segfault. I use ZED stereo camera to record image dataset. Our stereo image number is 10323 with 720p size. I check if we use the image less than 9000, the map will be generated well about 852MB, but with the whole, it will cause segfault in SaveMap function. I have check my PC memory is fine when segfault happened. Also, boost_serialization library has been updated to 1.65. I think it maybe MapPoint and KeyFrame too large in this case.

I have check my source code, the git log contains the Alkaid patch as follows,
commit 8c83a5c
Author: Alkaid zgf574564920@gmail.com
Date: Wed Jul 26 02:15:44 2017 +0800
lock mutex when serializing KeyFrames && update examples
System::SaveMap is supposed to work at any time, so locking mutex is necessa
serializing shared class members.
use mono_tum as the map save/load example. update readme

So does this patch fix your segfault already or I have the other segfault case?
Any thoughts? Have you encountered this also?

---

@Alkaid-Benetnash
I have git pull your code with the last git commit which is
commit e443ba5
Author: Alkaid zgf574564920@gmail.com
Date: Mon Aug 28 16:42:37 2017 +0800
remove conditional compilation of map save/load functionality

Is the segfault in SaveMap function solved or it still happens in some case?My problem still exists, and 100% can be reproduced the segfaults. The files link (video sequences, ZED_stereo_video.yaml, modified stereo_kitti.cc) are listed as below,

Google Drive - ORB_SLAM
https://drive.google.com/drive/folders/0B0_EwL-GwTcCb1REOThoMUlBV2s?usp=sharing

Could you do me a favor to check it? I really appreciate your help.

---

Here is the backtrace when segfault happened.
Program received signal SIGSEGV, Segmentation fault.
0x00007ffff7b0ece2 in void boost::serialization::saveboost::archive::binary_oarchive(boost::archive::binary_oarchive&, cv::Mat const&, unsigned int) ()
from /home/sam/ORB_SLAM2/lib/libORB_SLAM2.so
(gdb) bt -10
#38190 0x00007ffff51f5dfd in boost::archive::detail::basic_oarchive_impl::save_pointer(boost::archive::detail::basic_oarchive&, void const*, boost::archive::detail::basic_pointer_oserializer const*) () from /usr/local/lib/libboost_serialization.so.1.65.1
#38191 0x00007ffff7b2a40d in boost::archive::detail::oserializer<boost::archive::binary_oarchive, std::set<ORB_SLAM2::MapPoint*, std::less<ORB_SLAM2::MapPoint*>, std::allocator<ORB_SLAM2::MapPoint*> > >::save_object_data(boost::archive::detail::basic_oarchive&, void const*) const () from /home/sam/ORB_SLAM2/lib/libORB_SLAM2.so
#38192 0x00007ffff51f515d in boost::archive::detail::basic_oarchive::save_object(void const*, boost::archive::detail::basic_oserializer const&) ()
from /usr/local/lib/libboost_serialization.so.1.65.1
#38193 0x00007ffff7b2abd5 in void ORB_SLAM2::Map::serializeboost::archive::binary_oarchive(boost::archive::binary_oarchive&, unsigned int) ()
from /home/sam/ORB_SLAM2/lib/libORB_SLAM2.so
#38194 0x00007ffff7ac4ae9 in boost::archive::detail::oserializer<boost::archive::binary_oarchive, ORB_SLAM2::Map>::save_object_data(boost::archive::detail::basic_oarchive&, void const*) const () from /home/sam/ORB_SLAM2/lib/libORB_SLAM2.so
#38195 0x00007ffff51f515d in boost::archive::detail::basic_oarchive::save_object(void const*, boost::archive::detail::basic_oserializer const&) ()
from /usr/local/lib/libboost_serialization.so.1.65.1
#38196 0x00007ffff51f5dfd in boost::archive::detail::basic_oarchive_impl::save_pointer(boost::archive::detail::basic_oarchive&, void const*, boost::archive::detail::basic_pointer_oserializer const*) () from /usr/local/lib/libboost_serialization.so.1.65.1
#38197 0x00007ffff7ac23ae in ORB_SLAM2::System::SaveMap(std::string const&) () from /home/sam/ORB_SLAM2/lib/libORB_SLAM2.so
#38198 0x00007ffff7ac281c in ORB_SLAM2::System::Shutdown() () from /home/sam/ORB_SLAM2/lib/libORB_SLAM2.so
#38199 0x0000000000403cc2 in main ()

[necusjz]

@Alkaid-Benetnash
When I Ctrl+C the ORBSLAM2, it will output:
QObject::~QObject: Timers cannot be stopped from another thread
And there is no “map.bin“ created, how could I solve the problem?
Thank you~

[Alkaid-Benetnash]

@finphin
According to your backtrace, the segfault seems to happen in boost lib, whose correctness I don't doubt for now. I notice that the call stack is too deep. Can you try to enlarge your max stack size to eliminate the possibility of "stack overflow"? I think this post maybe helpful.
It is a great help that you can reproduce this segfault, but your dataset is too large (29G) and it may take days for me to get the data from google drive in china mainland. Please email me if you are capable to provide other methods (e.g. direct rsync/http/ftp from your server), it could be a huge acceleration.

And I want to comment on the "map size", I found that many unused MapPoint and KeyFrame (e.g. marked with mbBad) resist in memory and will also be saved into mapfile. I tried to figure out what kind of MapPoint and KeyFrame can be deleted safely but failed. This problem is more significant when considering semi-dense map reconstruction(dense image is attached to KeyFrame). I have surveyed @ayushgaud 's implementation and the memory leak makes it difficult to run on a common laptop, not to say saving the map.

[finphin]

@Alkaid-Benetnash
Increase the stack size above 16MB(Linux default:8MB) indeed solved the segfault issue. I have tried the code modified or direct linux command, and both works. Since the issue solved, do you still need the dataset for further analysis?
The map info is listed below for your reference:
Input: 10323 image with 720p size
Output: 1.15GB map with KFs:3414, MPs:260500

Besides, due to the stack size issue is caused by deep call stack, may I know the related between KFs/MPs and stack size? I have to estimate the stack size if map grows bigger.

[Alkaid-Benetnash]

@finphin

Since the issue solved, do you still need the dataset for further analysis?

Don't need the dataset anymore.

may I know the related between KFs/MPs and stack size?

Sorry, I can't estimate a sufficient stack size, either. I think there are two reasons for the deep call stack:

1. unnecessary KFs and MPs complicate the data structure to be saved.
2. I just use generalized serialization library, which may introduce extra overhead.

[skariel]

@bmende still getting a segfault after loading map when pushing new images... im researching this issue

[rpfly3]

@Alkaid-Benetnash You mentioned other threads are guaranteed to be closed. But this is not the case for me. The BA threads generated by LoopClosing are not closed for some cases. The system will get stuck at BA forever. I am not sure about the exact reason yet. If I break out from waiting BA finish, the map cannot be saved. Does anyone have experience on this problem? ?

[Alkaid-Benetnash]

@rpfly3 do you invoke the System::SaveMap() at arbitrary place in the code or invoke it through System::ShutDown() ?

[rpfly3]

@Alkaid-Benetnash I invoked savemap after waiting other threads to shutdown in System::ShutDown. I read the loop closing code. I found the main reasons are BA threads are not well managed. These threads are just left there. There could be BA threads and they probably always run without finish. But I have no idea on how to solve it yet.

[ksivakumar]

@Alkaid-Benetnash @finphin I am getting a similar error where I am able to load maps that are smaller but get errors with bigger maps. I tried increasing the stack size in my ros_stereo.cc file right after I initialize and start ROS, but this did not work. Here is the error I am getting:

Load Map Requested
{INFO}mspMapPoints size = 34520
*** Error in `/home/tstone10/orb_slam2/Examples/ROS/ORB_SLAM2/Stereo': malloc(): memory corruption: 0x0000000010dc7b40 ***

Any help or guidance on this would be very much appreciated, thank you in advance!

[Alkaid-Benetnash]

@ksivakumar Have you solved your problems? Can you use the getrlimit function in sys/resource.h to check how large your stack is?

[rpfly3]

@Alkaid-Benetnash I create a new pull request solving the bugs. Plz checkout my PR: #638

[Alkaid-Benetnash]

@Alkaid-Benetnash I create a new pull request solving the bugs. Plz checkout my PR: #638

I will check your PR and see if I can merge it to my fork. I guess raulmur will never merge these PRs.

[PierBJX]

Hi @Alkaid-Benetnash,
Amazing job ! It's working properly ! Thank you.

In order to make it more user friendly, I add a button on the viewer (image below). Like this, the user can save the map when he wants instead of shutdown ORB-SLAM.

However, doing that I have sometimes some issues "Segmentation fault (core dumped)".
This happens sometimes not each time. I guess it's because of the multithreading.
This is why, I put some safety checking like the tracking state to know if it's initializing or not.
I put some cout to know where this happens:

void System::SaveMap()
{
    if (mTrackingState == 3 || mTrackingState == 2)
    {
        const string filename = "Maps/" + to_string(time(0)) + ".bin";
        std::ofstream out(filename, std::ios_base::binary);
        if (!out)
        {
            cerr << "Cannot Write to Mapfile: " << filename << endl;
            exit(-1);
        }
        cout << "Saving Mapfile: " << filename << flush;
        boost::archive::binary_oarchive oa(out, boost::archive::no_header);
        cout << " -> HERE1" << flush;
        oa << mpMap;
        cout << " -> HERE2" << flush;
        oa << mpKeyFrameDatabase;
        cout << " -> ...done" << endl;
        out.close();
    }
}

In the terminal I have this:

Saving Mapfile: Maps/1547816372.bin -> HERE1 -> HERE2 -> ...done
Saving Mapfile: Maps/1547816372.bin -> HERE1 -> HERE2 -> ...done
Saving Mapfile: Maps/1547816373.bin -> HERE1Segmentation fault (core dumped)

As you can see it works two times and then it crashes when writing mpMap to oa.
And an other time I got this error :

terminate called after throwing an instance of 'boost::archive::archive_exception'
what(): output stream error
Aborted (core dumped)

Do you think it's because of multithreading ?

[Alkaid-Benetnash]

@PierBJX Can you try my master branch? (I gave up maintaining this PR for now, since it won't be merged anyway). I just commit 15726f4. If you still see seg fault, please compile a Debug version, run everything in gdb, and post at least the back trace here.
A good example is finpin's debug information

[PierBJX]

@Alkaid-Benetnash Finally, I succeeded to fix it. I just needed to stop the local mapping, save the map and release after.
Thanks
By the way, I have another question, if I have two maps and I would like to switch between both while ORB-SLAM is running. Do you think it's something feasible ?

[rahulsharma11]

Hi @PierBJX , @Alkaid-Benetnash ,
I used your modified version of Orbslam (map_save_load_and_bin). Built and ran, not able to see the save/load parameter on gui.
And one more thing i am running stereo based example so if your modifications are done only with Monocular?
Thanks.

[PierBJX]

@rahulsharma11

I used your modified version of Orbslam (map_save_load_and_bin). Built and ran, not able to see the save/load parameter on gui.

This is not in this pull request. Indeed, only @Alkaid-Benetnash works on this pull request.
I modify it for my use.

And one more thing i am running stereo based example so if your modifications are done only with Monocular?

Yes, I think @Alkaid-Benetnash only did it for monocular

[rahulsharma11]

Hi @PierBJX ,
Thanks for the reply.
I did that for my live stereo camera feed.

Can you tell me how can i modify that option on GUI?
Thanks

[PierBJX]

In viewer.cc after pangolin::Var<bool> menuShowGraph("menu.Show Graph", true, true); add this line :
pangolin::Var<bool> menuSaveMap("menu.Save Map", false, false);

Then in the while loop add these lines :

        if (menuSaveMap)
        {
            mpSystem->SaveMap();
            menuSaveMap = false;
        }

Then in System.cc modify the function SaveMap like this:

void System::SaveMap()
{
    if (mTrackingState == 3 || mTrackingState == 2)
    {
        cout << "Pause the local mapper to save a map" << endl;
        mpLocalMapper->RequestStop();
        while (!mpLocalMapper->isStopped())
        {
            usleep(5000);
        }
        
        string filename = to_string(time(0)) + ".bin";

        std::ofstream out(filename, std::ios_base::binary);
        if (!out)
        {
            cerr << "Cannot Write to Mapfile: " << filename << std::endl;
            exit(-1);
        }
        cout << "Saving Mapfile: " << filename << std::flush;
        boost::archive::binary_oarchive oa(out, boost::archive::no_header);
        oa << mpMap;
        oa << mpKeyFrameDatabase;
        cout << " ... done" << std::endl;
        out.close();
        mpLocalMapper->Release();
    }
    else
    {
        cout << "ORB-SLAM not initialised. Map not saved." << endl;
    }
}

[rahulsharma11]

Hi @PierBJX ,
What abt "mbPause" variable?

[PierBJX]

@rahulsharma11

What abt "mbPause" variable?

Sorry I forgot to remove it. It's because I modify a lot ORB-SLAM with many functionalities.
I edit the previous comment

[rahulsharma11]

Thanks for the guidance.
Will try to add similar functionality to load the maps also.
Should i have to use same modification for LoadMap() function also ? or existing LoadMap() function will work such as integration of "mpLocalMapper".

And one more question-
Have you tried or done Aruco integration with this?
THanks.

[PierBJX]

If the load map is done during the initialization it will be fine, no need to modify LoadMap().
But if you do it after and you use a button to load a map you will have to stop all the threads.

No I didn't

[rahulsharma11]

Thanks @PierBJX .
One last querry.
I am using stereo camera but still i am finding some drift in map. Once it gets initial frames then Loopcloser do the global optimization and drift reduces. In that case does it mean that Orb slam is not best for real time mapping and localization even though with stereo camera? as in Monocular based slam it is acceptable but i didn't thought it for stereo.

And how can i view the generated map.bin file separately from Orbslam. Any tool?
Thanks.

[PierBJX]

@rahulsharma11

I am using stereo camera but still i am finding some drift in map. Once it gets initial frames then Loopcloser do the global optimization and drift reduces. In that case does it mean that Orb slam is not best for real time mapping and localization even though with stereo camera? as in Monocular based slam it is acceptable but i didn't thought it for stereo.

Honestly, I do not know about stereo with ORB-SLAM sorry ...

And how can i view the generated map.bin file separately from Orbslam. Any tool?

No you can not because it is only for ORB-SLAM. What you can do is save the pointclouds.
https://medium.com/@j.zijlmans/orb-slam-2052515bd84c
Here is a tutorial to do so

[Alkaid-Benetnash]

@Alkaid-Benetnash Finally, I succeeded to fix it. I just needed to stop the local mapping, save the map and release after.
Thanks
By the way, I have another question, if I have two maps and I would like to switch between both while ORB-SLAM is running. Do you think it's something feasible ?
Oops, maybe you re-edit it so I didn't receive an email notification.

@PierBJX
I assume you are talking about switch maps in one ORBSLAM2::System instance without ORBSLAM2::System::ShutDown. In that case I don't know what other internal state stored in each thread you have to save/load when switching.
I feel using two ORBSLAM2::System instances with two maps and feed new incoming video frame to one of them is a better idea.

[haruishi43]

@Alkaid-Benetnash

Great job! I've wanted this functionality so much.

One question:
If I wanted to change a map right in the middle of the process of doing slam, could I make the LoadMap function a public function and load some file (given a file name).
In other words, does the LoadMap function always have to be in the initialization phase?

[Alkaid-Benetnash]

@Toraudonn
I haven't tested LoadMap in the middle of the process. I think it won't work correctly for now, since current code doesn't care internal state inside other threads (e.g. optimization and loopclosing). As I replied to PierBJX, you can either figure out all other internal state required to be save/load yourself, or use two System instances.
If you choose the first option, it will be helpful to share your findings.

[haruishi43]

@Alkaid-Benetnash @PierBJX

Thanks.
Yeah, I'm trying to figure out the internal states for all of the other threads, but I'm confused in where to look.
I was thinking loading after System::Reset, but that didn't work out so well since the map saved after loading for the second time had the same file size and KeyFrame number as the first saved map.
Do you think there is a way to clear everything (i.e., the map and keyframe before) and load the map without shutting down the system?

[gabarlacchi]

Hi to all,
i am running ORBSLAM2 in an RGBD system (I get a depth estimation from a neural network, so any RGB-D camera) and it works pretty good. The thing is that i am working on a 3-minutes videos and because of that the system fails at least one time in each video, also due to the high level of variation in the scenes (i am working on autonomous driving patterns). I would like to save the trajectory at the fail time, the start point and the end point of the last trajectory computed, and restart the system automatically at the first next available frame. Is this require a saving/loading maps stage?
Thanks for any help

[Shame-fight]

@Toraudonn
I haven't tested LoadMap in the middle of the process. I think it won't work correctly for now, since current code doesn't care internal state inside other threads (e.g. optimization and loopclosing). As I replied to PierBJX, you can either figure out all other internal state required to be save/load yourself, or use two System instances.
If you choose the first option, it will be helpful to share your findings.

thank you for your work. I just compiled your code, but I have some doubts about saving and loading maps with monocular and binocular cameras, because they run on ros. Can you tell me how I should modify the code? Looking forward to hearing from you.thanks in advance