That is the reconstruction of my office hallway using ORBSLAM2. 

ORBSLAM2 is an open-source vision slam implementation with a General Public License (GPL), meaning anyone may use the software for any purpose (and other things). There is some considerable set up involved to get it to work, and you need some idea of compiling c++, and I did all of it on VirtualBox running Ubuntu 14.04, and a native Windows OS.

Note the "double-vision" effect of the points near the bottom of the reconstruction. The batch adjustment algorithm would take care of that if I continued walking around a bit more.

The supposed scale-drift effect as described in the ORBSLAM2 paper by Raulmur cannot really be seen here (partly because I don't have a ground truth image) and based on my own estimation, it looks accurate. This is because I took care to make proper reconstructions while moving the camera. There is probably room for innovation in a method to move the robot such that proper reconstructions can be done. Alternatively, a stereo set up may be used to obtain depth information, as Raulmur also suggests.

Besides the scale-drift problem, in order to make ORBSLAM2 useful, a way to save and load the map created by your motion should be available. One application may not require simultaneous localisation and mapping, once a reliable map has been constructed, only localisation is needed. A robot can then be configured to be more specialised for other tasks besides SLAM. Since this implementation runs a multi-threaded process, a separate thread must be created so as to not interrupt  the other threads running in the background.

Getting the coordinates of MapPoints

There are a few methods that will help to get these coordinates, and I have listed them here.

GetAllMapPoints() is a method of Map that returns a vector list of the pointers of all the MapPoint type objects that have been created by the program and visualised on the Pangolin viewer.

GetWorldPos() is a method of the MapPoint type that will return the absolute coordinates of the object in a cv::Mat (OpenCV matrix) object.

With these in mind, after setting up a thread for user input (including syncing it with the other threads, this is actually the hard part), getting the 3D map information should be a matter of conversion from list of MapPoint objects, to list of cv::Mat objects containing just the absolute coordinates of all 3D points.

It is also worth noting that if the goal is to reuse the MapPoint objects, it is necessary to check how the Map is initialized, so the load function may initialize the Map.

Further, the default initialisation causes the Tracking thread to start initializing. However, once a Map is loaded, we want the tracking thread to start by relocalization.

Saving Coordinates

Usually this wouldn't be an issue, but ORBSLAM runs multithreaded, so a new thread might be necessary in order to process user (or other programs') input, much like the pangolin viewer. To simplify this, for now, placing the save or load functions in the initialisation and shutdown respectively might be more efficient.

Saving MapPoint Objects

I did not expect this to be a problem, but since the tracking, mapping and loop closing threads need MapPoint Objects to work, it is necessary to save all the information that they carry, in order to reuse them in a Load function later. After some searching I determined that the best way to do that is through the Boost library. For some reason, the boost documentation did not specify which libraries to link, and I had some trouble compiling the demo program.

In the end, I found here that the library flag for boost/file_system is -lboost_filesystem-mt, and by a bit of trial and error, since I was trying to use the serialization library, the library flag to use is -lboost_serialization (American spelling) and tadaaah it works.

I used:

g++ source_file.cpp -l boost_<relevant_library> -o target_name

There is also the option of using CMake to help to find your package, which, if you know how, is considerably easier. Using the find_package(Boost REQUIRED) command in CMake, the libraries and include directories are easily found.

TODO: how to use CMake.

After getting the boost libraries to work, we need to get boost to help us save and load the Map or MapPoint Objects - whichever is more convenient. It seems now, based on my understanding of boost, that it would make sense to just save the entire Map object. Initially I thought that boost would not be able to detect pointers, and try to serialize them (which doesn't make sense, because pointers just contain memory addresses, and not the more valuable information), but after looking into it a little more I found that the boost library is more intelligent than I initially thought. Boost will be able to identify pointers, look for the Object that it is pointing at, then serialize and track it. Boost knows what object it is looking at based on the address that it was taken from, and will not serialize and archive repeat objects.

So this means, in an object of class Human called brian, containing pointers to other objects of class Car and class House called car_ptr and house_ptr respectively, even though car_ptr and house_ptr contain addresses rather than the actual objects, Boost will look for the object that lives at the address given by the pointers, and serialize them to be archived.

So one tricky thing is that when you want boost to archive a custom something, you need to give it access to the information inside the object. This is done by sticking this member function into the class definition

// Allow serialization to access non-public data members.
friend class boost::serialization::access; 

// Serialize the std::vector member of Info
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & member_variable_name_1;
ar & member_variable_name_2;
ar & member_variable_name_3;   
}