raulmur / ORB_SLAM2

Real-Time SLAM for Monocular, Stereo and RGB-D Cameras, with Loop Detection and Relocalization Capabilities
Other
9.51k stars 4.71k forks source link

Depth threshold to discard features completely from tracking #845

Open RaghavaModhugu opened 5 years ago

RaghavaModhugu commented 5 years ago

How to get depth of all the detected features in each frame to discard from being used in tracking them with a threshold (example : 40 times the stereo baseline)?

RaghavaModhugu commented 5 years ago

@AlejandroSilvestri can you please help?

AlejandroSilvestri commented 5 years ago

Hi @RaghavaModhugu

Tracking happens in two places:

They populate mCurrentFrame.mvpMapPoints with tracked mappoints.

You can discard mappoints right after tracking, assigning null to the element, like:

mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);

How to filter? You can calculate the distance between map point an pose with

norm(mCurrentFrame.mvpMapPoints[i]->GetWorldPos() - mCurrentFrame.GetCameraCenter()) > 40 * SOME_CONSTANT

Note that stereo is not really my thing, I don't know what unit the world uses to map points and frames positions. May be meters, may be the distance between cameras. So I leave SOME_CONSTANT up to you.

This code wasn't tested, may be full of syntax errors.

I hope this helps.

RaghavaModhugu commented 4 years ago

Hi @AlejandroSilvestri,

Thanks for the reply. I appreciate it. I will try it and get back.

Regards, Raghava Modhugu

RaghavaModhugu commented 4 years ago

Hello,

I tried using it, immediately after track() in the GrabImageStereo. upon the usage of mCurrentFrame.mvpMapPoints[i]->GetWorldPos() , the programm errored out with segmentation fault 11. However, I was able to get the camera center mCurrentFrame.GetCameraCenter() with Can you please help?

Regards, Raghava Modhugu

RaghavaModhugu commented 4 years ago

I am able to find

Hello,

I tried using it, immediately after track() in the GrabImageStereo. upon the usage of mCurrentFrame.mvpMapPoints[i]->GetWorldPos() , the programm errored out with segmentation fault 11. However, I was able to get the camera center mCurrentFrame.GetCameraCenter() with Can you please help?

Regards, Raghava Modhugu

@AlejandroSilvestri I am able to resolve the error and delete those map points with the given logic in both TrackWithMotionModel and TrackReferenceKeyFrame. However, when I observed all the map points after running the entire code. I see some points very far away. I am not able to understand even if the map points are set to null (with distance threshold) immediately after tracking in both the methods.

AlejandroSilvestri commented 4 years ago

@RaghavaModhugu

About SegFault: you must check if mCurrentFrame.mvpMapPoints[i] is not NULL before calling mCurrentFrame.mvpMapPoints[i]->GetWorldPos()

About filtering mappoints: null out mCurrentFrame.mvpMapPoints[i] keeps Tracker from tracking that mappoint in mCurrentFrame. This action doesn't remove the mappoint from the map. The mappoint continues to exist, but is not viewed from current frame.