apriltags poor TF correction

[molinadavid]

Hi, I'm testing the latest version of rtabmap with the markers support, I am currently using apriltags but I am finding the correction to be very unreliable and was wandering if there is anything I could do to improve it.

This is a screencast of one of the samples sample1

This is shows a very wrong orientation after a correct localization. sample2

This one shows a static position but the TF keeps changing. sample3

Any information I can give you please let me know.

Really appreciate all your help.

Best regards, David

[matlabbe]

It is not clear if it is the visual loop closure detections or the landmark detections causing the wrong orientations. Can you try with Kp/MaxFeatures set to "-1" (loop closure detection disabled)? What is your configuration of rtabmap node?

There is also something wrong with the odometry. The "scans" don't follow the map. What is the source of odometry?

cheers, Mathieu

[molinadavid]

Hi Mathieu,

Thank you very much for your quick response. Indeed the odometry on the robot had a problem that we already fixed, we are currently testing the parameters you suggested and I will update as soon as we have some results.

In the mean time I will share the database we used for this videos. rtabmap.db

[matlabbe]

Thx for this database. Odometry is a lot better now. One problem I see when opening it on my side is that g2o (which is the optimizer used) breaks the map at some point. All poses are 2D and the graph is optimized in 2D, but we get errors in Z like this:

When selecting GTSAM instead we have the correct optimization in 2D:

However, the graph optimized with g2o in databaseViewer doesn't suffer the same problem than the one optimized by rtabmap standalone. I'll have to look in more depth to find the difference.

For reference, here is the map optimized with GTSAM including landmarks:

One small detail is the poor depth estimation on the floor by the zed camera, which creates points under the ground, though it is more a sensor issue (or with disparity approach used by zed):

cheers, Mathieu

[matlabbe]

I fixed the problem with g2o in the commit above, now the map is correctly optimized whether g2o or GTSAM are used.

[molinadavid]

Hi Mathieu,

We have been doing some testing with the apriltags support and are encountering some localization problems.

The following 2 videos shows the tags used with the loop closures disabled (Kp/MaxFeatures set to "-1" ).

For the first test we left the tag orientation estimation as default: noLoop_withAprilOri.mp4

On the second test we disabled the orientation estimation of the tags (Aruco/VarianceAngular set to 9999): noLoop_noAprilOri.mp4

The following 2 tests are done with the loop closure and the april tags enabled.

For the first one the orientation estimation of the tags is disabled: WithLoop_NoAprilOri.mp4

As you can see on the videos the orientation is very badly estimated around the tags and the use the flag "Aruco/VarianceAngular" does not seem to make much of a difference.

Best regards, David

[matlabbe]

Hi, changing Aruco/VarianceAngular won't do any difference in localization mode. The actual detection is taken "as is" for localization with orientation estimated and distance. It can be indeed difficult to debug in localization mode, as we don't see the actual transform computed by Aruco algorithm. If you can share the database of the map and a rosbag of the zed images looking at a tag whre you see large localization errors, it could help to debug.

Another way to debug is to use the apriltags2_ros package directly to see TF of the detections in rviz. An advantage to use this package is to use bundle tags (e.g. mosaic of tags) to have more robust orientation estimation. An example of usage can be found here with rtabmap_ros (Note: it looks like they renamed recently apriltags2_ros to apriltag_ros, the example launch file may be slightly modified to correct package name).

[molinadavid]

Hi Mathieu,

Thank you very much for your reply, this is a small bag we took where the localization problem appears, for taking this bag we set the Kp/MaxFeatures to "-1" to get localization only from the tag. 2019-05-16-07-29-35_without9999

and the database used for this bag is the following one: rtabmap_pallet.db

Best regards, David

[matlabbe]

Did you use apriltags2_ros? If so, can you share the apriltags2 config file? The launch file part of rtabmap could be convenient to have too. thx

Mathieu

[jinshengye-git]

Did you use apriltags2_ros? If so, can you share the apriltags2 config file? The launch file part of rtabmap could be convenient to have too. thx

Mathieu

Hi Mathieu,

I am Jin, colleague of David.

I tried to integrate the apriltags2_ros and with loop closures on.

<param name="Kp/MaxFeatures" type="string" value="0"/>

However, results shows external apriltags2_ros is not integrated very well.

