Your task will be to finish the prepared factorgraph localization pipeline to allow more precise localization of the robot and markers placed in the world.

Responsible lecturer: Martin Pecka (peckama2@fel.cvut.cz)

Relevant lectures: 00_1d_mle.pdf, 00_2d_mle.pdf

One of the drawbacks of ICP SLAM was that it was difficult to correct the ICP map built from historical measurements. Factorgraph SLAM allows to fully propagate all newly acquired knowledge about the world (observations/measurements) through the whole history of the robot motion and potentially fix even mapping/localization errors made earlier.

The particular task we solve in this homework has the following definition:

• As we solve the task in 2D, each pose variable or measurement is a 3-tuple that has coordinates $x$, $y$ and $yaw$.
• Let $x_t$ be robot pose 3-tuple of variables for time instant $t$, $m^r$ be relative marker pose 3-tuple of variables, and $z^{\ast}_t$ be a 3-tuple of measurements measured at time $t$. $w2r$ is a function converting from wcf to rcf (from world frame to robot frame).
• The factorgraph is defined by this optimization problem:

${\displaystyle \underset{x_0,\ldots,x_T,m^r}{\mathrm{argmin}} \sum_t ||w2r(x_{t+1}, x_t) − z^{odom}_t||^2_{\Sigma^{odom}_t} + \sum_t ||w2r(m^a, x_t) − z^{m^a}_t||^2_{\Sigma^{m^a}_t} + \sum_t ||w2r(m^r, x_t) − z^{m^r}_t||^2_{\Sigma^{m^r}_t} + \sum_t ||w2r(x_{t+1}, x_t) − z^{icp}_t||^2_{\Sigma^{icp}_t}}$

• The terms inside the norms are called residuals. These are the main point of interest to us, together with their derivatives.
• The main goal is to estimate $m^r$ as precisely as possible, and to localize the robot well enough so that it can return to its starting position (the control part of this problem will be the goal of the following homeworks).
• The problem can be formulated as Non-Linear Least Squares, for which there are plenty of solvers available. We have chosen Scikit Sparse Cholmod solver as it is fast and reliable when working with sparse matrices.
• These slides describe the math required for this homework in detail.

1. You should already have the ARO ROS environment installed from Lab 01. All the necessary files are already there.
2. To fully locally test this homework, you will also need a working implementation of Homework 3 (ICP SLAM).
3. Find places in aro_exploration/src/aro_localization/factor_graph.py with comment TODO HW 4: or FIXME: and fill in the missing pieces so that the Factorgraph SLAM works.
4. Test your implementation both using the tests described in Visualization and local testing and also interactively in simulation or on the real robots. The Brute testing dataset contains several difficult instances which will really test the quality of your implementation.
5. Also make sure that the optimization is fast enough when using the simulator, usually under 100 ms (you can see the optimization duration printed into console when debug_localization is true). Efficient implementations have no problem running this fast. If your solution is slower, have a look at turning some for-loops into Numpy vector expressions.

If you don't know how to solve some of the tasks, see Task details.

The following tests will be run to evaluate your submission:

1. Implementation of res() (1 point, required)
2. Implementation of res_jac() tested directly (2 points, required)
3. Localization of relative marker on a set of static trajectories (2 points)
   • Your factor_graph.py will be used in a manner very similar to the example usage in its factor_graph_demo() function.

Run script aro_exploration/create_hw_04_zip.sh to create an archive for Brute. Upload the archive to Brute. Your factor_graph.py file will be used on a set of static datasets to find out it works correctly. Only the factor_graph.py will be used for tests with a predefined set of configurations. So your configuration YAML files will not be used for the Brute tests.

These files are relevant for the localization. It should be okay to edit just factor_graph.py, but you might also want to change some of the YAML configs. Other files should not be modified.

• aro_exploration/config/localization contains various configurations of the package.
• aro_exploration/launch/localization provides convenient launchers for the nodes below:
  • aro_localization.launch.xml launches the localization nodes.
  • aro_localization_sim.launch.xml launches everything needed for interactive testing with Gazebo and ROS.
• aro_exploration/aro_localization/localization/aro_localization.py contains the ROS node that uses factor_graph.py module and feeds it with data coming from ROS.
• aro_exploration/src/aro_localization/factor_graph.py contains implementation of factorgraph-based localization.

