This weights IMU based on the time between nodes in the
pose graph optimization.
When moving slowly or stopping, IMU weights are reduced.
This improves quality in these cases. The parameters are
changed to approximately get the same behavior while
moving as before for the examples.
Signed-off-by: Wolfgang Hess <whess@lyft.com>
This PR adds the possibility to use the TolerantLoss function for INS in pose graph optimization. This is currently switched off, and so there's not functional change.
The ceres TolerantLoss function (see [description](http://ceres-solver.org/nnls_modeling.html), and [implementation](https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/loss_function.cc)) has the following property
- for large values of x it approaches a quadratic loss ("null loss") with the specified weight (fixed_frame_pose_translation_weight)
- for small values of x it approaches a quadratic loss with a smaller weight
- there's a crossover at some value x_c. The function is convex everywhere.
Add the parameter `OptimizationProblemOptions.use_online_imu_extrinsincs_in_3d`. Set it to true by default to not change existing behavior.
Using online IMU extrinsics is not always desirable, and particularly during localization, we have determined that it is preferable to turn this feature off.
"scan" is better named "node" if it refers to a node in global SLAM.
"penalty factor" is renamed "weight" for consistency with other similar options.
"scans_per_accumulation" is renamed "num_accumulated_range_data" to match the code and since the accumulated data is not called "scan".
Wolfgang Hess
Susanne Pielawa
Kevin Daun
Christoph Schütte
Arno Fleck
Michael Grupp
Wolfgang Hess