Are apriltags supported in the RtabMap library

[dragonblade316]

Trying to figure out how to go about adding apriltag support to depthai's rtabmap wrapper but I am failing to find any documentation on how apriltags work in rtabmap.

I might just be being stupid but google is not helping right now.

Any help is appreciated.

[matlabbe]

Can you link which launch files you are using? ros1 or ros2? which apriltag package are you using? (external or internal to rtabmap?)

For ros1, there is an example here: https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_examples/launch/test_apriltag_ros.launch

For ros2, we don't, but I can check to make one based on those examples: https://github.com/introlab/rtabmap_ros/pull/1334

[dragonblade316]

I am not using ROS. This project uses the RtabMap c++ library directly (Specifically I'm trying to integrate support for apriltags (and other landmarks) into the depthai wrapper for RtabMap). My current goal is to detect apriltags external to rtabmap but I would like to test both ways in case the internal detector is better than what I can make.

I have not been able to find any sort of api reference or documentation showing how this should be done. (I am working though the process or reading though some of the headers and the rtabmap_slam ros2 package but if there is a code snippet I can look at, that would be helpful).

Thanks

[dragonblade316]

After, doing a little more reading I think I can ask a better question.

When you add a landmark with SensorData::setLandmarks(), what does that do behind the scenes. Is that telling rtabmap, "we see the following landmarks with the following ids at these locations" or "This is a list of all landmarks you could encounter" and then turns on whatever internal system it uses.

I'm looking at Landmark.h and trying to figure out how I am supposed to use it and its fields.
id is fairly obvious (just the apriltag id)

What unit is size in? I assume its meters but I have seen mm used for this sort of thing.

Is pose the global pose of the apriltag (or other landmark) or the local pose relative to the camera (related to the above paragraph).

How would I go about finding covariance. Would that even be something I need to worry about with apriltags?

Thank you again

[matlabbe]

Hi, for your previous post, internal marker detection is handled by this class:
https://github.com/introlab/rtabmap/blob/master/corelib/src/MarkerDetector.cpp

If you use c++ api, you would need to pass these parameters to Rtabmap object to enable it:

ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kRGBDMarkerDetection(), "true"));
parameters.insert(ParametersPair(Parameters::kMarkerDictionary(), "20"));            // 20 for AprilTags 36h11
parameters.insert(ParametersPair(Parameters::kMarkerCornerRefinementMethod(), "3")); // 3 for AprilTags
parameters.insert(ParametersPair(Parameters::kMarkerLength(), "0.1"));               // marker size in meters
parameters.insert(ParametersPair(Parameters::kMarkerMaxRange(), "4"));               // in meters, recommended to avoid bad rotation estimation on far tags.

Note that tag ID "0" is ignored, only tags with ID >= 1 are detected. For more info about these parameters, do:

rtabmap --params | grep Marker

What unit is size in? I assume its meters but I have seen mm used for this sort of thing.

in meters

Is pose the global pose of the apriltag (or other landmark) or the local pose relative to the camera (related to the above paragraph).

relative to base frame (not camera optical frame). We can see here that we transform the tag detection from camera frame to base frame.

How would I go about finding covariance. Would that even be something I need to worry about with apriltags?

Yes and no. Depending on the optimizer it can do slightly different things as explained in this parameter description:

Param: Marker/VarianceOrientationIgnored = "false"         [When this setting is false, the landmark's orientation is optimized
     during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when
     the landmark's orientation estimation is not reliable. Note that for Optimizer/Strategy=1 (g2o), only Marker/VarianceLinear 
     needs be set if we ignore orientation. For Optimizer/Strategy=2 (GTSAM), instead of optimizing the landmark's position 
     directly, a bearing/range factor is used, with Marker/VarianceLinear as the variance of the range factor (with 9999 to optimize
     the position with only a bearing factor) and Marker/VarianceAngular as the variance of the bearing factor (pitch/yaw).]

However, in the normal usage of tags, you should only care about the overall variance set to linear part and rotational part of the covariance. In general, the position estimation is relatively good, but orientation estimation is often not super accurate or flaky, particularly at longer distances. Here the default variance set when detecting tags internally:

Param: Marker/VarianceAngular = "0.01" [Angular variance to set on marker detections. If Marker/
     VarianceOrientationIgnored is enabled, it is ignored with Optimizer/Strategy=1 (g2o) and it 
     corresponds to bearing variance with Optimizer/Strategy=2 (GTSAM).]
Param: Marker/VarianceLinear = "0.001" [Linear variance to set on marker detections. If Marker/
     VarianceOrientationIgnored is enabled and Optimizer/Strategy=2 (GTSAM): it is the variance of 
     the range factor, with 9999 to disable range factor and to do only bearing.]

Note that in the iOS app, I just set a maximum marker range instead ("Marker/MaxRange"), and use these default variances.

If you create the covariance matrix yourself, make a cv::Mat::eye(6,6,CV_64FC1) and set diagonal values inside top-left 3x3 of the matrix to linear variance wanted, and set the diagonal of the lower-right 3x3 of the matrix to the angular variance wanted.

cheers,
Mathieu

[dragonblade316]

Okay, I think I am starting to understand. It looks like I can set up the internal detection pretty easily just with params (I will be trying this first since I think the tools to set it up are already available to me in the depthai library).

Am I safe to assume that SensorData::setLandmarks() is how you pass in data from an external apriltag detector?

Also, how do I tell RtabMap the global poses of each of the tags? The current system of relative poses seems good for loop closure but not localization (if that makes sense).

Thanks again

[matlabbe]

Yes, use SensorData::setLandmarks() to pass detections from external detector (the pose should be relative to base frame, not the camera frame).

Tags should work both in mapping (loop closures) and localization modes (relative to map frame). However, if you want to localize in world frame (i.e., the tags have been surveying to a known pose in the real world), you would need to use these following parameters:

Param: Marker/Priors = ""                     [World prior locations of the markers. The map will be transformed in marker's world
                                               frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), 
                                               markers are separated by vertical line ("id1 x y z roll pitch yaw|id2 x y z roll pitch yaw"). 
                                               Example:  "1 0 0 1 0 0 0|2 1 0 1 0 0 1.57" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).]

Param: Marker/PriorsVarianceAngular = "0.001" [Angular variance to set on marker priors.]
Param: Marker/PriorsVarianceLinear = "0.001"  [Linear variance to set on marker priors.]

Note that "Optimizer/PriorsIgnored" should be set to "false" to use the marker priors in graph optimization.

Here some examples from other people:
https://github.com/introlab/rtabmap_ros/issues/867
http://official-rtab-map-forum.206.s1.nabble.com/Integrate-external-markers-from-apriltag-ros-with-Priors-td10358.html#a10425
http://official-rtab-map-forum.206.s1.nabble.com/How-to-improve-mapping-accuracy-based-on-ArUco-identification-code-td8393.html