[feat][ROS2] Create new keypoint extractor using a vertex map

Goal

This MR will implement an idea from !104 (closed) : using a vertex map to extract keypoints.

So far, we were only extracting keypoints looking at their neighborhood on their scanline.

For dense rotative lidar, the scanlines are close enough to each other to build a meaningful image and create 2D neighborhoods.

In this MR, we reproduce SSKE behaviour and improve it along the way. We also fix a few things related to keypoint extractors.

The next goal is to be able to use new criteria related to upper and lower lines. This will belong to another MR.

Changes

KeypointExtractor

KeypointExtractor aims to define common methods and parameters to keypoint extractors. This abstract class is called in Slam, because the choice of the extractor is now a parameter handled in lidar_slam wrapper : it allows to use the output of an extractor without knowing the process used inside.

plane_angle_threshold & edge_angle_threshold are new common parameters for extractors, used in different ways (for PlaneSinAngleThreshold & EdgeSinAngleThreshold in SSKE, for PlaneCosAngleThreshold & EdgeCosAngleThreshold in DSSKE). See below for details.

SpinningSensorKeypointExtractor

getNeighborhood()

In ComputeCurvature(), we changed the way we extract neighborhood for computation time concerns, and to add the central point to its own neighborhoods. Indeed, it was useful for the new vision of FitLineAndCheckConsistency().

FitLineAndCheckConsistency()

We decided to change the theory of FitLineAndCheckConsistency() to something less precise but way more efficient. Instead of doing a RANSAC, we now do two things. First, we check the relative distance between each point. The ratio between max and min distance between two consecutive points have to be inferior to a certain ratio called SquaredRatio. Secondly, we fit a line between the first (central) point and the last point of the neighborhood, define its position as the centroid of the whole neighborhood, and then check if any point is too far from the fitted line (if it is outside the widthThreshold we allow).

We removed MaxLineWidth parameter in LineFitting. Indeed, it made sense when the length of the neighborhood was depending on a MinNeighNb. Since we use MinNeighRadius to extend the neighborhood until it has a convenient line length, a MaxLineWidth makes less sense than a LengthWidthRatio.

Small fix

• We added the possibility to have random laser_id in GetDebugArray(), as it has already been added to the rest of SpinningSensorKeypointExtractor.
• We changed the way we cleared the Keypoints map, because it is better to clean it all for LidarView live usage when one can choose to change enabled keypoints between 2 frames.

DenseSensorKeypointsExtractor

A new keypoint extractor class is created, derived from KeypointExtractor.

Vertex Map

The vertex map is the projection of the 3D pointcloud onto a 2D image. The type of data stored is a smart pointer to a new structure PtFeat in an array of structure like container.

PtFeat stores the index of the point in the ScanCloud, and all of its features (depth, spacegap...).

Extraction of features

ComputeCurvature() will extract a 2D neighborhood of a point (using a Kernel, for now diamond or square, see below for illustration).

It will then extract features as SSKE does, in the scanline of the current point.

Some differences have to be pointed out :

• For space gap : we now count the number of direct neighbors in the vertex map which have PtFeat set as nullptr : they are points fired by a laser but never returned to the sensor. This is faster than the technic used in SSKE

• For depth gap : we look at the direct neighbor of the central point in the vertex map, and the direct neighbor of this direct neighbor (to avoid extracting points on a bended wall as keypoints). Supposing that sometimes for an edge a point can be missed, we decided to add the possibility to have the second direct neighbor and the third for depth gap calculus is the direct neighbor has not been returned.

• For angle : to be more precise in angle selection, we decided to use the cosine and use thresholds in a different way (in SSKE, we use the absolute value of sine).

  

Keypoints extraction

Create2DGrid / Create3DGrid

Create the grids to downsample the keypoints. Useful in the case of a MaxPoints (= max number of keypoints to extract for each type) lower than the number of keypoints candidates. We will use the grids to extract the keypoints uniformly in the 3D space.

• 2D Grid

  Downsample using 2D patches in the vertex map

• 3D Grid

  Downsample using a 3D voxel grid, analogous to the Rolling Grid used in SLAM. It is less efficient than 2D mode, but will certainly extract keypoints more uniformly as it uses another dimension.

Note that, for efficiency purposes, the only points added to the grids have to respect the thresholds specific to a keypoint type, hence the isPtFeatValid function as argument.

AddKptsUsingGrid

Use the selected grid to extract keypoints progressively

Config files

• Extraction mode : One can choose between dense or sparse extractor setting the boolean dense_extractor to respectively true or false

• Plane and Edge angle threshold : One can set characteristic angles for planes and edges. They will then be used to define absolute sine thresholds in SSKE and cosine thresholds in DSSKE, following the theory illustrated above.

• Downsampling section : We moved max_points and voxel_grid_resolution (useful for SSKE & DSSKE) in the downsampling section to make their purpose clear.

• Sampling mode :

  One can choose between 2 types of division in grid for keypoints extraction.

  • Patch : division of the Vertex Map, which is a 2D projection of the pointcloud. This mode is efficient but can have trouble extracting keypoints far away
  • Voxel : division of the Scan Cloud, which is the 3D pointcloud. This mode is costlier but gives better keypoint outputs (better distributed).