You can directly run the Python module using python3 aro_localization/factor_graph.py (run it from folder aro_exploration/src; if it cannot import aro_exploration, then export PYTHONPATH=/path/to/your/student-packages/aro_exploration/src:$PYTHONPATH). There are some static data your code will be tested on. Passing these tests is mandatory for having a chance that the whole localization algorithm will work. The script will tell you the achieved marker localization accuracy on a randomly generated trajectory. Try to achieve accuracy better than 0.2 m in most cases.

You can also run python3 -m unittest aro_exploration/src/aro_localization/factor_graph.py (or instruct the IDE to run unit tests). There are some unit tests your code will be tested with. Passing these tests is mandatory (but not sufficient) for getting points for the tested functionalities.

You can also run ros2 launch aro_exploration aro_localization_sim.launch.xml world:=aro_maze_1 to test your code interactively in various simulated worlds. Read further to find out how to (interactively) control the robot.

To use your random walking algorithm from HW 02, call the roslaunch like this: ros2 launch aro_exploration aro_localization_sim.launch.xml world:=aro_maze_1 reactive_control:=true

The launch file has also parameter fuse_icp_slam which controls whether your ICP SLAM implementation generates additional factors for the localization or not. This parameter is turned on by default.

The launch file has also argument debug_localization which enables debug (green) messages in the console from the aro_localization node. This argument is set to true when launching aro_localization_sim.launch.xml and defaults to false when the localization is launched by later homeworks or the semestral work. However, you are free to pass debug_localization:=true even to these later homeworks to see how the factorgraph is doing. If, in your code, you use loginfo() or logwarn() to log some information, it will be shown regardless of the setting of this argument. Only logdebug() prints are affected.

Difficult worlds

To test the localization on a world where ICP odometry may fail horribly, test with world aro_hallway_1 and point_to_plane ICP alignment.

Gamepad teleoperation

If you have a gamepad that works on Linux, you should be able to use it to control the simulated robot. Keep top right trigger button (RB) pressed all the time and use left thumbstick to control the robot. XBox gamepads via dongle can be used with the xow or xone drivers (do not mix both!). It is, however, much easier to use them connected via USB-C cable.

On MacOS and some other computers, the touchpad may be recognized as a gamepad, too. If it is selected as the default one, use joy_dev_id:=1 argument for launch files to select a different one.

Keyboard teleoperation

If you add roslaunch argument keyboard_teleop:=true, it opens a window in which you can control the robot using WXAD keys to test your algorithm.

RViz visualization

The launch file aro_exploration/launch/localization/aro_localization_sim.launch.xml starts also a preconfigured RViz window that shows several odometry estimates. It also shows a visualization of the marker pose estimates and ground truth. Color-coding of the visualized entities is the same as in the Factorgraph visualization section. The displayed image shows a superposition of the camera image and the detected Apriltags. The small pink ellipses visualize covariance of the ICP odometry (it is enlarged for visualization purposes by a factor).

Please note that there are problems with time synchronization in the RViz window, so things will be slightly jumping around when the robot is driving. It is normal and nothing to be scared of. It is only a visualization problem.

Factorgraph visualization

The launch file aro_exploration/launch/localization/aro_localization_sim.launch.xml also opens a Matplotlib window that shows some interesting parts of the factorgraph internals. Use this window to examine the values of residuals, distances from ground truth etc.

You can see multiple odometries. The blue one is the output of the factor graph localization. Red odometry is ground truth from simulator. Green one is ICP odometry, and yellow is wheel-odometry. You can also see visualization of the poses of markers (and the estimated pose of the relative marker).

Second from top, there is visualizatíon of yaw (heading) of the robot. Color coding is the same as for the first subplot.

Third and fourth are visualizations of the residuals (errors in individual factors). The first of these two shows unscaled residuals, i.e. residuals before applying cost. The latter shows the actual residual values including costs that were used for the optimization.

Last two graphs show error of the odometries compared to ground truth (you can also observe the errors in each of the 3 estimated dimensions x, y, yaw).

Working with bag files

If you found a critical point for your algorithm you want to improve or if the Gazebo simulation is too slow on your computer, you can record a bag file of the simulation and then run your Factorgraph localization again on this bag file. This has the benefits of being much faster and much more repeatable than manually driving around.

To record the bag file, add record:=true argument to your simulation launch command. After you quit the simulation, you will find the bag file at /tmp/WORLD_NAME_loc_input where WORLD_NAME is the name of the world used in simulation.

