Orb_slam2: Display current points on Map Viewer

[PierBJX]

Using monocular, I would like to know if it was possible to color the current points that the camera sees (green square on "Current Frame" window) on the Map Viewer window.

ORB SLAM uses two functions to get the points on the map (code):

    const vector<MapPoint *> &vpMPs = mpMap->GetAllMapPoints();          // Black points
    const vector<MapPoint *> &vpRefMPs = mpMap->GetReferenceMapPoints(); // Red points

From these two vectors how could I find the current points displayed on the "Current Frame" window ?
I found that the current points displayed on the current frame window are mCurrentFrame.mvKeys. However, these points are not vector Map Point but a vector of KeyPoints.

[AlejandroSilvestri]

@PierBJX

Let say you want to populate the custom variable

 const vector<MapPoint *> &vpCurrentMPs  = SomehowGetCurrentMapPoints();          // Green points

You can retrieve those mappoints from

(vector<MapPoint*>) mCurrentFrame.mvpMapPoints

This vector has size mCurrentFrame.N and most of its elements are null.

[PierBJX]

Thanks, I succeeded to do it. Here is the code to modify :

Map.h

• Insert line 54

  // Function to manage the current points to color them in green
  void AddCurrentMapPoint(MapPoint *pMP);        // add
  void EraseCurrentMapPoint();                   // delete
  std::vector<MapPoint *> GetCurrentMapPoints(); // get

• Insert line 72
  std::set<MapPoint *> mspCurrentMapPoints; // Current map points

MapDrawer.h

• Change line 41 with
  void DrawMapPoints(const bool bDrawCurrentPoints);

Map.cc

• Insert line 132

void Map::AddCurrentMapPoint(MapPoint *pMP)
{
    unique_lock<mutex> lock(mMutexMap);
    mspCurrentMapPoints.insert(pMP);
}

void Map::EraseCurrentMapPoint()
{
    unique_lock<mutex> lock(mMutexMap);
    mspCurrentMapPoints.clear();
}

vector<MapPoint *> Map::GetCurrentMapPoints()
{
    unique_lock<mutex> lock(mMutexMap);
    return vector<MapPoint *>(mspCurrentMapPoints.begin(), mspCurrentMapPoints.end());
}

• Add in the clear function line 118
  mspCurrentMapPoints.clear();

MapDrawer.cc

• Change line 44 with
  void MapDrawer::DrawMapPoints(const bool bDrawCurrentPoints)

• Insert after the line 80

    if (bDrawCurrentPoints)
    {
        // Define points
        glPointSize(5);
        glBegin(GL_POINTS);
        glColor3f(0.0, 1.0, 0.0);

        // All map points
        for (std::vector<MapPoint *>::const_iterator i = vpCurrentMPs.begin(); i != vpCurrentMPs.end(); i++)
        {
            if ((*i)->isBad())
                continue;
            cv::Mat pos = (*i)->GetWorldPos();
            glVertex3f(pos.at<float>(0), pos.at<float>(1), pos.at<float>(2));
        }
        glEnd();
    }

Tracking.cc

• Insert line 419

        // Empty the current map points vector
        mpMap->EraseCurrentMapPoint();

        // Add the current map points in the vector
        for (int i = 0; i < mCurrentFrame.N; i++)
        {
            if (mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
            {
                mpMap->AddCurrentMapPoint(mCurrentFrame.mvpMapPoints[i]);
            }
        }

Viewer.cc

• Change line 68 with
  pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(185));

• Insert line 72
  pangolin::Var<bool> menuShowCurrentPoints("menu.Show Current Points", true, true);

• Change line 133 with
  mpMapDrawer->DrawMapPoints(menuShowCurrentPoints);

[AlejandroSilvestri]

@PierBJX , glad to know you succeeded.

If you have any time, please post a screen capture, and better if you can upload a video to YouTube and share the link here with us.

[PierBJX]

@AlejandroSilvestri, I have no time yet to do the video but I will and share the link.
For the moment, here is the screenshot

As you can see the current points are in green.

[rahulsharma11]

Hi @PierBJX ,
I tried exact same modifications but still getting only red and black points.
Is there any other modification left?
Thanks.

[PierBJX]

@rahulsharma11
Have you built orb-slam ?
Because it's supposed to work. Send a screenshot of the viewer GUI

[rahulsharma11]

Yes, i have built orbslam.

[KaninchenM]

Thanks, I succeeded to do it. Here is the code to modify :

Map.h

• Insert line 54

  // Function to manage the current points to color them in green
  void AddCurrentMapPoint(MapPoint *pMP);        // add
  void EraseCurrentMapPoint();                   // delete
  std::vector<MapPoint *> GetCurrentMapPoints(); // get

• Insert line 72
  std::set<MapPoint *> mspCurrentMapPoints; // Current map points

MapDrawer.h

• Change line 41 with
  void DrawMapPoints(const bool bDrawCurrentPoints);

Map.cc

• Insert line 132

void Map::AddCurrentMapPoint(MapPoint *pMP)
{
    unique_lock<mutex> lock(mMutexMap);
    mspCurrentMapPoints.insert(pMP);
}

void Map::EraseCurrentMapPoint()
{
    unique_lock<mutex> lock(mMutexMap);
    mspCurrentMapPoints.clear();
}

vector<MapPoint *> Map::GetCurrentMapPoints()
{
    unique_lock<mutex> lock(mMutexMap);
    return vector<MapPoint *>(mspCurrentMapPoints.begin(), mspCurrentMapPoints.end());
}

• Add in the clear function line 118
  mspCurrentMapPoints.clear();

MapDrawer.cc

• Change line 44 with
  void MapDrawer::DrawMapPoints(const bool bDrawCurrentPoints)
• Insert after the line 80

    if (bDrawCurrentPoints)
    {
        // Define points
        glPointSize(5);
        glBegin(GL_POINTS);
        glColor3f(0.0, 1.0, 0.0);

        // All map points
        for (std::vector<MapPoint *>::const_iterator i = vpCurrentMPs.begin(); i != vpCurrentMPs.end(); i++)
        {
            if ((*i)->isBad())
                continue;
            cv::Mat pos = (*i)->GetWorldPos();
            glVertex3f(pos.at<float>(0), pos.at<float>(1), pos.at<float>(2));
        }
        glEnd();
    }

Tracking.cc

• Insert line 419

        // Empty the current map points vector
        mpMap->EraseCurrentMapPoint();

        // Add the current map points in the vector
        for (int i = 0; i < mCurrentFrame.N; i++)
        {
            if (mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
            {
                mpMap->AddCurrentMapPoint(mCurrentFrame.mvpMapPoints[i]);
            }
        }

Viewer.cc

• Change line 68 with
  pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(185));
• Insert line 72
  pangolin::Var<bool> menuShowCurrentPoints("menu.Show Current Points", true, true);
• Change line 133 with
  mpMapDrawer->DrawMapPoints(menuShowCurrentPoints);

Thanks to PierBJX.
Remember to add the following content to functionDrawMapPoints(const bool bDrawCurrentPoints) in file MapDrawer.cc:
const vector<MapPoint*> &vpCurrentMPs = mpMap->GetCurrentMapPoints();
After const vector<MapPoint*> &vpRefMPs = mpMap->GetReferenceMapPoints();
Or you will not compile well.