Feature/lidar features bundle adjustment

mariusbruehlmeier commented 4 years ago

Overview

This PR includes the parts of the LiDAR features pipeline that are run in postprocessing. This includes loop closure, bundle adjustment, and retriangulation. It does not include map building, feature extraction, feature tracking, or image generation. These parts are on the branch that contains the whole pipeline.

A short summary of the changes:

ceres_error_term: A new error term is defined and implemented for the LiDAR features. It is a Euclidean error between the position of the landmark in the map and the 3D measurement of the LiDAR. Generally, the added files are similar to the existing visual error term. Note, the only new Jacobian is J_p_C_fi_wrt_p_C_I , which is an Identity matrix (lidar-error-terms-inl.h l.126). All other Jacobians are derived from this matrix.
map-optimization: A new function, AddLidarErrorTerm, that is very similar to AddVisualTerm. The new function gets called if there is 3D data (KeypointVectors) in the queried visual frame. Per default, this new error term is not added to the optimization. It can be added with either the command optvi --ba_include_lidar=true or the new command optvil, which adds visual and LiDAR features.
__aslam_cv__: The new visual frame includes additional channels for 3D measurements, 2D measurements, Track IDs and descriptors of LiDAR features. The PR for this is here.
landmark-triangulation: Added new command retriangulate_lidar_landmarks / rtll to retriangulate LiDAR features.
__loop_closure__: If the camera of the loop closure is a LiDAR camera, then the loop closure will use a 3D RANSAC to check for geometric consistency. This RANSAC is in aslam_cv. Otherwise, the loop closure engine is the same as for visual mapping. Note, that at the moment only frames that contain either visual or LiDAR data can be loop-closed.

Testing

A dataset of LiDAR features can be found here. It contains visual frames with only LiDAR data and was generated by running the complete pipeline and launched with roslaunch maplab_node maplab_node_rovioli_lidar_features.launch

Possible tests:

Landmark triangulation --> rtll with different LiDAR landmark quality settings
Bundle adjustment --> optvil --ba_include_visual=false, if you run just optvil, it will try to add visual constraints to the optimization problem and give a warning as there are none. Note that it takes much more iterations (in this data set: ca. 173 iterations) to converge compared to pure visual.
Loop closure --> lc
Landquark quality --> ellq

There is also a second data set, which is a vi-map with frames that contain only visual data. It should be possible to use this and any other dataset with the new LiDAR features.

Open Questions

Loop closure: Works poorly and is more specific to my thesis than the other parts. Does it belong here?
Map building: Still in the other branch. Should it be here?

Results

A visualization of the bundle adjustment is shown here:

LiDAR bundle adjustment