Calibrating world to base_footprint in softbot

[miguelriemoliveira]

@brunofavs , when you do this you will have this error.

ATOM Error: Additional_tf world_to_base_link defines transformation world to base_footprint to be calibrated but this transformation is not static and therefore cannot be calibrated.

Talk to me when you get there.

[brunofavs]

Hey @miguelriemoliveira Ok I will mention or email you when I get there. I already know what will trigger that crash, but I still have some things prior to complete

[brunofavs]

I will work on this now.

Earlier today, I wrote a bit on my blog about a few theoretical doubts I have regarding this matter :

~https://www.notion.so/Week-6-cb45af3b97174e7a8c672b343303879e?pvs=4#b78f40c94f144d37b984be6c7b931e4b~

Forgot this is private I don't know of permissions work on Notion so here's a screenshot :

[miguelriemoliveira]

Hi @brunofavs ,

we can talk about it in the meeting in a moment ...

[miguelriemoliveira]

Related to #581

[miguelriemoliveira]

So I just changed the configure_calibration_package to allow for additional_tfs to be dynamic. Then created a dataset and tried to calibrate.

As expected, the calibrate creates an optimization problem with the following parameters:

Parameters:
                                                                                                        Group        x0         x      data  Min  Max
lidar3d_plate_link-lidar3d_base_link_x                                   lidar3d_plate_link-lidar3d_base_link  0.000000  0.000000  0.000000 -inf  inf
lidar3d_plate_link-lidar3d_base_link_y                                   lidar3d_plate_link-lidar3d_base_link  0.000000  0.000000  0.000000 -inf  inf
lidar3d_plate_link-lidar3d_base_link_z                                   lidar3d_plate_link-lidar3d_base_link  0.000000  0.000000  0.000000 -inf  inf
lidar3d_plate_link-lidar3d_base_link_r1                                  lidar3d_plate_link-lidar3d_base_link  0.000000  0.000000  0.000000 -inf  inf
lidar3d_plate_link-lidar3d_base_link_r2                                  lidar3d_plate_link-lidar3d_base_link  0.000000  0.000000  0.000000 -inf  inf
lidar3d_plate_link-lidar3d_base_link_r3                                  lidar3d_plate_link-lidar3d_base_link  0.000000  0.000000  0.000000 -inf  inf
front_right_camera_link-front_right_camera_rgb_frame_x   front_right_camera_link-front_right_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
front_right_camera_link-front_right_camera_rgb_frame_y   front_right_camera_link-front_right_camera_rgb_frame -0.045000 -0.045000 -0.045000 -inf  inf
front_right_camera_link-front_right_camera_rgb_frame_z   front_right_camera_link-front_right_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
front_right_camera_link-front_right_camera_rgb_frame_r1  front_right_camera_link-front_right_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
front_right_camera_link-front_right_camera_rgb_frame_r2  front_right_camera_link-front_right_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
front_right_camera_link-front_right_camera_rgb_frame_r3  front_right_camera_link-front_right_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
world-base_footprint_x                                                                   world-base_footprint  0.209351  0.209351  0.209351 -inf  inf
world-base_footprint_y                                                                   world-base_footprint -0.692575 -0.692575 -0.692575 -inf  inf
world-base_footprint_z                                                                   world-base_footprint -0.001008 -0.001008 -0.001008 -inf  inf
world-base_footprint_r1                                                                  world-base_footprint  0.000348  0.000348  0.000348 -inf  inf
world-base_footprint_r2                                                                  world-base_footprint -0.002264 -0.002264 -0.002264 -inf  inf
world-base_footprint_r3                                                                  world-base_footprint  0.304454  0.304454  0.304454 -inf  inf
front_left_camera_link-front_left_camera_rgb_frame_x       front_left_camera_link-front_left_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
front_left_camera_link-front_left_camera_rgb_frame_y       front_left_camera_link-front_left_camera_rgb_frame -0.045000 -0.045000 -0.045000 -inf  inf
front_left_camera_link-front_left_camera_rgb_frame_z       front_left_camera_link-front_left_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
front_left_camera_link-front_left_camera_rgb_frame_r1      front_left_camera_link-front_left_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
front_left_camera_link-front_left_camera_rgb_frame_r2      front_left_camera_link-front_left_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
front_left_camera_link-front_left_camera_rgb_frame_r3      front_left_camera_link-front_left_camera_rgb_frame  0.000000  0.000000  0.000000 -inf  inf
world-pattern_link_x                                                                       world-pattern_link  2.131433  2.131433  2.131433 -inf  inf
world-pattern_link_y                                                                       world-pattern_link  0.240979  0.240979  0.240979 -inf  inf
world-pattern_link_z                                                                       world-pattern_link  0.798899  0.798899  0.798899 -inf  inf
world-pattern_link_r1                                                                      world-pattern_link -1.783295 -1.783295 -1.783295 -inf  inf
world-pattern_link_r2                                                                      world-pattern_link  0.759856  0.759856  0.759856 -inf  inf
world-pattern_link_r3                                                                      world-pattern_link -1.262079 -1.262079 -1.262079 -inf  inf

where we can see that the world-base_footprint transformation is only one for all collections.

[miguelriemoliveira]

First problem is that the calibrate script has no access to information about whether a transform is dynamic or fixed. This is generated in configure_calibration_pkg, but not saved anywhere.

After talking to @manuelgitgomes we decided on the following:

1. The configure_calibration_pkg saves a \<pkg>_calibration/calibration/transforms_graph.json file.
2. The collect data script reads this file and creates a transforms dictionary to be saved into the dataset.

This is how the new dataset looks:

3. The calibrate will then use this information to see if the additional_tfs are dynamic or fixed.

[miguelriemoliveira]

related to #895

[miguelriemoliveira]

I think I got it. The sparse matrix is now like this

https://docs.google.com/spreadsheets/d/1MJ4Qz1veFdkAF22Q3xv81e2baGAqEod0w-gsXHzLVjk/edit?usp=sharing

Makes sense?

[miguelriemoliveira]

Hi @brunofavs ,

this is now working. You can define a dynamic_tf as an additional transform.

[manuelgitgomes]

Makes sense?

Seems fine! Great job!

[brunofavs]

I will test it soon :)

[brunofavs]

It's not fine :C

Now no prior dataset is compatible anymore.

[miguelriemoliveira]

Hi @brunofavs ,

let me know what happens so I can try to fix.

What is the error?

[miguelriemoliveira]

Ooops. You already created another issue. Closing this one.