Integration week changes - Improve maplab_node <-> maplab_server

mfehr commented 4 years ago

This PR is a bit a mixed bag and contains various bug fixes, and small features as a result of deploying the system onto actual robots and testing it.

Changes

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
- We decided to not use this feature in the end though, as relax can be quite unstable and the risk was greater than the potential benefit.
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.
Add a flag to set IMU biases to zero at when receiving them as part of the odometry message in the maplab node.
Add flag to limit the max optimization time.
Add options to accumulate point clouds only for missions with known base-frame
Add timestamp prefix to submap folders (to prevent collisions later on when running the node again)
Add option to attach lc constraints to stationary maps. The idea is to use this as a simple way to enforce a sort of zero velocity update in the pose graph.
Publish tf tree of whole sensor setup based on calibration file.
Improve handling of map anchoring in the maplab server. When a mission has absolute pose constraints, it is already considered anchored, as we do have an absolute reference and can set the base-frame accordingly. This is done in the "aam" command, which tries to anchor the missions with visual loop closures.

Remaining issues:

Adding more constraints to the pose graph increases the complexity and makes param tuning. We observed some issues regarding odometry vs visual constraints. This needs some more work/tuning to run smoothly for a specific robot/setup.

Contributions / Credits:

The optimization integration of the odometry constraints was already 99% done thanks to the 6DoF-capable relative pose constraint implementation that was added to support wheel odometry by @smauq and @b-hahn
@LBern, @victorreijgwart, @gasserl