Lab 06

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

Lecturer: Martin Pecka (peckama2@fel.cvut.cz)

Relevant lectures: 00_localization_mle.pdf, 00_localization_se2.pdf, 00_kf.pdf, 01_ekf.pdf.

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

The whole 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.

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

Download ekf_slam_simple.py and 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?

aro_hw4_2024.pdf

What would the residuals and Jacobian entries look like?

• Reasonable factors:
  • 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 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?).
  • 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)

Fig. 1: Example of Apriltags. Image courtesy of https://april.eecs.umich.edu/software/apriltag.

Apriltags are visual markers designed specifically so that it is easy to estimate their pose in full 6 DOF (x, y, z, roll, pitch, yaw). Thus they are very comfortable for being used as absolute localization markers. Moreover, they are easily distinguishable from each other, so the detector can output not only the 6 DOFs of the marker, but also its unique ID.

Here is an example of a tag localized by the ROS node from package apriltag_ros:

Read and try to understand the assignment of the homework HW4.