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: 00_1d_mle.pdf, 00_2d_mle.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

Boost your creativity and come up with an original sci-fi postapocalyptic universe with:

• 1) Unique law of physics (e.g. variable gravity even negative, speed of light = 60km/h)
• 2) Unique sensor with non-linear measurement function (e.g. z = sin(x) )
• 3) Unique state estimation problem (describe + draw corresponding factorgraph)
• 4) Implement a two-state example in pytorch
• 5) Optionally: use the AI image generative model to create supplementary art materials
• 6) Upload to BRUTE (rur task)

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?).

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.