Improvements to the maplab server & Adding option to include odometry constraints

mfehr commented 4 years ago

This PR is a bit a mixed bag and the result of our attempt to improve the robustness of the maplab_server_node.

The problems this PR addresses are:

Since the maplab_server_node can only run few optimization steps each iteration, if a large loop closure is found it took a while for the map to converge
The maps sometimes diverged, in nearly all cases this happened when there were few/no visual observations between some frames.

To address this the following changes/new features were implemented:

There is an option to attach 6DoF odometry constraints in between vertices. These constraints are simply derived from the Odometry6DoF sensor that is used to build the map in the maplab_node. Activate with maplab_node flag: --map_add_odometry_edges_if_less_than_n_common_landmarks Options:
- -1 Don't add edges (default)
- 0 Add between all vertices
- n Only if the number of co-observed landmarks is < n
The 6DoF edges can be added to the optimization problem using the following flags:
- --ba_include_6dof_odometry
- --ba_fix_6dof_odometry_extrinsics
Use the pose graph relaxation command in the maplab_server_node to close large loops faster. However since this command executes a full loop closure search, but doesn't merge landmarks, it wouldn't fully replace the loop closure. Hence we introduce an option to merge landmarks as well, which should result in a relax command that contains all functionality of the the lc command as well. The flag is --lc_relax_merge_landmarks
Some general improvements to the maplab_server_node, mostly restructuring to make the code a bit more readable.
maplab_server_node publishes T_G_M and T_G_B (both as they are currently and as they were when the map arrived at the server) for the most recent vertex of each robot after each iteration.

Credits: The optimization integration was already 99% there thanks to the 6DoF-capable relative pose constraint implementation that was added to support wheel odometry by @smauq and @b-hahn