Here I upload some screen cast videos and the db file I was using.

• 1 test: Video00001.mp4

No big jump. but external apriltags only likes a loop closure feature provider.

• 2 test: Video00002.mp4

There was jump, but corrected by loop closures.

• 3 test: Video00003.mp4

With both rtabmap apriltags and external apriltags on, things getting even worse, the robot jumps frequently.

• This is the map rtabmap.db file I was using.

• tag_settings.yaml

• tags.yaml

• Launcher file

To integrate apriltags properly would you give some hints?

Cheers,

Jin

[matlabbe]

Thank you for the config and launch files. I can reproduce the jumps when using the tags. I see that when the tags are far from the camera, the orientation is not very reliable, we should indeed set 9999 to angular marker variance to optimize without theta. For mapping it is okay, but for localization, right know we use directly the tag orientation even if we set 9999 or not. A new approach is needed to ignore tag orientation during localization, I'll think about it. In the mean time, maybe inhibit apriltags2_ros from publishing a detection if the tag doesn't appear enough large (pixels) in the image as the biggest orientation issues are when the tags appear very small in the images. tag_bundles (see tags.yaml) could be used if they can provide more reliable orientations. For internal tag detection, we could accept detections here only if distance from the camera (tvecs[i].val[2]) is under a fixed threshold:

if(tvecs[i].val[2] < 4) // <4 meters
{
  Transform pose = model.localTransform() * t;
  detections.insert(std::make_pair(ids[i], pose));
}

Note also that I did some changes to your launch file:

<remap from="tag_detections" to="/tag_detections"/>
<param name="odom_frame_id"             type="string"   value="odom"/>
<param name="odom_tf_linear_variance"   type="double"   value="0.001"/>
<param name="odom_tf_angular_variance"  type="double"   value="0.0005"/>
<param name="Optimizer/Robust"          type="string"   value="false"/>
<param name="RGBD/OptimizeMaxError"     type="string"   value="3"/>
<param name="Aruco/VarianceLinear"      type="string"   value="0.1"/>
<param name="Aruco/VarianceAngular"     type="string"   value="9999"/>
<param name="landmark_linear_variance"  type="double"   value="0.1"/>
<param name="landmark_angular_variance" type="double"   value="9999"/>

First the tag_detections was not correctly connected to rtabmap, so all detections from apriltags2_ros node were ignored. The odometry topic has large covariance, I used TF approach instead by setting manually the covariance for convenience. Optimizer/Robust doesn't seem to work well with landmark constraints, disable it. If odometry covariance is not overestimated or underestimated, RGBD/OptimizeMaxError would be over 1 (default 3). When using external tag detections, we should use landmark_linear_variance and landmark_angular_variance parameters to set their variance. In future version, I'll removed them to use only Aruco/VarianceLinear and Aruco/VarianceAngular for both internal and external tag detection.

cheers, Mathieu

[jinshengye-git]

Hi Mathieu,

we will try on your suggestions and send you feedback, thank you for the kind reply as always.

cheers, Jin

[Tim-Fronsee]

Greetings Mathieu,

We implemented your suggestion for reducing the detection range inside the MarkerDetector.

At 4 meters we still experienced large TF correction jumps during localization. Thus, we experimented with smaller ranges and found 2 meters to produce the best results for TF correction.
That said, this gives us a short-range for detection.

Do you have any suggestions for alternative solutions that require less sacrifice on detection range?

Best,

Tim

[matlabbe]

You may try apriltag bundles, increase tag size, increase camera resolution... the idea is to increase apriltag orientation estimation accuracy.

If you are using apriltag_ros, note that I fixed a bug recently to get better estimation of the tags in the map (a time synchronization problem that would appear if tags are detected while moving). You may give a try.

[kiprock]

Hi Matt, David, all,

I am having a similar issue with localization jumping around using apriltags:

http://kiphaynes.com/LIL/TagDetection.mp4

Did you ever find a solid solution? It seems to me that the tag location is pretty reasonable, just the rotation estimation jumps around, causing the rover to have large localization estimation errors the farther it gets away.

[matlabbe]

Hi,

I've implemented a more robust and smoother localization approach (including tags) in this commit last friday. Just update the code. You can then tune RGBD/MaxOdomCacheSize (default 10), the higher would give smoother localizations.

Regards, Mathieu