====== Lab 05 ======

During this lab, you should learn how to work with factorgraph-based SLAM and how to run it in the simulator.

**Relevant lectures:** {{ :courses:aro:lectures:00_1d_mle.pdf |}}, {{ :courses:aro:lectures:00_2d_mle.pdf |}}.
===== Factorgraph SLAM in action =====

https://www.youtube.com/live/YCE1Aj0k1UA?feature=share&t=2045

The whole [[https://theairlab.org/tartanslamseries2/|Tartan SLAM Series]] is a great study material for those who want to dive deep into how SLAM in 3D is done in state-of-the-art robotics.

===== KF vs Factorgraph SLAM =====

What is the difference between (E)KF SLAM and Factorgraph SLAM?

^       ^ (E)KF       ^ Factorgraph          ^
| **State**    | Latest robot position, relative marker positions | All robot positions, relative marker positions |
| **Memory Requirements** | Constant in trajectory length, linear in #markers | Linear in trajectory length, linear in #markers |
| **Loop Closures** | Only help current position estimate and markers | Help with whole trajectory estimate and markers |

{{ :courses:aro:tutorials:01_ekf.gif |}}

/*
==== Lab Task ====

Download {{ :courses:aro:tutorials:ekf_slam_simple.py |}} and {{ :courses:aro:tutorials:drw_tools.py |}} and examine the files. Find places commented with ''PLAY HERE'' and try to find a way to break the EKF optimization. You can also edit other parts of the code. By replacing ''opt = ekf'' with ''opt = fg'', you'll instruct the script to do the estimation using Factorgraph instead. What are the differences? Can you break the Factorgraph?
*/
===== Computing Jacobians for factorgraphs =====

{{ :courses:aro:tutorials:aro_hw4_2024.pdf |}}


===== RUR Challenge Worlds =====

What would the residuals and Jacobian entries look like?

  * Reasonable factors:
    * 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 [[https://github.com/ctu-mrs/uvdar_core|LED in camera]] (cannot tell its distance): $res_t^{led} = ?$
  * Silly factors (RUR Challenge). Figure out a sensor that could use them.
    * Slow light propagation: Marker detections are delayed.
    * Gravity field changing in space (can you try to map it?).
    * Time "speed" distortion changes with position (+ sensor that measures sin(distortion)). (Darius Diebold)
    * Gravitational field that changes rapidly with position on surface (+ sensor that measures sin(||x||)). (Laura G. Hernandez)
    * Wavy gravity field (+ sensor that measures sin(x*x)). (Marc Simon)
    * Mass of drilling robot increases exponentially with depth (+ sensor that measures x*exp(-x)). (Jan Kubis)
    * Space-dependent time distortion (+ sensor that measures x+sin(x+distortion)). (Stanislav Kunst)
    * Gravity depends on sin(x) (+ sensor that observes a landmark and measures sign of gravity). (Michal Meciar)
    * Pose updates with x + sin(x) + u (+ sensor that measures log(1 + x*x)). (Mark Horpynych)
    * Gravity decreases with r^(1.5), (+ sensor that localizes known black holes). (Stefan Kando)
    * Light propagates according to c * log(t) (+ sensor that measures exp(||x||) and angle to markers). (Votjech Klouda)
    * Gravity field is influenced by cosine of position (+ sensor that measures sin(omega * x)). (Matej Mihulka)
    * Strong magnetic field causing observable Faraday rotation (+ sensor that measures 1/x). (Tomas Ort)
    * Magic crystals slowing down time (+ sensor that measures 1/||x-m||). (Valeriia Timonova)
    * Gravity as a sine wave (+ time-delayed sensor that measures sin(x-m)). (Shaawaiz Haider)
    * Teleportation world (+ sensor measuring sine of position). (Theodoros Kokkoris)
    * Time-warping on ash-filled orbit (+ sensor that measures sin(x)). (Muhammad Moeed Zeb)
    * Non-Newtonian fog (+ sensor that measures exp(|x-m|^2)). (Adam Cetlovsky)
    * Time-varying gravity (+ drunken shepherd sensor that measures gravity). (Jozef Gal)
    * World with waving string interaction strength, materials transmuting all the time (+ sensor that measures ln(1 + x)). (Krystof Svejda)
    * Gas giant world (+ sensor measuring (x-m)^2). (Vit Zoubek)
    * Objects with same phases attracted (+ sensor that measures sin(r+phi)). (Patrik Martisko)
    * Friction is inversely proportional to sound level (+ sensor that measures acoustic slip). (Kyaw Htet Lin)

/*
    * Gravity field constant in space but changing in time following a defined known function.
    * The universe randomly switches left-right symmetry (i.e. mirrors the world along an axis).
    * The robot is on a Little Prince planet with a very small diameter. The circumference of the planet is so small the robot does not know how many times it had circled around the planet when he has moved and received a new GPS measurement.
    * The robot inspects pipelines, drags a "tape measure" behind it and can read how much of the tape has been unrolled.
    * Friction (and thus efficiency of control commands) depends on orientation of the robot w.r.t. world.
    * Weird gravity acting to the side and a sensor measuring a wild function of the robot state (Ondřej Matoušek, matouon5)
    * Timestone and Gravitime force affecting the flux of time in different parts of the space (Matěj Trnka, trnkamat)
*/

===== Homework 4 assignment =====

Read and try to understand the assignment of the homework [[homework04|HW4]].