Localize in a pre-loaded local map created by RGB-D mapping

[jdcart]

Hello,

I am trying to use the capability that is described here, I've been following the steps with no success.

I'm mapping using a kinect v2 and trying to localize using a regular laptop webcam. I am able to create the database and save it using the settings that are described in the instructions, then I set that database as my source for Odometry and I also open it to be able to see the map as I localize. I set RTAB to localization mode (with PnP on), select webcam as my source and run it.

When I run it the camera works but it stops immediately and I get the following error:

[ERROR] (2016-11-23 12:31:55.970) OdometryF2M.cpp:163::OdometryF2M() The loaded fixed map from "/home/juan/Documents/RTAB-Map/CROfficeRGBLoc_11-23-16.db" is too small! Only 0 unique features loaded. Odometry won't be computed!

Not sure what I'm doing wrong. Is it because I'm activating PnP for the mapping?

I appreciate any help you can give me with this.

Juan

[matlabbe]

Hi,

The approach in your referred link is obsolete (june 2015). It doesn't work with the new versions, I tried as is and I reproduced your error or it crashed at another place.

I did a revision of the code to make RGB-only localization working with a pre-built map. This new approach works from this commit:

1- Open RTAB-Map->Preferences, then reset all settings to default to start clean.
2- In Source panel, select your RGB-D or stereo sensor in order to create the 3D map.
3- [Optional but this will increase localization success] In RTAB-Map panel, set Detection Rate to 0 Hz (to add all frames to map). Only do this on small environments as the map size will increase very fast.
3- [Optional] In Vocabulary panel, check "Use odometry features" to use same parameters for the map than odometry.
4- Start mapping.
5- Close/Save the database (this will be our fixed map)
6- Re-open the database you just saved
7- Open Preferences: A) In RTAB-Map panel, set Detection Rate to 0 Hz (if not already set) and uncheck SLAM Mode. B) In General settings (GUI), check "Odometry disabled". C) In Visual Registration panel, select 3D to 2D motion estimation approach.
8- Before trying an other camera than one used for mapping, you can go in Source panel under RGB-D section and check "Only RGB images are published". This will simulate a RGB-only camera using your current calibration.
9- Start detection. The camera should be able to localize on the map at this point.

10- If 9 is working, you can now try another RGB-only camera in the Source panel (e.g., standard USB camera). You will have to calibrate it (see Calibrate button in Source panel) if not yet calibrated.
11- If you restart the camera (stop/start), localization would start again.

cheers,
Mathieu

[jdcart]

Thanks for the quick reply! I followed your steps and it works, just need to tweak some settings to get better results. I'm assuming this method can be implemented directly using the RTAB library, right?

[matlabbe]

Yes, you only to have to instantiate CameraRGB and Rtabmap objects. Something similar to this (example without events like this other example):

#include <rtabmap/core/CameraRGB.h>
#include <rtabmap/core/Rtabmap.h>

int main(int argc, char * argv[])
{

Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
rtabmap::CameraVideo camera(0, true, 0, opticalRotation); // assuming USB camera (cv::VideoCapture used)
rtabmap::Rtabmap rtabmap;

rtabmap::ParametersMap parameters;

// match the odometry features used in the database for localization
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), "1000"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "6")); // GFTT+BRIEF

// 3D -> 2D registration
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVis/EstimationType(), "1"));

// Localization
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));

rtabmap.init(parameters, "my3DMap.db");

if(camera.init("calibration/directory", "calibrationName"))
{
   SensorData data = camera.takeImage();
   while(data.isValid())
   {
      if(rtabmap.process(data))
      {
         if(rtabmap.getLoopClosureId() > 0)
         {
            printf("Localized! pose: %s\n", rtabmap.getMapCorrection().prettyPrint().c_str());
         }
         else
         {
            printf("Not localized\n");
         }
      }
      data = camera.takeImage();
   }
}

return 0;
}

See Parameters.h for an exhaustive description of all parameters. I didn't test this code, but it is mostly the same steps as the example above.

cheers,
Mathieu

[jdcart]

So far everything is working great, thanks for the help!

I have another question. In the standalone version, is there a way to select the RGB camera that I want to use, it always defaults to my laptop's webcam, how do I point it to a USB camera?

Thanks again.

Juan

[matlabbe]

Hi,

Though not tested often, you can set "ID of the device" to 1 for example, to use the second USB camera connected to computer.



cheers,
Mathieu

[heimsi]

This feature is not implemented in the ROS implementation of RTAB, right?

[matlabbe]

Hi,

Yes it works. Here is the example with rtabmap_ros (make sure you have latest version from source) and a kinect:

1- Create a map

$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/DetectionRate 0 --Kp/DetectorStrategy 6"

2- Close the previous launch files to save the database. Replay in localization mode and subscribing to just the RGB topic from the kinect. Create fake TF odometry.

$ roscore

$ rosrun rtabmap_ros rtabmap \
_subscribe_depth:=false \
_Mem/IncrementalMemory:=false \
rgb/image:=camera/rgb/image_rect_color \
rgb/camera_info:=camera/rgb/camera_info \
_odom_frame_id:=odom \
_frame_id:=camera_link

$ rosrun tf static_transform_publisher 0 0 0 0 0 0 odom camera_link 100

$ roslaunch freenect_launch freenect.launch

$ rosrun rtabmap_ros rtabmapviz


cheers,
Mathieu

[Timofejev]

matlabbe wrote

Hi,

I did a revision of the code to make RGB-only localization working with a pre-built map. This new approach works from this commit:

Hello there!
is there any easy way to enable camera position updates in the 3D map when using an RGB camera without metric slam mode? In my case multiple loop closures are detected but the camera does not move in the 3D map.
I skimmed through the rtabmap.cpp and it seems that a lot of localization functionality is not available when _rgbdSlamMode is not true.

edit: the problem should have been some settings I changed in the gui. I resetted them all to default, then applied the settings from the cited posting and now the camera is localized in the 3d map view.

[matlabbe]

Good that you found a solution. For your first question, metric mode should be enabled to get a geometric position, otherwise it is pure appearance-based localization (image matching without geometric transformation).

[Timofejev]

> metric mode should be enabled to get a geometric position

thanks, this indeed was the setting I had set to a wrong value