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?
(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 |
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?
What would the residuals and Jacobian entries look like?
bounds
parameter of least_squares
.
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.