Visual landmark integration

[inuex3]

Hello, I would like to ask how could I integrate a visual landmark in rtabmap. I am planning to use rtabmap for outdoor mapping and navigation with object detection. When the robot finds a landmark with object detection, I want to optimize the map considering the landmark location.

I have already implemented the object detection part. So, I have a relative location from the robot(from base_link to landmark) and absolute location(GPS coordinate) of landmarks.

I read these related issues https://github.com/introlab/rtabmap/issues/188 https://github.com/introlab/rtabmap/issues/345 https://github.com/introlab/rtabmap/pull/196 and I think I could implement this using this global_pose and a link to it. https://github.com/introlab/rtabmap/pull/196#issuecomment-364581068

When I was looking into Link.h, I found this link type but I don't know what link should I use for this use. https://github.com/introlab/rtabmap/blob/200ec8e5db69310f781ae54a9c3ff52c65e07d8e/corelib/include/rtabmap/core/Link.h#L41-L54

Is there a better and easier way to do this? If my implementation is right, what type of link is suitable?

Thank you in advance.

[matlabbe]

For the integration of the GPS for the landmarks, it is not implemented in rtabmap. You may add GPS as prior for the node observing the landmark (assuming you can determine the GPS coordinate of the robot by looking at the landmark). However as you maybe saw in the linked posts that there are still some issues. For the rtabmap interface, you could use directly gps/fix instead of global_pose.

For the landmark link, it is a relative link from the current position of the robot. For convenience, you may try to match the apriltags2_ros::AprilTagDetectionArray msg format supported by rtabmap on topic tag_detections. If you don't know the orientation of the landmarks (just XYZ pose), you can set landmark_angular_variance to 9999 so that orientation is not used during graph optimization. Note that each landmark should have a unique ID, otherwise wrong loop closures will be added to map.

So for your problem, I would try to integrate the relative landmark poses before the GPS.

cheers, Mathieu

[inuex3]

Thank you for the detailed explanation! I think I can implement by using your method. I'm going to try the relative landmarks first within this or next week and see how it goes. However,if I can't determine the GPS coordinate of the robot and only can determine the GPS coordinate of the landmark, could I allocate each GPS value to each landmark? Our landmark is like a pole, we can't determine the orientation of it. If it's difficult, I should consider implementing the way that is able to determine the GPS coordinate of the robot as you said.

[inuex3]

I forgot to mention, I am planning to use two cameras forward and backward. According this line, tag detection does not work with multi cameras. https://github.com/introlab/rtabmap/blob/d089e95e5a53a964293fcde078cbe03923aaf093/corelib/src/Memory.cpp#L4607 If I use it with multi cameras, do I need some modification in source code and where in detail?

[matlabbe]

However,if I can't determine the GPS coordinate of the robot and only can determine the GPS coordinate of the landmark, could I allocate each GPS value to each landmark?

We cannot set GPS values to landmarks. If you know the GPS values of the landmark, and if you observe them with a sensor, you should be able to estimate the GPS of the robot.

For the tag detection with multiple cameras, I didn't have time to implement it and test it. To make it work with multiple cameras, a fast approach would be to do something like this on line 4607 you referred:

int subImageRGBWidth = data.imageRaw().cols/data.cameraModels().size();
int subImageDepthWidth = data.depthRaw().cols/data.cameraModels().size();
for(unsigned int i=0; i<data.cameraModels().size(); ++i)
{
    cv::Mat rgb(data.imageRaw(), cv::Rect(subImageRGBWidth*i, 0, subImageRGBWidth, data.imageRaw().rows);
    cv::Mat depth(data.depthRaw(), cv::Rect(subImageDepthWidth*i, 0, subImageDepthWidth, data.depthRaw().rows);
    std::map<int, Transform> camMarkers = _markerDetector->detect(rgb, data.cameraModels()[i], depth); 
    markers.insert(camMarkers.begin(), camMarkers.end());
}

[inuex3]

Okay, thank you for your cooperation!

[NunoGuedesFollow]

However,if I can't determine the GPS coordinate of the robot and only can determine the GPS coordinate of the landmark, could I allocate each GPS value to each landmark?

We cannot set GPS values to landmarks. If you know the GPS values of the landmark, and if you observe them with a sensor, you should be able to estimate the GPS of the robot.

For the tag detection with multiple cameras, I didn't have time to implement it and test it. To make it work with multiple cameras, a fast approach would be to do something like this on line 4607 you referred:

int subImageRGBWidth = data.imageRaw().cols/data.cameraModels().size();
int subImageDepthWidth = data.depthRaw().cols/data.cameraModels().size();
for(unsigned int i=0; i<data.cameraModels().size(); ++i)
{
    cv::Mat rgb(data.imageRaw(), cv::Rect(subImageRGBWidth*i, 0, subImageRGBWidth, data.imageRaw().rows);
    cv::Mat depth(data.depthRaw(), cv::Rect(subImageDepthWidth*i, 0, subImageDepthWidth, data.depthRaw().rows);
    std::map<int, Transform> camMarkers = _markerDetector->detect(rgb, data.cameraModels()[i], depth); 
    markers.insert(camMarkers.begin(), camMarkers.end());
}

This code is working fine btw.

[inuex3]

@matlabbe Hello I would like to comment on this issue because I finally published the paper about agricultural robot using rtabmap. Let me say thank you for the cooperation. It’s going to be published soon. I uploaded a short movie about my research here. I could not finish this research without your code and help. https://youtu.be/h_7y-r1qzcE

[matlabbe]

@inuex3 This is a super cool video, thx for sharing! Putting your project link here: https://github.com/inuex3/robot_mower_2dnav

[KSorte]

Hello @matlabbe, So if I know the GPS coordinate of the landmark, I would need to write my own node to estimate the lat long coordinate of my bot from the landmark observation right?

I am planning on using April Tags as landmarks with known GPS coordinates and would like to have my robot pose and heading in lat long from those and then use those odometry messages for rtabmap. Could you please share your thoughts on this approach?

[matlabbe]

If you defined Tag 1 as your local origin (0,0), and set a relative pose of all your tags based on that tag, you could fill up Marker/Priors vector. When rtabmap will be detecting landmarks, the map will be transformed in that local coordinate frame (make sure Optimizer/PriorsIgnored is false). If you want to transform the map poses into GPS coordinates, you can offset the GPS coordinate of Tag 1 with the relative map pose (a simple translation node could listen to map->base_link TF, transform that pose in GPS coordinates and republish it).

[KSorte]

So essentially we set Tag1 as map to get the priors which we can subsequently feed into our setup next time with Optimizer/PriorsIgnored set to false right? Understood how to convert to GPS coordinates. Thanks! :)