Draft: [ROS2][feat] Add 2D features in DSSKE and filter edges
Goal
This MR follows the implementation of !324 (merged) : we added a keypoint extractor for dense LiDAR, in order to use their dense scan lines to extract 2D features and compare points with their top and bottom neighbors.
There are 2 major axis of work :
 Extracting 2D features, looking at what we call a "kernel", a 2D neighborhood. Here, we fitted a plane on the kernel of a point and tested two features.
 Filtering keypoints after feature computing but before extraction, looking at the keypoints candidates in their neighborhood.
Changes
DenseSpinningSensorKeypointExtractor
We'll split our explanations between the 2 axis of work explained above.
1) Extracting 2D features

We first tried two types of kernel. The first commit leaves a trace of what could be a "square" kernel, but we didn't keep it because extracting a square kernel of more than 1 neighbor per direction was quite tricky. It would also take a lot of computation time.
> So we kept a "cross" kernel, literally taking left, right, top and bottom neighbors under a kernel radius.

We computed the normal of the plan formed by the kernel using a PCA.

We tried two features to replace angles
 (a) Mean angle between the normal and the vectors formed by the central point and a point of the kernel.
 (b) Mean distance between the plane and the kernel points.
 Note : we avoid computing an angle or a distance for points too close for the central point, because we consider them as noise. For that, we defined a DistToNeighThreshold

We filter the features using the values of the left neighborhood to keep only the most meaningful, only for edges.
Let's dig into (a) and (b) principles :
(a) : We define two new PtFeat : one CosNormal and one SinNormal. They stock the cos and sin values of the mean angle, and are respectively used for Plane and Edge extraction. We consider that the central point belongs to a plane if the cosine of the mean angle tends to 0, and it belongs to an edge if the sine of the mean angle tends to 0. We had to define two thresholds as we are locally filtering the minimal value for future edges extraction.
(b) : We define two features PlaneDist and EdgeDist, that contain the same value (but the EdgeDist will be locally filtered to only keep the maximal value for future edge extraction). We consider that if a mean distance to the kernel plane is small, the central point belongs to a plane. If it is big, the central point belongs to an edge.
2) Filtering keypoints
 We used the cross kernel concept, but adapted it to only extract top and bottom neighborhoods as they were the ones we were wanting to look at to filter edges. But we could easily adapt to look at left and right also.
 We added a hasEdge feature in each part of the kernel to save a boolean indicating if the vector of PtFeat contains an edge candidate.
 We also look at top and bottom neighborhoods of direct left and right neighbors of the central point. So we check if there is any edge in the 3 columns around the central point (using the hasEdge feature), and if there is not, we remove it from keypoints candidates by adding a test in the
isPtValid()
check.
Results
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.
Build
colcon build basepaths slam/ros2_wrapping cmakeargs DCMAKE_BUILD_TYPE=Release
Parameters
 Change the parameters :

To set the kernel radius (similar to neighborhood radius):
slam: ke: kernel_radius: [m], minimum length of a neighborhood.

For commit with angles between normals and vectors formed by central point and a kernel point :
slam: ke: dist_to_neighbor_threshold: [m] minimal distance between central point and a kernel point to consider that this point does not belong to the noi
slam: ke: edge_sin_normal_threshold: [m] maximal sin value for the mean of the angles between the normal and a vector formed by central point and a kernel point to caracterize an edge
slam: ke: plane_cos_normal_threshold: [m] maximal cos value for the mean of the angles between the normal and a vector formed by central point and a kernel point to caracterize an plane

For commit with dist to plane :
slam: ke: plane_pt_dist_thresh: [m], maximal distance between the plane fitted on the kernel and the mean distance of the kernel points to the plane for a central point to be considered a plane
slam: ke: edge_pt_dist_thresh: [m], minimal distance between the plane fitted on the kernel and the mean distance of the kernel points to the plane for a central point to be considered an edge

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 startoffset 10.5
to the bag play to control the arriving frame.
Output
Performances
 We wanted to fit a plane using the mean cross product of all vectors from the kernel, but this was costlier than using a PCA.
 As we optimized FitLineAndCheckConsistency in !324 (merged), it is going to be hard having better performances with the normals feature. However, we should determine if the results with normals have interest compared to 1D angles.
Checklist

Camel case everywhere except for ROS variables/parameters 
Lower case for local variables and lambda functions 
Upper case for 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) 
Check your spaces 
between if, while, for and parenthesis 
between operators and variables: e.g. a + b 
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 
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

Check which feature between CosNormal/SinNormal and PlaneDist/EdgeDist is the most relevant 
Decide which form of kernel is the best (cross, square) 
Improve keypoints filtering > add a filter for planes ? use left & right neighborhoods for edges ? 
Improve computation time 
Reflect on new 2D features to extract