Before this commit, the auto angular resolution computation relied on the assumption
that each scanline was already sorted by increasing timestamp/azimuth.
This implied a very specific organization of input cloud.

Now, each scanline is sorted, so this assumption is not needed anymore.
The only relying prior is that all points must be expressed in local lidar sensor
coordinates system (centered on (0, 0, 0) and spinning along -Z axis).

- Properly manage the spherical projection to correctly get neighbor points.
- Check that timestamp is increasing along a clockwise scanline.
If a time gap is detected (because of rolling shutter), invalidate neighboring points.

- Remove ScanLines member variables as we now use VertexMap.
- Remove AzimuthalResolution to use only NbFiringsPerLaserRing.
- Remove old unused SphericityThreshold and IncertitudeCoef.
- NbLaserRings and NbFiringsPerLaseRing are cast from unsigned to int.
- Clean getters/setters for these 2 attributes.
- Reset these auto-estimated parameters when resetting Slam.
- Rename EstimateAzimuthalResolution to EstimateLaserRingsParameters.
- Sort input pointcloud by scan line only in this method.
- Merge ConvertAndSortScanLines and PrepareDataForNextFrame into PrepareDataForNewFrame.
- Rename ComputeCurvature by ComputeFeatures

- Build the vertex map from input pointcloud
- Left and right neighbors are extracted from vertex map
- New keypoints are added if no left/right neighbor laser beam returned to sensor
- Points missing some (but not all) neighbors are skipped from the keypoints analysis
- Represent point features as VertexMap

In prevision of vertex map storage, it is necessary to store internal vectors as plain numeric types.

- This drops the std::bitset usage in favor of integer type with bitwise operations.
- This also specifies the enum Keypoint to be an uint8.
- This also moves LineFitting to Utils namespace.

Fix catch-by-value warning and fix undistortion time range

See merge request !105

- We were currently returning the reverse mapping.
  This reverses the mapping by an additional SortIdx call.
- Change default behavior of Utils::SortIdx to sort by default in ascending order, like std::sort.

Auto compute angular resolution for SSKE

See merge request !102

This parameter will now be auto-computed with first frame.
Rename AngleResolution to AzimuthalResolution.

The horizontal resolution is computed for the first frame only.
This process still requires a scan sorted by laser rings,
each ring being sorted by increasing azimuth/timestamp.

NLasers -> NbLaserRings
pclCurrentFrame -> Scan
pclCurrentFrameByScan -> ScanLines

[feat] Change keypoint invalidation criteria

See merge request !93

Julia Sanchez authored Feb 15, 2021 and Nicolas Cadart committed Mar 05, 2021

* Before searching for keypoints, an invalidation step is run. This step
ensures the first and last points of scan rays are not taken into
account (because the spherical aspect of Lidar is not considered),
neither the points which correspond to the boundary of an occluded area
or which lay on a very oblique wall relatively to the laser beam.

* By now, this step is performed with non-intuitive coefficients to set
thresholds. With this commit, the unique threshold is the maximum
orientation of such oblique planes relatively to laser beam to consider
a depth gap.

Fix keypoints extraction

Closes #6

See merge request !100

If there is a depth gap with previous part occluded, discard points
from current central point to left instead of contrary.

Do not select planar keypoints if angle has not been computed
(and is therefore equal to zero) : they are invalid!

This leads to much less but better extracted planar keypoints (~2x less in a urban environment),
so faster and better processing.

When checking if neighborhood was approximately flat, we were not comparing the neigborhood points,
but always the first points from the entire cloud !
This lead to completely wrong behavior, selecting only keypoints along the same direction.

Now, we estimate the approximate line linking the first and last points form the neighborhood,
and compare the angle of each local vector to this line.

Remove MatchInfo default values to fix compilation with MSVC

See merge request !99

[feat] Simplify auto diff ceres

See merge request !95

* Comments are updated to reflect the new ceres behaviour
* The tangent unit vector name is changed (n->u)

* The distance operator for blob has to be updated as A.transpose()*A /=
A
* A squareout step is added

* Ceres internally squares the cost function residual.
* By now, the square of the residual is computed, then the sqrt and
Ceres computes the square again afterwards.
* This commit changes the dimension of the residual (1D->3D) to avoid
this behaviour.
* The mean point argument name is changed (C->P) to uniformize the code

MSVC cannot use brace initialization if on a non-aggregate type.
From what I understand, if some default values are specified in the struct,
then a default constructor is created, and the struct is not anymore an aggregate.

[fix] Fix ICP saturation range

See merge request !94

* The ICP loss function uses a saturation value to exclude outliers

* By now, the minimal value of the range set by the user is not reached,
this commit fixes the problem

Improve residuals weighting

See merge request !90

Major undistortion refactoring

See merge request !87

Introduce KeypointsRegistration::MatchResults::MatchInfo struct.
This can be useful to check which and how much each keypoint contributes to optimization.

LossScale was the scale used by arctan loss function.
This parameter was difficult to understand and set for user.

By using Tukey loss, the loss reaches its saturation value at its squared scale.
By introducing SaturationDistance = sqrt(scale), this parameter becomes much more intuitive.
Beyond this saturation distance, the residual reaches its saturation, limiting outliers contributions.

Saturation distances have been tweaked to lead to very similar results as before.

TukeyLoss is more agressive to outliers, and reach its saturation at the given squared scale.

Each residual was weighted several times :
- In the ceres cost function itself, the squared residual was multiplied by weight.
  This resulted in the residual weighted by sqrt(weight).
- In the scaled loss function, the robustified loss was scaled by weight.

For small residuals, this leads to a scaling of ~weight^1.5.
For bigger residuals, this leads to strange behavior of robustifier, as if the scale was also weighted.

Ceres doc suggests to simply use the ScaledLoss function for such behavior.
So this commit removes the weighting inside the ceres residual computation.

After PGO, frames were only rigidly aggregated.
Now, it is possible to undistort them using optimized motion.
This also fixes a small time duration display as timers should only be reset on a hard reset.

ONCE: undistortion is done ony once at the beginning of localization step.
REFINED: ONCE + undistortion refined at the end of each ICP-LM iteration.

Current*Keypoints become CurrentRaw*Keypoints, raw extracted keypoints in BASE without undistortion.
Current*Keypoints are the keypoints clouds in BASE, with undistortion (if enabled).

Remove check in AddFrames verifying if we deal with first frame or not.
We need to run Localization step to update Current*Keypoints from CurrentRaw*Keypoints.
EgoMotion and Localization steps are safe if previous pointclouds are empty.

Enrich Utils::Timer functionnalities

See merge request !89

Accelerate x3 linear transform interpolation

Closes #13

See merge request !88

This greatly simplifies code organization.
WithinFrameMotion is now expressed in BASE coordinates,
and can be used to undistort clouds into BASE at frame header timestamp.

Allow specifying a number of digits to display timer duration in milliseconds.
Add the Display method, that prints the average duration of a given timer, in milliseconds.

Use Eigen::Isometry3d::linear() method instead of .rotation() to avoid performing useless SVD.
This brings a nice x3 speedup.

Fixes #13