Get the subgrid containing all non-empty voxels, and convolve only this part.
This allows saving a lot of time.

Resolves #11.

Extract less keypoints from maps for faster KD-trees building and NN queries.
First, we check in which rolling grid's voxels lie each current frame keypoint.
Then we extract map keypoints from these cells and their neighbors.
The "voting dilatation" is done by convolving the voxels "to take".

This implementation uses unsupported Eigen Tensor module.

[fix] Correctly extract points from rolling grid using bounds

See merge request !58

[fix] Use WORLD coordinates input bounding box to extract points from RollingGrid

See merge request !55

Before, user had to specify relative bounding box inb BASE coordinates.
This same relative bounding box was used to extract local keypoints from map, and to roll it later.

But this bounding box was in relative coordinates, and was not correctly rotated to WORLD coordinates.
Therefore, some cells were incorrectly extracted, and others were missing.

Now, user can directly specify a WORLD bounding box to extract keypoints.
Also, the Add() method can automatically call the Roll() method by computing the bounding box of the new points to add.

By fixing previous wrong extraction and rolling, this improves SLAM precision as more keypoints can be matched.

Add color to verbose display

See merge request !53

From Windows 10, ANSI color codes are natively available.
This allows us to simplify the printing helpers.

Check that time is roughly increasing to perform frame undistortion using previous motion

See merge request !52

[fix] Check that time scale is roughly increasing to perform frame undistortion using previous motion

If points timestamps were not excatly computed, it could happen that FrameDuration was bigger than NewFrameTime - PreviousFrameTime.
In that case, approximated undistortion using previous motion was not performed as it detected that time wasn't strictly increasing.

Now, we only check that time is roughly increasing and that FrameDuration is roughly the same as the current and previous timestamps difference.

[fix] Add missing std before ios in PointCloudStorage.h

See merge request !42

Fix Maps update

See merge request !50

Replace max variance error by max position and orientation errors

See merge request !47

Before this commit, if the new frame does not fit at all in all directions (if min point and max point are out of the grid),
the grid would firstly roll to include the min point, then roll to include the max point.
This behavior resulted in always discarding the min point to the benefit of the max one.

Now, if the new frame does not fit into grid, the grid is rolled by the mean of the offsets.

This allows to properly run SLAM when using smaller grids (e.g. indoor).

All points are extracted, then grid/position/resolution is resized/updated, and then the points are added back.
This ensures all previously mapped points lie in the right voxel after modification.

If we change VoxelResolution or RollingGridSize during execution, some weird behavior may happen by using voxels indices.
Using absolute positions allow to be independent from these conversions.

As NbrFrameProcessed is incremented into SLAM algorithm, its value in vtkSlam is 1 after a reset.
By considering NbrFrameProcessed - 1, we update maps display right after the 1st frame is received,
and we reset properly maps display after reset button is clicked.

Ignore Ego-Motion extrapolation and Localization results if errors occurs

See merge request !49

[PV] Add verbose timings and perform less maps updates

See merge request !48

This enables refreshing maps display when we hit Reset button,
even if maps update step has not been reached.

In case too few keypoints have been extracted, we reset current state to previous one.
This avoids unstability in case of bad optimization at one step.

If frames timestamps are incorrect and are not strictly increasing,
using them to perform motion interpolation or extrapolation will lead to wrong behavior.
In this case, we just ignore Ego-Motion extrapolation and Undistortion.

Conversions from PCL to VTK formats are slow.
Keypoints maps can rapidly grow heavy.
At some point, most of the SLAM filter duration is spent in the maps output conversions !
To avoid that, this commit enables updating SLAM maps only 1 frame over MapsUpdateStep frames.

Maximum variance is not meaningful.
Position and orientation errors (in meters and degrees) are easier to understand.
Display these errors and the direction of error.

[ROS] Publish SLAM-estimated speed in GPS-like message

See merge request !46

[ROS] Publish SLAM pose as `gps_common/GPSFix` msg instead of `sensor_msgs/NavSatFix`

See merge request !45

sensor_msgs/NavSatFix only gives position.
gps_common/GPSFix adds orientation (and speed, even if unused).

Fix ego motion extrapolation when using PGO

See merge request !44

Fix transformation matrix multiplication order.
Use modern tf2 server to get transform.
Reset current keypoints and previous pose to skip next ego-motion step due to discontinuity.

When using Pose Graph Optimization, last Tworld is beeing updated after optimization succeded.
However, PreviousTWorld was not updated.
When extrapolating motion from PreviousTWorld to TWorld, this lead to very important motion.
This resulted in a wrong big jump for next TWorld estimation.

This commit also updates PreviousTWorld after PGO to repair Ego Motion extrapolation.

Misc bug fixes and code refactoring in ICP functions

See merge request !43