Search
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_localization_mle.pdf, 00_localization_se2.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:
${\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}}$
aro_exploration/src/aro_localization/factor_graph.py
TODO HW 4:
FIXME:
debug_localization
The following tests will be run to evaluate your submission:
res()
res_jac()
factor_graph_demo()
Run script aro_exploration/create_hw_04_zip.sh to create an archive for Brute. Upload the ZIP 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 and the YAML configs will be used from your codebase - all other files will be ignored (your icp.py implementation from HW03 is also a part of the archive, but it is not tested in HW04).
aro_exploration/create_hw_04_zip.sh
factor_graph.py
Deadline: Before the start of Lab 9.
What to do:
roslaunch aro_exploration aro_localization_real.launch
run_driver:=false
roslaunch --screen aro_exploration/launch/localization/record.launch record_output:=true
rosbag info
fused_odom
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.
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
aro_exploration/launch/localization
aro_localization.launch
aro_localization_sim.launch
aro_exploration/nodes/localization/aro_localization
You can directly run the Python module using python3 aro_exploration/src/aro_localization/factor_graph.py. 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.
python3 aro_exploration/src/aro_localization/factor_graph.py
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.
python3 -m unittest aro_exploration/src/aro_localization/factor_graph.py
You can also run roslaunch aro_exploration aro_localization_sim.launch world:=aro_maze_1 to test your code interactively in various simulated worlds.
roslaunch aro_exploration aro_localization_sim.launch world:=aro_maze_1
To use your random walking algorithm from HW 02, call the roslaunch like this: roslaunch aro_exploration aro_localization_sim.launch world:=aro_maze_1 reactive_control:=true
roslaunch aro_exploration aro_localization_sim.launch 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.
fuse_icp_slam
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 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.
aro_localization
debug_localization:=true
loginfo()
logwarn()
logdebug()
Difficult worlds
To test the localization on a world where ICP odometry may fail horribly, test with world aro_hallway_1 and point_to_point ICP alignment.
aro_hallway_1
point_to_point
Gamepad teleoperation
If you have a gamepad that appears as `/dev/js0` device in Linux, you should be able to use it to control the simulated robot. Keep button A pressed all the time and use left thumbstick to control the robot. XBox gamepads can be used with the xow or xone drivers (do not mix both!).
A
It can happen that your gamepad model is not recognized. In such case, the console would get spammed by a lot of error messages. To get rid of them, disable gamepad control by adding roslaunch argument joy_teleop:=false.
joy_teleop:=false
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.
keyboard_teleop:=true
RViz visualization
The launch file aro_exploration/launch/localization/aro_localization_sim.launch 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).
aro_exploration/launch/localization/aro_localization_sim.launch
Factorgraph visualization
The launch file aro_exploration/launch/localization/aro_localization_sim.launch 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).
There are a few differences between the simulator and the real robot:
scipy.linalg.sqrtm()
/odom
/apriltag
res_odom
res_mr
res_ma
res_icp
J
J1
solver_options
aro_exploration/config/localization/solver.yaml
aro_exploration/config/localization/costs_icp.yaml
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/nodes/localization/aro_localization source code.
map_frame
odom_frame
odom
body_frame
publish_tf
invert_tf
max_iters
c_
apriltag
icp_odom
ground_truth_odom
mobile_base/sensors/core
bumper
fused_odom_viz
fused_odom_path
relative_marker_pose
map_frame_fast
fused_map_fast_publisher