How to extend RTAB-Map Database information

[cpsarras]

Hello Mathieu,

I would like to extend RTAB-Map functionality to store additional information in every node that is registered to the database. For example, I want to include a boolean variable, indicating whether my robot received a message or not between two consecutive nodes. I want to add this information when a new node is created and added to the database. Also, I would like to be able to traverse the database while RTAB-Map is running, to extract and post-process this information.

Could you point me to the appropriate .cpp files I should edit to append the information to the new node? (and the .cpp files that create the database schema probably)
Also, what .cpp files traverse the database, so I can get an idea on how to do this?

Thank you,
Chris

[matlabbe]

Hi Chris,

If you are using ROS, this can be similar to User Data tutorial here: http://wiki.ros.org/rtabmap_ros/Tutorials/WifiSignalStrengthMappingUserDataUsage

In RTAB-Map library, Rtabmap::setUserData(int id, cont cv::Mat & data). If id is <=0, the data will be set to latest node in the graph. If you are going to set only a boolean, use cv:Mat(2,1,CV_8UC1) format (with dummy second row) to avoid rtabmap thinking that data is already compressed (one row CV_8UC1).

RTAB-Map doesn't save in the database exactly when a new node is created. The node stays in RAM until it is transferred to long-term memory or the application finishes (see this issue). You should then use Memory interface instead to access all nodes (those in RAM and those in database). Memory::getAllSignatureIds() return all Ids in the map (RAM and database), then with Memory::getSignatureDataConst(locationId, false, false, true, false), you can get back the userData.

Example with CoreWrapper.cpp

std::set<int> ids = rtabmap_.getMemory()->getAllSignatureIds();
for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
   int id = *iter;
   SensorData data = rtabmap_.getMemory()->getSignatureDataConst(id, false, false, true, false);
   cv::Mat userData;
   data.uncompressDataConst(0, 0, 0, &userData)
   if(!userData.empty() && userData.at<unsigned char>(0,0) != 0)
      printf("Boolean is true for node %d!\n", id);
}

cheers,
Mathieu

[cpsarras]

Thank you Mathieu, that was exactly what I needed.

[cpsarras]

Hi Mathieu,

I'd like your input on a problem I'm having with rtabmap_ros and the wifi signal demo. Specifically, I am using a modified version of the wifi_signal_pub node to publish mock wifi level values on the /user_data_async topic. Then through the wifi_signal_sub node I print the wifi levels from all node signatures received on the /rtabmap/mapData topic. However, in each received mapData msg, all user_data are empty except the user data on the latest node. Is that the expected behaviour? Is there another way that I can receive the user data from all created nodes?

Thanks in advance.

[matlabbe]

Hi,

Only data of the last node is published. It is for bandwidth efficiency. The node subscribing to mapData should buffer the values on its side, like wifiLevels map used in the WifiSignalSub example.

To get all data from all nodes, you can call service "/rtabmap/get_map". It returns a MapData with all data when global=true, optimized=true and graphOnly=false. This may be called once at start if the mapping node has already a map, then keep what you need in cache on your side and subscribe to /rtabmap/mapData just to get latest data added.

cheers,
Mathieu