Implementation of TF derivation errors in ATOM's optimization for IMU calibration

[Kazadhum]

Hello @miguelriemoliveira and @rarrais!

Continuing my work from #1015 and #1017...

I am now working on implementing the inclusion of the derivation errors from #1015 in the ATOM optimization process, in order to calibrate the extrinsic parameters of IMU sensors.

I will update this issue with my progress as I go along!

[Kazadhum]

The optimization process runs, but I'm ran into an issue where the errors remain the same before and after optimization.

After some testing, I've determined that this is caused by a bug where the optimized tfs don't replace the old tfs in the objective function. Working on the fix...

[Kazadhum]

I tried using all of the data in continuous_data but I think this is leading to some errors... Will try to use only collections like the rest of ATOM to make it a simpler problem and to better integrate it into ATOM.

[Kazadhum]

Hi @miguelriemoliveira and @arrais!

After changing to using only the data from each collection (as opposed to using the errors at every datapoint in the dataset), the problem still persisted.

What was happening was that the optimization process wasn't leading to a significant decrease in the residuals. A variety of bugs was behind the problem, which have now been fixed. Here is a video of the optimization process at work:

test7.mp4

I want to be able to visually show the pose being optimized in RViz but I still need to make some adjustments to the calibrate script for the ros_visualization flag to work. I'll work on it now.

[miguelriemoliveira]

Hey @Kazadhum ,

very nice. Congrats!

I am curious about the bugs you mentioned.

Optimization is optimizing, but sometimes this just means the optimizer is being able to reduce the error.
To be sure your approach is working well we need the comparison to the ground truth.

Do you have it?

[Kazadhum]

I am curious about the bugs you mentioned.

There were two problems:

• I was adding noise to the TF via calibrate (the normal way) but then also adding the noise inside the derivation function, so it was re-adding random noise every iteration of the optimization and the residuals weren't changing.
• There was an error in a variable name which led to not compensating for gravity when calculating an error. This led to the error in the linear acceleration along Z being almost constant at 9.81.

Optimization is optimizing, but sometimes this just means the optimizer is being able to reduce the error. To be sure your approach is working well we need the comparison to the ground truth.

Do you have it?

Yes! That appears to be happening. Here is the comparison table:

+-------------------------+-------------+---------+---------+-------------+------------+
|        Transform        | Description | Et0 [m] |  Et [m] | Rrot0 [rad] | Erot [rad] |
+-------------------------+-------------+---------+---------+-------------+------------+
| base_link-accelerometer | imu_chassis | 0.05389 | 3.00870 |   0.09639   |  0.00275   |
|         Averages        |     ---     | 0.05389 | 3.00870 |   0.09639   |  0.00275   |
+-------------------------+-------------+---------+---------+-------------+------------+

So it seems there are still bugs about...

[miguelriemoliveira]

yeah. Translation is getting absurdely wrong ... but rotation does decrease significanlty