Lab 06

During this lab, we will continue with factorgraph-based localization.

What would the residuals and Jacobian entries look like?

• Global absolute localization (GNSS, Vicon, RFID): $res_t^{gps} = ?$
  • 2-DOF, 3-DOF
• Compass: $res_t^{compass} = ?$
• Absolute pose priors: $res_t^{prior} = ?$
• Interpolate marker measurement between two poses for better precision: $res_t^{mri} = ?$
• Motion model (e.g. differential drive model): $res_t^{motion} = ?$
  • How to construct the model if $u_t$ are wheel velocities?
• Loop closures: $res_t^{loop} = ?$
• Velocity measurements in body frame: $res_t^{vel} = ?$
• UWB localization (radio beacons with distance measurement): $res_t^{uwb} = ?$
• UWB relative marker: $res_t^{uwbm} = ?$
• Bluetooth detection (radio beacons without distance measurement): $res_t^{bt} = ?$
  • This introduces inequality constraints which are generally not very well handled.
  • You can use a robust loss to approximate the inequality.
  • Or you can pass the inequality bounds to the bounds parameter of least_squares.
• Marker as LED (cannot tell its distance): $res_t^{led} = ?$

Use the same codes you already have in your workspace, and make sure you also have a working SLAM implementation in the workspace (aro_slam package). Fill in the missing pieces to add ICP-based odometry (marked with a TODO comment).

Run with:

roslaunch --sigint-timeout 1 --sigterm-timeout 1 aro_localization sim.launch fuse_icp_slam:=true

Watch how the factorgraph struggles keeping up with the ICP odometry. Try to explain that!

Change the code so that ICP odometry is the “base one” and IMU-wheel odometry is just an “addon” to the factorgraph. Does it behave better now?