• Patch Size : useful for Patch sampling mode, size of a square patch, the patch will contain at most (patch_size * patch_size) points.

  Output

  Points & features

  One can output points and their features as pgm images using OutputFeatures().

  The absolute black corresponds to points which never returned to the sensor. The other shades of grey until the absolute white represent the intensity of the feature in comparison with the feature maximum on the map.

  Keypoints

  One can output keypoints as 2 different formats using OutputKeypoints().

  • As pgm image to see the keypoint repartition in 2 dimensions, useful if one's using Patch mode.
  • As csv file, to be used to observe the resulting 3D pointcloud of keypoints in an adapted software.

Results

1000 Edges extraction with SSKE, voxel size set to 1f.

1000 Edges extraction with 3D sampling, voxel size set to 1f.

1000 Edges extraction with 2D sampling, patch size set to 32.

---

100 000 000 Edges extraction with SSKE.

100 000 000 Edges extraction with DSSKE.

---

100 000 000 Planes extraction with SSKE.

100 000 000 Planes extraction with DSSKE.

Reproduce

Data

I have used data of an Ouster Lidar with 128 lasers in an enclosed warehouse with what seems to be an office space.

I also tested a bag from a Velodyne 16 in outdoor mode, and used the "save maps" option to save keypoints map that you can find here.

Build

colcon build --base-paths slam/ros2_wrapping --cmake-args -DCMAKE_BUILD_TYPE=Release

Parameters

• Change the parameters :

  • To select the keypoint extractor :

    slam: ke: extractor_mode: [0, 1] to set at 0 for sparse (SSKE), 1 for dense (DSSKE).

  • To set the thresholds for angle feature in both extractors:

    slam: ke: edge_angle_threshold: [°], maximum value for an angle between two lines to form an edge.

    slam: ke: plane_angle_threshold: [°], minimum value for an angle between two lines to form a plane (also used with its opposite cosine value in DSSKE to filter too sharp edges).

  • Parameters specific to dense extractor (DenseSpinningSensorKeypointExtractor) :

    slam: ke: downsampling: sampling_mode: [0, 1] to set at 0 for patch (2D grid), 1 for voxel (3D grid).

    slam: ke: downsampling: patch_size: the dimension of a patch in term of number of points in one dimension (if set to 32, the patch will contain at most 32x32 points).

    note : for voxel downsampling, use voxel_grid_resolution used in SSKE as well.

Run

ros2 launch lidar_slam slam_ouster.py outdoor:=false replay:=true use_sim_time:=true

ros2 bag play --clock path/to/this_bag.bag --clock

You can add the -p --start-offset 10.5 to the bag play to control the arriving frame.

Performances

This table gives the duration of the Keypoint extraction step in different configurations, in release mode. The patch size was set to 32, and the voxel size to 1.f. We were extracting planes, edges and intensity edges.

Using 1 thread :

Number of keypoints	DSSKE - 2D for all	DSSKE - 3D for all	SSKE - new	SSKE - old
1000	75 ms	78 ms	49 ms	98 ms
huge (+100 000 000)	79 ms		48 ms	96 ms

Using 5 threads :

Number of keypoints	DSSKE - 2D for all	DSSKE - 3D for all	SSKE - new	SSKE - old
1000	44 ms	47 ms	34 ms	51 ms
huge (+100 000 000)	46 ms		30 ms	50 ms

Checklist

• Camel case everywhere except for ROS variables/parameters
• Lower case for local variables and lambda functions
• Upper case for pour members, methods and tool functions (in Utils)
• Precise namespace when calling a function (or this->X or classe.X)
• Align code (for multiline if and while, "&&" or "||" go in upper line to ensure alignement)
• Space between if, while, for and parenthesis
• Space between operators and variables: e.g. a + b
• Space after ","
• Mind your commit titles/desc (plurals, he/she + "s", correct tags, title should begin by a verb...)
• Function names should start with a verb, variable names should start with a name
• Macros should be between {}
• Do not use negative boolean (i.e. noJoe)
• Check minimal size of the types (double -> float -> int -> uint)
• Check const and ref in functions arguments
• References should be written "type& name", not "type &name"
• Update documentation if needed
• Add MR labels [ROS]/[ROS2]/[PV]
• If ros/ros2, update task table here
• Add a comment over each non trivial function in header files
• Add a header to each new file

What remains to solve

• Create a function to create the vertex map from the pointcloud

• Create a struct like : ptFeat = {index, feat1, feat2, feat3} corresponding to each point and sort them in the vertex map (of type vec<vec<ptFeat*>>)

• Create a function to test the vertex map (output as BMP format)

• Compute criterions and create output functions related

• Extract a limited number of kpts by cutting the image into patches / voxels

• Create a downsample function for the vertex map

• For future MR :

  • Seek to robustify them using kernels
  • Create new criteria in the map
  • Observe the criterions, keep the most relevant
  • Apply morphological filters if relevant
  • Kernel mode : One can choose between 2 types of neighborhood for keypoint extraction:
  • Cross :

  

  • Square :