Orb_slam2: How to obtain 3D point cloud of the map?

[akashshar]

Hello,

I am a newbie and I am sorry if this is the wrong place to ask this question.
I want to use the 3D point cloud obtained from ORB slam in order to recognize structures like walls/floor/ceiling etc. And after this, I want to generate some augmentations.
I just want to know how to obtain the pose of the camera and 3D positions of all landmarks (point cloud). Also all these are with respect to the world co-ordinate system?

Thank you very much.
Cheers!

[AlejandroSilvestri]

@akashshar

You can use this Map method:

vector< MapPoint * > ORB_SLAM2::Map::GetAllMapPoints()

You can get the Map object pointer with

Map* ORB_SLAM2::System::

So, in main() you have SLAM and can use

vector< MapPoint * > allMapPoints = SLAM.>GetMap().>GetAllMapPoints();

To get a mapPoint position, use

cv::Mat ORB_SLAM2::MapPoint::GetWorldPos()

like in

Mat pos = allMapPoints[i]->GetWorldPos();

World reference is arbitrarily defined in initialization, randomlike, almost impossible to control. InitialFrame is the origin position and orientation, scale is taken from initial map's median depth.

ORB-SLAM2 uses camera reference, so Z is heading forward, Y is heading down.

You must get a 4x4 Mat transformation from this reference to real world reference by your own.

[akashshar]

@AlejandroSilvestri
Thank you very much for your answer!
How were you able to figure it out? Was it intuition or is there any file/document which lists all these out?
Or have you gone through the code and understood it?

[AlejandroSilvestri]

I myself documented my own modification to ORB-SLAM2, but hadn't time to translate to english.

https://alejandrosilvestri.github.io/os1/doc/html/class_o_r_b___s_l_a_m2_1_1_map.html

[akashshar]

Thanks for sharing that. It is still helpful, even without a translation.

[NeXT1995]

@akashshar Finally Do you obtain 3D point cloud of the map ？

[mhubii]

Hello folks,

this post may be a little outdated by now so here is a new documentation I found http://code.luohuiwu.cc/annotated.html

[maedakosuke]

In my case, there is no GetMap() method in System class ( I wonder why... ).
So, I add Map* GetMap() { return mpMap; }; as public member of System class in System.h.
main() is like below:

#include <System.h>
#include <Converter.h>  // to use ORB_SLAM2::Converter
int main(int argc, char **argv)
{
        // argv[1]: vacabulary file path, argv[2]: parameter yaml file path
        ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);
        // do process
        // output map points
    std::vector<ORB_SLAM2::MapPoint*> allMapPoints = SLAM.GetMap()->GetAllMapPoints();
    std::cout << "# size=" << allMapPoints.size() << std::endl;
    std::cout << "# x,y,z" << std::endl;
    for (auto p : allMapPoints) {
        Eigen::Matrix<double, 3, 1> v = ORB_SLAM2::Converter::toVector3d(p->GetWorldPos());
        std::cout << v.x() << "," << v.y() << "," << v.z() << std::endl;
    }
}

[nalinraut]

@maech13 The main() file is your own right? However, I assume that this is after the Mapping is completed. What changes would you suggest if I want this to run in loop and extract map points every time the map is being updated?

[maedakosuke]

@nalinraut A way is watching the number of KeyFrame because Map Points would change when new KeyFrame is inserted or deleted. The part of code sample is like below.

int nkeyframes = 0;
for (int ni = 0; ni < nImages; ni++) {
    // Read image from file
    im = cv::imread(string(argv[3]) + "/" + vstrImageFilenames[ni], CV_LOAD_IMAGE_UNCHANGED);
    double tframe = vTimestamps[ni];

    SLAM.TrackMonocular(im, tframe);

    ORB_SLAM2::Map* map = SLAM.GetMap();
    std::vector<ORB_SLAM2::KeyFrame*> keyframes = map->GetAllKeyFrames();

    if (SLAM.GetTrackingState() == ORB_SLAM2::Tracking::eTrackingState::OK && nkeyframes != keyframes.size()) {
        // The way to access MapPoints is same as my above code.
    }

    nkeyframes = keyframes.size();
}

This is just one sample. There is more smart way, may be.

If you want to know big change of MapPoints, then you can watch the difference of Map::GetLastBigChangeIdx().

[UannaFF]

Is there a way to get color in the pointclouds?

[nalinraut]

@maech13 Thank you! It makes sense and I got the map points for every frame( The number of which does not change until next keyframe is selected). How can I get the camera trajectory every frame with respect to the world frame? Now I know that we get translation and rotations for every keyframe. However, is there any way I could save the hassle of computing the coordinates from the translation and rotation vectors?

[maedakosuke]

@nalinraut How about to make the trajectories computing method like System::SaveTrajectoryTUM in System.cc and stack the result into std::vector.

[nalinraut]

@maech13 I could get the trajectories by
#include "MapDrawer.h"
cv::Mat Tcw_;
Tcw_ = mCameraPose;
cv::Mat Cxyz(3,1,CV_32F);
Cxyz = mCameraPose.rowRange(0,3).col(3)

Thanks for the response! That should work too.

[AlejandroSilvestri]

@UannaFF

I did it, you can see them in this video, but most are grayish. Look for green and pink dots from grass and flowers.

It's not straightforward. ORB-SLAM2 uses gray images: color images are converted to grayscale in Tracking::GrabImageMonocular, so when mappoints are created you should find a way to get the original color images to get the feature's pixel color.

[UannaFF]

Thanks for the answer, Do you know if the pointcloud we get is in the coordinate system of the first photo? because the poses we get take the first photo as the origin.

[AlejandroSilvestri]

@UannaFF

Yes, immediately after initialization the first keyframe is constructed based on first frame, and its pose is canonical, i.e. it's the center of the map, the reference system of the so called world. The only information missing this keyframe can't obtain is the scale, which is kind of random and heavily related to scene, taken from the medium size of the initial map.

[YujiElfahkrany]

@maech13 maybe you need to lock the map first before using it
istead of Map* GetMap() { return mpMap; };
I would do this
Map* System::GetMap() { unique_lock<mutex> lock(mMutexState); return mpMap; }
inside of system.cc

[abhy1012]

@nalinraut Can you explain what changes you made exactly to get the mappings for each keyframe. I am no that proficient with C++ so I don't know where to add the main().
What did you do after adding Map* GetMap() { return mpMap; }; as a public member in System.h?

Can you walk me through your process?

Thanks

[NescobarAlopLop]

@maedakosuke Thank you, exactly what I was looking for!