To replay the bag file and run your ICP implementation, run ros2 launch aro_exploration aro_localization_bag.launch.xml bag:=/tmp/WORLD_NAME_loc_input.

Deadline: Before the start of Lab 9.

What to do:

• deploy your workspace on the robot
• drive it manually around (using gamepad or keyboard) (or you can use your solution of HW 2)
• run the localization on the robot, e.g. ros2 launch aro_exploration aro_localization_real.launch.xml
  • add run_driver:=false if you run the robot driver in a separate tmux window
• record data using ros2 launch aro_exploration/launch/localization/record.launch.xml record_output:=true
  • Test that the recording works correctly. Use ros2 bag info to check whether the output bag in /tmp has some messages on topic fused_odom.
• try to create some difficult situations for localization when recording the data
• take a few photos or screenshots during your try
• write a 1-2 page PDF report analyzing performance of your algorithm

What to submit: A single ZIP archive containing the BAG file(s), PDF and photos. File upload limit is 350 MB. Submit the archive to task hw04-bonus in Brute.

There are a few differences between the simulator and the real robot:

• In simulation, marker detection is done from RGB camera, while on the real robot, it is done from near infrared camera. Try to figure out why!
• In simulation, the bumper is the whole body of the robot. On the real robot, the bumper only covers the front part of the robot.
• Quality of the wheel-IMU odometry differs between the simulation and the real robot.
• In simulation, ground truth position is available for free. To get something similar for the real robot, a complicated setup of the room would be required.

• The expressions $||X - Z||^2_{\Sigma}$ are the so-called Mahalanobis distance of the residuals and are equal to $(X-Z)^T\Sigma^{-1}(X-Z)$. Mathematically, you can convert them to L2 distance of altered residuals that look like $||\Sigma^{-\frac{1}{2}}(X-Z)||^2$.
  • Please note the $\Sigma^{-\frac{1}{2}}$ is the inverse of the matrix square root of $\Sigma$, and not a pointwise inverse square root. In Python, you can use scipy.linalg.sqrtm() for the matrix square root.
• The factorgraph state and measurements are expanded by one “time slice” with incoming wheeled odometry measurements (topic /odom) or marker detections (topic /apriltag). However, the odometry normally comes at 10 Hz, which would cause the FG to grow too quickly. So the ROS node “subsamples” the odometry by taking one measurement every second. This means every second, a new $x_t$ will be added to the FG, with $z^{odom}_t$ and other measurements filled if they exists for the given time instant. To better represent marker observations, if a marker is observed, a new state is created immediately and odometry measurements are interpolated. The marker-generated states are limited to one each 0.25 s.
  • This subsampling is called preintegration.
• The factor graph is internally represented as the following table:

• Put correct formula in function res() in factor_graph.py. Further in the text, we will use the following residuals induced by measurements at time $t$
  • $res^{odom}_t$ is the odometry residual that is a function of $x_t$, $x_{t+1}$ and $z^{odom}_t$ (denoted res_odom in code).
  • $res^{mr}_t$ is the relative marker residual that is a function of $x_{t1}$, $x_{t2}$, $mr$ and $z^{mr}_t$ (denoted res_mr in code).
  • $res^{ma}_t$ is the absolute marker residual that is a function of $x_{t1}$, $x_{t2}$ and $z^{ma}_t$ (denoted res_ma in code).
  • $res^{icp}_t$ is the ICP odometry residual that is a function of $x_t$, $x_{t+1}$ and $z^{icp}_t$ (denoted res_icp in code).
  • Each residual is the error between the observation $z$ and the estimated robot state $x$. All residuals we use are relative poses. Odometry and ICP odometry measurements tell the relative motion of the robot between two time instants (in robot frame). Marker measurements tell the relative pose of the marker in the robot frame.
• Put correct formulas in the definition of J and J1 in res_jac() function. Each of these is a 3×3 submatrix of the whole factorgraph Jacobian.
  • J is $\frac{\partial res^{?}_t}{\partial x_t}$.
  • J1 is $\frac{\partial res^{?}_t}{\partial x_{t+1}}$ or $\frac{\partial res^{?}_t}{\partial mr}$ .
