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 :

1. 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.
2. 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 --base-paths slam/ros2_wrapping --cmake-args -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 --start-offset 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