3D sensor + motion models

[giafranchini]

Implement 3D sensors and motion models plugins

motion models

• [x] unicycle_3d --> move to omnidirectional3d

sensor models

• [x] odometry_3d
• [x] imu_3d

[giafranchini]

Unicycle 3D motion model update state

HEADERS

• [x] unycycle_3d
• [x] unicycle_3d_ignition
• [x] unicycle_3d_predict
• [x] unicycle_3d_state_cost_function
• [x] unicycle_3d_state_cost_functor
• [x] unicycle_3d_state_kinematic_constraints

CPP

• [x] unycycle_3d
• [x] unicycle_3d_ignition
• [x] unicycle_3d_state_kinematic_constraints

[giafranchini]

Odometry 3D sensor model update state

HEADERS

• [x] odometry_3d
• [x] odometry_3d_publisher
• [x] common/sensor_proc

CPP

• [x] odometry_3d
• [x] odometry_3d_publisher

TODO:

• [x] porting of all the 3d functions in sensor_proc
  • absolute pose cost function with analytical derivatives
  • relative pose cost function with analytical derivatives
  • remove cov_geometry dep from relative pose

DONE:

• [x] odometry3d publisher
  • [x] missing predict_to_current_time since we did not implemented the jacobian in unicycle 3d predict
  • [x] check how to correctly publish covariances in relation to selected measurement indices
• [x] odometry3d:
  • [x] process of differential measurements does not take into account the translational part of the constraint, but only the rotational part (works fine, but the difference between the 2 poses is just really small AND the parameter differential in fuse has a different meaning from the one in RL)
  • [x] add compatibility with position and orientation indexes selection

[giafranchini]

IMU 3D sensor model update state

HEADERS

• [x] imu_3d
• [x] imu_3d_params

CPP

• [x] imu_3d