• You can play with the solver_options defaults which defines behavior of the nonlinear least squares solver when running factor_graph.py directly, or you can edit aro_exploration/config/localization/solver.yaml and aro_exploration/config/localization/costs_icp.yaml to change behavior of the solver when testing interactively in ROS. But it should work okay even with the default settings.
• To efficiently solve the Non-Linear Least Squares problem, it is essential to represent the

Jacobian as a sparse matrix.

In this homework, you are not required to touch the ROS node at all. It is quite complicated so we did all the heavy lifting for you. However, it might come handy to know how does the node behave.

This is definition of the API of the aro_localization node.

Only the important parameters are listed here. For the full view, refer to aro_exploration/aro_exploration/localization/aro_localization source code.

• map_frame (string, default fused_odom): Name of the frame in which the fused localization will be published.
• odom_frame (string): This is not an actual parameter, but we refer to it in this document as if it were. This is name of the frame in which wheel-IMU odometry is published. The “parameter” is automatically set to the parent frame of the first odometry message on topic odom.
• body_frame (string): This is not an actual parameter, but we refer to it in this document as if it were. This is name of the robot body frame. The “parameter” is automatically set to the child frame of the first odometry message on topic odom.
• publish_tf (bool, default True): Whether to publish the map_frame → odom_frame transform.
• invert_tf (bool, default True): Whether to publish transform map_frame → odom_frame (False) or odom_frame → map_frame (True).
• max_iters (int, default 50): Maximum number of optimizer iterations.
• Parameters starting with c_ configure the scale of costs of the individual loss terms. You can play with these to tune the algorithm behavior.
• The node reads the following parameters from node /marker_config to recieve the marker properties:
  • abs_marker_id, abs_marker_x, abs_marker_y, abs_marker_yaw: Properties of the absolute marker (all required).
  • rel_marker_id: ID of the relative marker.
  • rel_marker_gt_x, rel_marker_gt_y, rel_marker_gt_yaw: Position of the relative marker. Not available in Brute tests or semestral work runs on real robots.

• apriltag (type [https://github.com/christianrauch/apriltag_msgs/blob/master/msg/AprilTagDetectionArray.msg|apriltag_msgs/AprilTagDetectionArray]]) Detected apriltags.
• odom (type nav_msgs/Odometry) Wheel-IMU odometry provided by the platform (short-term accurate, long-term inconsistent, no discrete jumps, regular frequency ~10 Hz). Position estimate is used, velocity estimate is not used.
• icp_odom (type nav_msgs/Odometry) Odometry from SLAM node (if available, short-term accurate, long-term consistent, discrete jumps, mostly regular frequency ~10 Hz). Position estimate is used, velocity estimate is not used.
• ground_truth_odom (type nav_msgs/Odometry) Ground truth odometry (only in simulator and in testing mode, short-term accurate, long-term consistent, no discrete jumps, regular frequency ~10 Hz). Position estimate is used, velocity estimate is not used.
• mobile_base/sensors/core (type kobuki_ros_interfaces/SensorState) State of the Turtlebot2 real robot sensors. Only the bumper state is read.
• bumper (type ros_gz_interfaces/Contacts) Bumper state from simulator.

• fused_odom (type nav_msgs/Odometry) The main output of the localization. The best estimate of robot position in frame map_frame (parameter of the launch file).
• fused_odom_viz (type visualization_msgs/MarkerArray) Visualization helper for displaying the estimated (and ground truth) poses of the markers.
• fused_odom_path (type nav_msgs/Path) Helper output of the localization algorithm. It represents the whole path in map_frame from the instant the localization node was started. Publishing the whole path is usually inefficient, but it is convenient for debugging and evaluation.
• relative_marker_pose (type geometry_msgs/PoseStamped). The estimated pose of the relative marker.

• map_frame (node parameter) → odom_frame (node parameter): Best estimate of the robot pose from the fused localization algorithm. Short-term accurate, long-term consistent (if markers are observed regularly), discrete jumps, irregular frequency 10 Hz to 0.01 Hz.
• map_frame_fast → body_frame: Debug/helper frame that is a composition of the latest map_frame → odom_frame and the latest odom_frame → body_frame. Short-term accurate, long-term inconsistent, discrete jumps, regular frequency ~20 Hz. This frame is published by separate node fused_map_fast_publisher.

• marker camera frame → body_frame (node parameter): Calibrated static transform between the marker detection camera's optical frame and the body frame of the robot.
• odom_frame (node parameter) → body_frame (node parameter): Wheel-IMU odometry.

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: