====== Lab 06 ======

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

===== Other types of residuals =====

What would the residuals and Jacobian entries look like?

  * Global absolute localization (GNSS, Vicon, RFID): $res_t^{gps} = ?$ /*||x_t - z_t^{gps}||*/
    * 2-DOF, 3-DOF
  * Compass: $res_t^{compass} = ?$
  * Absolute pose priors: $res_t^{prior} = ?$ /* ||x_t - x_t^{prior}|| */
  * Interpolate marker measurement between two poses for better precision: $res_t^{mri} = ?$
  * Motion model (e.g. differential drive model): $res_t^{motion} = ?$ /*||g(x_{t-1}, u_t) - x_t||*/
    * How to construct the model if $u_t$ are wheel velocities?
  * Loop closures: $res_t^{loop} = ?$  /* ||x_i - x_j|| */
  * 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 [[https://arxiv.org/pdf/1701.03077v1.pdf|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} = ?$

===== Lab Task =====

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?