From 39f6a1e37fdce1668838c8232fd68d8fbce0abb9 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Tue, 23 Jan 2024 15:39:42 +0100
Subject: [PATCH 01/13] [fix] Allow to have random laser_id in GetDebugArray

---
 slam_lib/src/SpinningSensorKeypointExtractor.cxx | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/slam_lib/src/SpinningSensorKeypointExtractor.cxx b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
index f4393d7ee..8ca1e2e6e 100644
--- a/slam_lib/src/SpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
@@ -624,7 +624,8 @@ std::unordered_map<std::string, std::vector<float>> SpinningSensorKeypointExtrac
     for (unsigned int i = 0; i < this->Scan->size(); i++)
     {
       const auto& laserId = this->Scan->points[i].laser_id;
-      v[i] = vector2d[laserId][indexByScanLine[laserId]];
+      int scanlineIdx = std::distance(this->ScanLines.begin(), this->ScanLines.find(laserId));
+      v[i] = vector2d[scanlineIdx][indexByScanLine[laserId]];
       indexByScanLine[laserId]++;
     }
     return v;
@@ -637,7 +638,8 @@ std::unordered_map<std::string, std::vector<float>> SpinningSensorKeypointExtrac
     for (unsigned int i = 0; i < this->Scan->size(); i++)
     {
       const auto& laserId = this->Scan->points[i].laser_id;
-      v[i] = vector2d[laserId][indexByScanLine[laserId]][flag];
+      int scanlineIdx = std::distance(this->ScanLines.begin(), this->ScanLines.find(laserId));
+      v[i] = vector2d[scanlineIdx][indexByScanLine[laserId]][flag];
       indexByScanLine[laserId]++;
     }
     return v;
-- 
GitLab


From 9d1d153b726179a8d7f4e9afbbe3c5f3c199e6f5 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Tue, 23 Jan 2024 15:47:17 +0100
Subject: [PATCH 02/13] [fix] Clear completely Keypoint map for each frame

Useful for live usage in LidarView, when different keypoint types can be enabled and disabled while running the SLAM.
---
 slam_lib/src/SpinningSensorKeypointExtractor.cxx | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/slam_lib/src/SpinningSensorKeypointExtractor.cxx b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
index 8ca1e2e6e..f5e175263 100644
--- a/slam_lib/src/SpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
@@ -212,10 +212,11 @@ void SpinningSensorKeypointExtractor::PrepareDataForNextFrame()
   LidarPoint minPt, maxPt;
   pcl::getMinMax3D(*this->Scan, minPt, maxPt);
 
+  // Clear keypoints
+  this->Keypoints.clear();
+  // Initialize keypoints
   for (auto k : KeypointTypes)
   {
-    if (this->Keypoints.count(k))
-      this->Keypoints[k].Clear();
     if (this->Enabled[k])
       this->Keypoints[k].Init(minPt.getVector3fMap(), maxPt.getVector3fMap(), this->VoxelResolution, this->Scan->size());
   }
-- 
GitLab


From 36079936d7be5c76378ac994706d97e09a358c39 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Wed, 31 Jan 2024 10:58:20 +0100
Subject: [PATCH 03/13] [feat][perf] Speed up and add central point in
 FitLineAndCheckConsistency

* We decided to add central point to its own neighborhood, in order to have more accurate lines.

* Moreover, we removed the neighborhood sampling from getNeighbors. Indeed, it seemed more logical to sample it when needed (i.e. for intensity edges and in FitLineAndCheckConsistency). We needed a complete neighborhood for maximum angle keypoint local check.

* Besides, the theory behind FitLineAndCheckConsistency changed. Now, we check the relative distance between consecutive points and define a ratio that max and min dist have to respect. In addition to that, we compute the line formed by the last and the first point (the central point), using the centroid of the neighborhood for the line position; then we exclude a neighborhood containing points too far from fitted line.
This change sped up the whole ComputeCurvature process without having such an impact on the quality of the extracted keypoints.

* We also removed MaxLineWidth parameter in LineFitting to only keep the LengthWidthRatio since we use a minimum radius in neighborhood definition. The MaxLineWidth parameter used to make sense for neighborhood of small length.
---
 .../lidar_slam/params/slam_config_indoor.yaml |   6 +-
 .../params/slam_config_outdoor.yaml           |   6 +-
 .../SpinningSensorKeypointExtractor.h         |  11 +-
 .../src/SpinningSensorKeypointExtractor.cxx   | 189 ++++++++----------
 4 files changed, 97 insertions(+), 115 deletions(-)

diff --git a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
index cea3bcab1..464c0157b 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
@@ -386,7 +386,7 @@ slam:
     min_azimuth: 0.                    # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     max_azimuth: 360.                  # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
-    neighbors_side_nb: 4               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
+    neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
     plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
     edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
@@ -414,7 +414,7 @@ slam:
     #   min_azimuth: -90.                  # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   max_azimuth: 90.                   # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
-    #   neighbors_side_nb: 4               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
+    #   neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     #   neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
     #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
     #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
@@ -434,7 +434,7 @@ slam:
     #   min_azimuth: -45.                  # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   max_azimuth: 45.                   # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
-    #   neighbors_side_nb: 4               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
+    #   neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     #   neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
     #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
     #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
diff --git a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
index 99152a426..82d08de0f 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
@@ -387,7 +387,7 @@ slam:
     min_azimuth: 0.                    # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     max_azimuth: 360.                  # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
-    neighbors_side_nb: 4               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
+    neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
     plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
     edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
@@ -415,7 +415,7 @@ slam:
     #   min_azimuth: -45.                  # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   max_azimuth: 45.                   # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
-    #   neighbors_side_nb: 4               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
+    #   neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     #   neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
     #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
     #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
@@ -435,7 +435,7 @@ slam:
     #   min_azimuth: -45.                  # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   max_azimuth: 45.                   # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
-    #   neighbors_side_nb: 4               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
+    #   neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     #   neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
     #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
     #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
diff --git a/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
index 9abd942f0..b085845d4 100644
--- a/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
@@ -55,11 +55,11 @@ struct LineFitting
   inline float DistanceToPoint(Eigen::Vector3f const& point) const;
 
   // Direction and position
-  Eigen::Vector3f Direction;
-  Eigen::Vector3f Position;
+  Eigen::Vector3f Direction = Eigen::Vector3f::Zero();
+  Eigen::Vector3f Position = Eigen::Vector3f::Zero();
 
-  //! Max line width to be trustworthy for lines < 20cm
-  float MaxLineWidth = 0.02;  // [m]
+  //! Ratio (squared) for squared distances of direct neighbors to consider a neighborhood as valid to fit line on
+  float SquaredRatio = 100.; // [.]
 
   // Ratio between length and width to be trustworthy
   float LengthWidthRatio = 10.; // [.]
@@ -206,7 +206,7 @@ private:
   float InputSamplingRatio = 1.;
 
   // Minimum number of points used on each side of the studied point to compute its curvature
-  int MinNeighNb = 4;
+  int MinNeighNb = 5;
 
   // Minimum radius to define the neighborhood to compute curvature of a studied point
   float MinNeighRadius = 0.10f;
@@ -228,7 +228,6 @@ private:
 
   // Sharpness threshold to select an edge keypoint
   float EdgeSinAngleThreshold = 0.86;  // ~sin(60°) (selected, if sin angle is more than threshold)
-  float MaxDistance = 0.20;  // [m]
 
   // Threshold upon depth gap in neighborhood to select an edge keypoint
   float EdgeDepthGapThreshold = 0.5;  // [m]
diff --git a/slam_lib/src/SpinningSensorKeypointExtractor.cxx b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
index f5e175263..75425d308 100644
--- a/slam_lib/src/SpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
@@ -35,66 +35,41 @@ namespace
 {
 //-----------------------------------------------------------------------------
 bool LineFitting::FitLineAndCheckConsistency(const SpinningSensorKeypointExtractor::PointCloud& cloud,
-                                            const std::vector<int>& indices)
+                                             const std::vector<int>& indices)
 {
-  // Check line width
-  float lineLength = (cloud[indices.front()].getVector3fMap() - cloud[indices.back()].getVector3fMap()).norm();
-  float widthTheshold = std::max(this->MaxLineWidth, lineLength / this->LengthWidthRatio);
-
-  float maxDist = widthTheshold;
-  Eigen::Vector3f bestDirection;
-
-  this->Position = Eigen::Vector3f::Zero();
-  for (int idx : indices)
-    this->Position += cloud[idx].getVector3fMap();
-  this->Position /= indices.size();
+  // Check consistency of the line
+  if (indices.size() < 2)
+    return false;
 
-  // RANSAC
-  for (int i = 0; i < int(indices.size()) - 1; ++i)
+  float sqDistMax = 0.f;
+  float sqDistMin = std::numeric_limits<float>::max();
+  float sqDistCurr;
+  for (int i = 1; i < indices.size(); ++i)
   {
-    // Extract first point
-    auto& point1 = cloud[indices[i]].getVector3fMap();
-    for (int j = i+1; j < int(indices.size()); ++j)
-    {
-      // Extract second point
-      auto& point2 = cloud[indices[j]].getVector3fMap();
-
-      // Compute line formed by point1 and point2
-      this->Direction = (point1 - point2).normalized();
-
-      // Reset score for new points pair
-      float currentMaxDist = 0;
-      // Compute score : maximum distance of one neighbor to the current line
-      for (int idx : indices)
-      {
-        currentMaxDist = std::max(currentMaxDist, this->DistanceToPoint(cloud[idx].getVector3fMap()));
-
-        // If the current point distance is too high,
-        // the current line won't be selected anyway so we
-        // can avoid computing next points' distances
-        if (currentMaxDist > widthTheshold)
-          break;
-      }
+    sqDistCurr = (cloud[indices[i]].getVector3fMap() - cloud[indices[i-1]].getVector3fMap()).squaredNorm();
+    sqDistMax = std::max(sqDistMax, sqDistCurr);
+    sqDistMin = std::min(sqDistMin, sqDistCurr);
+  }
+  if (sqDistMax / sqDistMin > this->SquaredRatio)
+    return false;
 
-      // If the current line implies high error for one neighbor
-      // the output line is considered as not trustworthy
-      if (currentMaxDist > 2.f * widthTheshold)
-        return false;
+  Eigen::Vector3f diffVec = cloud[indices.back()].getVector3fMap() - cloud[indices.front()].getVector3fMap();
+  this->Direction = diffVec.normalized();
+  Eigen::Vector4f centroid;
+  pcl::compute3DCentroid(cloud, indices, centroid);
+  this->Position = centroid.head(3);
 
-      if (currentMaxDist <= maxDist)
-      {
-        bestDirection = this->Direction;
-        maxDist = currentMaxDist;
-      }
+  // Check line width
+  float lineLength = diffVec.norm();
+  float widthThreshold = lineLength / this->LengthWidthRatio;
 
-    }
+  for (auto idx : indices)
+  {
+    float error = (cloud[idx].getVector3fMap() - this->Position).cross(this->Direction).norm();
+    if (error > widthThreshold)
+      return false;
   }
 
-  if (maxDist >= widthTheshold - 1e-10)
-    return false;
-
-  this->Direction = bestDirection;
-
   return true;
 }
 
@@ -288,40 +263,36 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
 
       // Fill left and right neighbors
       // Those points must be more numerous than MinNeighNb and occupy more space than MinNeighRadius
-      auto GetNeighbors = [&](bool right) -> std::vector<int>
+      auto getNeighbors = [&](bool right, std::vector<int>& neighbors)
       {
-        std::vector<int> neighbors;
-        neighbors.reserve(this->MinNeighNb);
-        int plusOrMinus = right? 1 : -1;
-        int idxNeigh = 1;
-        float lineLength = 0.f;
-        while ((int(neighbors.size()) < this->MinNeighNb
-                || lineLength < this->MinNeighRadius)
-                && int(neighbors.size()) < nPts)
+        neighbors.reserve(nPts);
+        // Check number criterion
+        int step = right ? 1 : -1;
+        int idxNeigh = index;
+        while (int(neighbors.size()) < this->MinNeighNb &&
+              neighbors.size() < nPts) // ensure enough points in the line
         {
-          neighbors.emplace_back((index + plusOrMinus * idxNeigh + nPts) % nPts); // +nPts to avoid negative values
-          lineLength = (scanLineCloud[neighbors.back()].getVector3fMap() - scanLineCloud[neighbors.front()].getVector3fMap()).norm();
-          ++idxNeigh;
+          neighbors.emplace_back(idxNeigh);
+          idxNeigh = (idxNeigh + step + nPts) % nPts;
         }
-
-        // Sample the neighborhood to limit computation time
-        if (neighbors.size() > this->MinNeighNb)
+        // Check radius criterion
+        float lineLength = ((scanLineCloud[neighbors.back()]).getVector3fMap() - (scanLineCloud[neighbors.front()]).getVector3fMap()).norm();
+        while (lineLength < this->MinNeighRadius &&
+              neighbors.size() < nPts) // ensure enough points in the line
         {
-          int step = neighbors.size() / this->MinNeighNb;
-          std::vector<int> newIndices;
-          newIndices.reserve(neighbors.size() / step);
-          for (int i = 0; i < neighbors.size(); i = i + step)
-            newIndices.emplace_back(neighbors[i]);
-          neighbors = newIndices;
+          idxNeigh = (idxNeigh + step + nPts) % nPts;
+          neighbors.emplace_back(idxNeigh);
+          lineLength = ((scanLineCloud[neighbors.back()]).getVector3fMap() - (scanLineCloud[neighbors.front()]).getVector3fMap()).norm();
         }
-        return neighbors;
+        neighbors.shrink_to_fit();
       };
 
-      std::vector<int> leftNeighbors  = GetNeighbors(false);
-      std::vector<int> rightNeighbors = GetNeighbors(true);
+      std::vector<int> leftNeighbors, rightNeighbors;
+      getNeighbors(false, leftNeighbors);
+      getNeighbors(true, rightNeighbors);
 
-      const auto& rightPt = scanLineCloud[rightNeighbors.front()].getVector3fMap();
-      const auto& leftPt = scanLineCloud[leftNeighbors.front()].getVector3fMap();
+      const auto& rightPt = scanLineCloud[rightNeighbors[1]].getVector3fMap();
+      const auto& leftPt = scanLineCloud[leftNeighbors[1]].getVector3fMap();
 
       const float rightDepth = rightPt.norm();
       const float leftDepth = leftPt.norm();
@@ -354,17 +325,20 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
           distLeft = diffLeftNorm;
 
         this->SpaceGap[scanLine][index] = std::max(distLeft, distRight);
+      }
 
-        // Stop search for first and last points of the scan line
-        // because the discontinuity may alter the other criteria detection
-        if (index < int(leftNeighbors.size()) || index >= nPts - int(rightNeighbors.size()))
-          continue;
+      // Stop search for first and last points of the scan line
+      // because the discontinuity may alter the other criteria detection
+      if (index < int(leftNeighbors.size()) || index >= nPts - int(rightNeighbors.size()))
+        continue;
 
+      if (this->Enabled[EDGE])
+      {
         // Compute depth gap
 
-        // Reinit variables
-        distRight = -1.f;
-        distLeft = -1.f;
+        // Init variables
+        float distRight = -1.f;
+        float distLeft = -1.f;
 
         if (cosAngleRight > cosMaxAzimuth)
           distRight = centralPoint.dot(diffVecRight) / centralDepth;
@@ -375,7 +349,7 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
         // If the points lay on a bended wall, previous and next points should be in the same direction
         if (distRight > this->EdgeDepthGapThreshold)
         {
-          auto nextdiffVecRight = (scanLineCloud[rightNeighbors[1]].getVector3fMap() - rightPt).normalized();
+          auto nextdiffVecRight = (scanLineCloud[rightNeighbors[2]].getVector3fMap() - rightPt).normalized();
           if ((nextdiffVecRight.dot(diffVecRight) / diffRightNorm) > cosMinBeamSurfaceAngle ||
               (-diffVecLeft.dot(diffVecRight) / (diffRightNorm * diffLeftNorm)) > cosMinBeamSurfaceAngle)
             distRight = -1.f;
@@ -385,7 +359,7 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
         // If the points lay on a bended wall, previous and next points should be in the same direction
         if (distLeft > this->EdgeDepthGapThreshold)
         {
-          auto prevdiffVecLeft = (scanLineCloud[leftNeighbors[1]].getVector3fMap() - leftPt).normalized();
+          auto prevdiffVecLeft = (scanLineCloud[leftNeighbors[2]].getVector3fMap() - leftPt).normalized();
           if ((prevdiffVecLeft.dot(diffVecLeft) / diffLeftNorm > cosMinBeamSurfaceAngle) ||
               (-diffVecRight.dot(diffVecLeft) / (diffRightNorm * diffLeftNorm)) > cosMinBeamSurfaceAngle)
             distLeft = -1.f;
@@ -415,18 +389,30 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
       if (this->Enabled[INTENSITY_EDGE])
       {
         // Compute intensity gap
-        if (std::abs(scanLineCloud[rightNeighbors.front()].intensity - scanLineCloud[leftNeighbors.front()].intensity) > this->EdgeIntensityGapThreshold)
+        if (std::abs(scanLineCloud[rightNeighbors[1]].intensity - scanLineCloud[leftNeighbors[1]].intensity) > this->EdgeIntensityGapThreshold)
         {
           // Compute mean intensity on the left
-          float meanIntensityLeft = 0;
-          for (int indexLeft : leftNeighbors)
-            meanIntensityLeft += scanLineCloud[indexLeft].intensity;
-          meanIntensityLeft /= leftNeighbors.size();
+          float meanIntensityLeft = 0.f;
+          // We sample neighborhoods for computation time concerns
+          int step = leftNeighbors.size() > this->MinNeighNb ? leftNeighbors.size() / this->MinNeighNb : 1;
+          int cptMean = 0;
+          // The first element of the neighborhood is the central point itself so we skip it
+          for (int i = 1; i < leftNeighbors.size(); i += step)
+          {
+            meanIntensityLeft += scanLineCloud[leftNeighbors[i]].intensity;
+            cptMean++;
+          }
+          meanIntensityLeft /= cptMean;
           // Compute mean intensity on the right
-          float meanIntensityRight = 0;
-          for (int indexRight : rightNeighbors)
-            meanIntensityRight += scanLineCloud[indexRight].intensity;
-          meanIntensityRight /= rightNeighbors.size();
+          float meanIntensityRight = 0.f;
+          cptMean = 0;
+          step = rightNeighbors.size() > this->MinNeighNb ? rightNeighbors.size() / this->MinNeighNb : 1;
+          for (int i = 1; i < rightNeighbors.size(); i += step)
+          {
+            meanIntensityRight += scanLineCloud[rightNeighbors[i]].intensity;
+            cptMean++;
+          }
+          meanIntensityRight /= cptMean;
           this->IntensityGap[scanLine][index] = std::abs(meanIntensityLeft - meanIntensityRight);
 
           // Remove neighbor points to get the best intensity discontinuity locally
@@ -437,10 +423,6 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
         }
       }
 
-      // Check point is not too far from the fitted lines before computing an angle
-      if (leftLine.DistanceToPoint(centralPoint) > this->MaxDistance || rightLine.DistanceToPoint(centralPoint) > this->MaxDistance)
-        continue;
-
       if (this->Enabled[PLANE] || this->Enabled[EDGE])
       {
         // Compute angles
@@ -449,10 +431,11 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
         if (this->Enabled[EDGE] && this->Angles[scanLine][index] > this->EdgeSinAngleThreshold)
         {
           // Check previously computed angle to keep only the maximum angle keypoint locally
-          for (int indexLeft : leftNeighbors)
+          // We avoid first point as it's the central point itself
+          for (int i = 1; i < leftNeighbors.size(); i++)
           {
-            if (this->Angles[scanLine][indexLeft] <= this->Angles[scanLine][index])
-              this->Angles[scanLine][indexLeft] = -1;
+            if (this->Angles[scanLine][leftNeighbors[i]] <= this->Angles[scanLine][index])
+              this->Angles[scanLine][leftNeighbors[i]] = -1;
             else
             {
               this->Angles[scanLine][index] = -1;
-- 
GitLab


From 85ee3119e5787b9973677aebcd2265d1193414e5 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Wed, 31 Jan 2024 10:54:18 +0100
Subject: [PATCH 04/13] [ROS][style] Add forgotten and move some parameters in
 config files

---
 .../lidar_slam/params/slam_config_indoor.yaml | 19 ++++++++++++-------
 .../params/slam_config_outdoor.yaml           | 19 ++++++++++++-------
 ros_wrapping/lidar_slam/src/LidarSlamNode.cxx |  4 ++--
 3 files changed, 26 insertions(+), 16 deletions(-)

diff --git a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
index 464c0157b..19c5c9a1c 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
@@ -371,15 +371,13 @@ slam:
                             # and the parameterization : see leaf_size keyframes parameters for more details
 
   # Keypoint extractor for each LiDAR sensor
-  # Single LiDAR input
   ke:
+    # Single LiDAR input
     enable:
       edge: true
       intensity_edge: true
       plane: true
       blob: false
-    max_points: 1000                   # [points] Maximum number of keypoints of each type to extract
-    voxel_grid_resolution: 1.          # [m/voxel] Size of a voxel to downsample the extracted keypoints
     input_sampling_ratio: 1.           # [0-1] Ratio of points from which to extract the keypoints (for computation time issues)
     min_distance_to_sensor: 1.         # [m] Minimal point to sensor distance to consider a point as valid.
     max_distance_to_sensor: 200.       # [m] Maximal point to sensor distance to consider a point as valid.
@@ -393,6 +391,9 @@ slam:
     edge_depth_gap_threshold: 0.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     edge_intensity_gap_threshold: 20.  # [0-255] Threshold upon intensity gap to select an edge keypoint.
+    downsampling:
+      max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
+      voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
 
     # # Multi LiDAR inputs
     # # The multiple devices to use for SLAM.
@@ -407,10 +408,9 @@ slam:
     #     intensity_edge: true
     #     plane: true
     #     blob: false
-    #   max_points: 1000                   # [points] Maximum number of keypoints of each type to extract
-    #   voxel_grid_resolution: 0.5         # [m/voxel] Size of a voxel to downsample the extracted keypoints
     #   input_sampling_ratio: 1.           # [0-1] Ratio of points from which to extract the keypoints (for computation time issues)
     #   min_distance_to_sensor: 1.         # [m] Minimal point to sensor distance to consider a point as valid.
+    #   max_distance_to_sensor: 200.       # [m] Maximal point to sensor distance to consider a point as valid.
     #   min_azimuth: -90.                  # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   max_azimuth: 90.                   # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
@@ -421,16 +421,18 @@ slam:
     #   edge_depth_gap_threshold: 0.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     #   edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     #   edge_intensity_gap_threshold: 20.  # [0-255] Threshold upon intensity gap to select an edge keypoint.
+    #   downsampling:
+    #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
+    #     voxel_grid_resolution: 0.5       # [m/voxel] Size of a voxel to downsample the extracted keypoints.
     # device_lidar_frame_1:
     #   enable:
     #     edge: true
     #     intensity_edge: true
     #     plane: true
     #     blob: false
-    #   max_points: 1000                   # [points] Maximum number of keypoints of each type to extract
-    #   voxel_grid_resolution: 0.5         # [m/voxel] Size of a voxel to downsample the extracted keypoints
     #   input_sampling_ratio: 0.6          # [0-1] Ratio of points from which to extract the keypoints (for computation time issues)
     #   min_distance_to_sensor: 1.         # [m] Minimal point to sensor distance to consider a point as valid.
+    #   max_distance_to_sensor: 200.       # [m] Maximal point to sensor distance to consider a point as valid.
     #   min_azimuth: -45.                  # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   max_azimuth: 45.                   # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
@@ -441,3 +443,6 @@ slam:
     #   edge_depth_gap_threshold: 0.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     #   edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     #   edge_intensity_gap_threshold: 200. # [0-255] Threshold upon intensity gap to select an edge keypoint.
+    #   downsampling:
+    #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
+    #     voxel_grid_resolution: 0.5       # [m/voxel] Size of a voxel to downsample the extracted keypoints.
diff --git a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
index 82d08de0f..d7a3cc297 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
@@ -372,15 +372,13 @@ slam:
                             # and the parameterization : see leaf_size keyframes parameters for more details
 
   # Keypoint extractor for each LiDAR sensor
-  # Single LiDAR input
   ke:
+    # Single LiDAR input
     enable:
       edge: true
       intensity_edge: true
       plane: true
       blob: false
-    max_points: 1000                   # [points] Maximum number of keypoints of each type to extract
-    voxel_grid_resolution: 2.          # [m/voxel] Size of a voxel to downsample the extracted keypoints
     input_sampling_ratio: 1.           # [0-1] Ratio of points from which to extract the keypoints (for computation time issues)
     min_distance_to_sensor: 1.5        # [m] Minimal point to sensor distance to consider a point as valid.
     max_distance_to_sensor: 200.       # [m] Maximal point to sensor distance to consider a point as valid.
@@ -394,6 +392,9 @@ slam:
     edge_depth_gap_threshold: 1.       # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     edge_intensity_gap_threshold: 50.  # [0-255] Threshold upon intensity gap to select an edge keypoint.
+    downsampling:
+      max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
+      voxel_grid_resolution: 2.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
 
     # # Multi LiDAR inputs
     # # The multiple devices to use for SLAM.
@@ -408,10 +409,9 @@ slam:
     #     intensity_edge: true
     #     plane: true
     #     blob: false
-    #   max_points: 1000                   # [points] Maximum number of keypoints of each type to extract
-    #   voxel_grid_resolution: 1.          # [m/voxel] Size of a voxel to downsample the extracted keypoints
     #   input_sampling_ratio: 0.6          # [0-1] Ratio of points from which to extract the keypoints (for computation time issues)
     #   min_distance_to_sensor: 1.5        # [m] Minimal point to sensor distance to consider a point as valid.
+    #   max_distance_to_sensor: 200.       # [m] Maximal point to sensor distance to consider a point as valid.
     #   min_azimuth: -45.                  # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   max_azimuth: 45.                   # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
@@ -422,16 +422,18 @@ slam:
     #   edge_depth_gap_threshold: 1.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     #   edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     #   edge_intensity_gap_threshold: 30.  # [0-255] Threshold upon intensity gap to select an edge keypoint.
+    #   downsampling:
+    #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
+    #     voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
     # device_lidar_frame_1:
     #   enable:
     #     edge: true
     #     intensity_edge: true
     #     plane: true
     #     blob: false
-    #   max_points: 1000                   # [points] Maximum number of keypoints of each type to extract
-    #   voxel_grid_resolution: 1.          # [m/voxel] Size of a voxel to downsample the extracted keypoints
     #   input_sampling_ratio: 1.           # [0-1] Ratio of points from which to extract the keypoints (for computation time issues)
     #   min_distance_to_sensor: 1.5        # [m] Minimal point to sensor distance to consider a point as valid.
+    #   max_distance_to_sensor: 200.       # [m] Maximal point to sensor distance to consider a point as valid.
     #   min_azimuth: -45.                  # [°] Minimal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   max_azimuth: 45.                   # [°] Maximal azimuth angle to consider the point as keypoint. Right thumb rule between min/max azimuth angles.
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
@@ -442,3 +444,6 @@ slam:
     #   edge_depth_gap_threshold: 1.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     #   edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     #   edge_intensity_gap_threshold: 200. # [0-255] Threshold upon intensity gap to select an edge keypoint.
+    #   downsampling:
+    #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
+    #     voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
\ No newline at end of file
diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
index 80fbf1b1b..bb5ac1a73 100644
--- a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
+++ b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
@@ -1747,8 +1747,8 @@ void LidarSlamNode::SetSlamParameters()
     SetKeypointsExtractorParam(float, prefix + "edge_depth_gap_threshold", EdgeDepthGapThreshold)
     SetKeypointsExtractorParam(float, prefix + "edge_nb_gap_points", EdgeNbGapPoints)
     SetKeypointsExtractorParam(float, prefix + "edge_intensity_gap_threshold", EdgeIntensityGapThreshold)
-    SetKeypointsExtractorParam(int,   prefix + "max_points", MaxPoints)
-    SetKeypointsExtractorParam(float, prefix + "voxel_grid_resolution", VoxelResolution)
+    SetKeypointsExtractorParam(int,   prefix + "downsampling/max_points", MaxPoints)
+    SetKeypointsExtractorParam(float, prefix + "downsampling/voxel_grid_resolution", VoxelResolution)
     SetKeypointsExtractorParam(float, prefix + "input_sampling_ratio", InputSamplingRatio)
     #define EnableKeypoint(kType) \
     { \
-- 
GitLab


From 24208a583d238a954f39d572d8dd8d0fb1bb94d0 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Tue, 5 Dec 2023 13:40:47 +0100
Subject: [PATCH 05/13] [refact] Create a parent abstract class for keypoint
 extractors

As we were adding a new keypoint extractor, we decided to create a KeypointExtractor class defining all common methods and parameters useful in keypoint extractors.
---
 .../lidar_slam/params/slam_config_indoor.yaml |  12 +
 .../params/slam_config_outdoor.yaml           |  12 +
 ros_wrapping/lidar_slam/src/LidarSlamNode.cxx |  75 +++--
 slam_lib/CMakeLists.txt                       |   4 +
 .../DenseSpinningSensorKeypointExtractor.h    |  81 ++++++
 slam_lib/include/LidarSlam/Enums.h            |  13 +
 .../include/LidarSlam/KeypointExtractor.h     | 274 ++++++++++++++++++
 slam_lib/include/LidarSlam/Slam.h             |   4 +-
 .../SpinningSensorKeypointExtractor.h         | 182 +-----------
 .../DenseSpinningSensorKeypointExtractor.cxx  |  86 ++++++
 slam_lib/src/KeypointExtractor.cxx            | 177 +++++++++++
 .../src/SpinningSensorKeypointExtractor.cxx   | 226 +++------------
 12 files changed, 770 insertions(+), 376 deletions(-)
 create mode 100644 slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
 create mode 100644 slam_lib/include/LidarSlam/KeypointExtractor.h
 create mode 100644 slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
 create mode 100644 slam_lib/src/KeypointExtractor.cxx

diff --git a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
index 19c5c9a1c..8b8df31e2 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
@@ -373,6 +373,10 @@ slam:
   # Keypoint extractor for each LiDAR sensor
   ke:
     # Single LiDAR input
+    # The extractor modes allow to decide which keypoint extractor to choose
+    # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
+    # 1) Dense extractor called DenseSpinningSensorKeypointExtractor. Better option for lidars with 64 and 128 lasers.
+    extractor_mode: 0
     enable:
       edge: true
       intensity_edge: true
@@ -403,6 +407,10 @@ slam:
     #   - "lidar_frame_1"
     # # Keypoint extractors parameters for each LiDAR sensor "device_<device_id>"
     # device_lidar_frame_0:
+    #   # The extractor modes allow to decide which keypoint extractor to choose
+    #   # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
+    #   # 1) Dense extractor called DenseSpinningSensorKeypointExtractor. Better option for lidars with 64 and 128 lasers.
+    #   extractor_mode: 0
     #   enable:
     #     edge: true
     #     intensity_edge: true
@@ -425,6 +433,10 @@ slam:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
     #     voxel_grid_resolution: 0.5       # [m/voxel] Size of a voxel to downsample the extracted keypoints.
     # device_lidar_frame_1:
+    #   # The extractor modes allow to decide which keypoint extractor to choose
+    #   # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
+    #   # 1) Dense extractor called DenseSpinningSensorKeypointExtractor. Better option for lidars with 64 and 128 lasers.
+    #   extractor_mode: 0
     #   enable:
     #     edge: true
     #     intensity_edge: true
diff --git a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
index d7a3cc297..ff0992b78 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
@@ -374,6 +374,10 @@ slam:
   # Keypoint extractor for each LiDAR sensor
   ke:
     # Single LiDAR input
+    # The extractor modes allow to decide which keypoint extractor to choose
+    # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
+    # 1) Dense extractor called DenseSpinningSensorKeypointExtractor. Better option for lidars with 64 and 128 lasers.
+    extractor_mode: 0
     enable:
       edge: true
       intensity_edge: true
@@ -404,6 +408,10 @@ slam:
     #   - "lidar_frame_1"
     # # Keypoint extractors parameters for each LiDAR sensor "device_<device_id>"
     # device_lidar_frame_0:
+    #   # The extractor modes allow to decide which keypoint extractor to choose
+    #   # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
+    #   # 1) Dense extractor called DenseSpinningSensorKeypointExtractor. Better option for lidars with 64 and 128 lasers.
+    #   extractor_mode: 0
     #   enable:
     #     edge: true
     #     intensity_edge: true
@@ -426,6 +434,10 @@ slam:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
     #     voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
     # device_lidar_frame_1:
+    #   # The extractor modes allow to decide which keypoint extractor to choose
+    #   # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
+    #   # 1) Dense extractor called DenseSpinningSensorKeypointExtractor. Better option for lidars with 64 and 128 lasers.
+    #   extractor_mode: 0
     #   enable:
     #     edge: true
     #     intensity_edge: true
diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
index bb5ac1a73..1444b2c3a 100644
--- a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
+++ b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
@@ -1731,6 +1731,24 @@ void LidarSlamNode::SetSlamParameters()
   this->LidarSlam.SetBaseFrameId(this->TrackingFrameId);
 
   // Keypoint extractors
+  auto setExtractorMode = [this](LidarSlam::KeypointExtractorMode& mode,
+                                 const std::string& prefix,
+                                 const std::string& message = "")
+  {
+    int extractorMode;
+    if (this->PrivNh.getParam(prefix + "extractor_mode", extractorMode))
+    {
+      mode = static_cast<LidarSlam::KeypointExtractorMode>(extractorMode);
+      if (mode != LidarSlam::KeypointExtractorMode::SPARSE &&
+          mode != LidarSlam::KeypointExtractorMode::DENSE)
+      {
+        ROS_ERROR_STREAM("Incorrect keypoint extractor mode value (" << extractorMode << ")"
+                         << message << ". Setting it to '0', corresponding to sparse extractor.");
+        mode = LidarSlam::KeypointExtractorMode::SPARSE;
+      }
+    }
+  };
+
   auto initKeypointsExtractor = [this](auto& ke, const std::string& prefix)
   {
     #define SetKeypointsExtractorParam(type, rosParam, keParam) {type val; if (this->PrivNh.getParam(rosParam, val)) ke->Set##keParam(val);}
@@ -1767,33 +1785,48 @@ void LidarSlamNode::SetSlamParameters()
     EnableKeypoint(LidarSlam::Keypoint::BLOB);
   };
 
-  // Multi-LiDAR devices
+  auto addExtractor = [this](auto& ke,
+                             const std::string& name,
+                             const std::string& message = "",
+                             const std::string& deviceId = "")
+  {
+    deviceId != "" ? this->LidarSlam.SetKeyPointsExtractor(ke, deviceId)
+                   : this->LidarSlam.SetKeyPointsExtractor(ke);
+    ROS_INFO_STREAM("Adding " << name << " keypoints extractor " << message);
+  };
+
   std::vector<std::string> deviceIds;
   if (this->PrivNh.getParam("slam/ke/device_ids", deviceIds))
-  {
     ROS_INFO_STREAM("Multi-LiDAR devices setup");
-    for (auto deviceId: deviceIds)
-    {
-      // Init Keypoint extractor with default params
-      auto ke = std::make_shared<LidarSlam::SpinningSensorKeypointExtractor>();
-
-      // Change default parameters using ROS parameter server
-      std::string prefix = "slam/ke/device_" + deviceId + "/";
-      initKeypointsExtractor(ke, prefix);
-
-      // Add extractor to SLAM
-      this->LidarSlam.SetKeyPointsExtractor(ke, deviceId);
-      ROS_INFO_STREAM("Adding keypoints extractor for LiDAR device " << deviceId);
-    }
-  }
-  // Single LiDAR device
   else
   {
     ROS_INFO_STREAM("Single LiDAR device setup");
-    auto ke = std::make_shared<LidarSlam::SpinningSensorKeypointExtractor>();
-    initKeypointsExtractor(ke, "slam/ke/");
-    this->LidarSlam.SetKeyPointsExtractor(ke);
-    ROS_INFO_STREAM("Adding keypoints extractor");
+    deviceIds.push_back("");
+  }
+
+  for (auto deviceId: deviceIds)
+  {
+    std::string prefix = "slam/ke/";
+    if (deviceIds.size() > 1)
+      prefix += "device_" + deviceId + "/";
+
+    // Set keypoints extractor mode. If multilidar, will add an information in error message.
+    LidarSlam::KeypointExtractorMode mode;
+    deviceIds.size() == 1 ? setExtractorMode(mode, prefix)
+                          : setExtractorMode(mode, prefix, " for LiDAR device " + deviceId);
+
+    // Set keypoints extractor
+    std::shared_ptr<LidarSlam::KeypointExtractor> ke;
+    if (mode == LidarSlam::KeypointExtractorMode::SPARSE)
+      ke = std::make_shared<LidarSlam::SpinningSensorKeypointExtractor>();
+    else
+      ke = std::make_shared<LidarSlam::DenseSpinningSensorKeypointExtractor>();
+
+    // Set keypoints extractor parameters and enable keypoints
+    initKeypointsExtractor(ke, prefix);
+    // Add extractor to LidarSlam
+    deviceIds.size() == 1 ? addExtractor(ke, LidarSlam::KeypointExtractorModeNames.at(mode))
+                          : addExtractor(ke, LidarSlam::KeypointExtractorModeNames.at(mode), "for LiDAR device " + deviceId, deviceId);
   }
 
   // Ego motion
diff --git a/slam_lib/CMakeLists.txt b/slam_lib/CMakeLists.txt
index 420bd65cf..f0f1674bc 100644
--- a/slam_lib/CMakeLists.txt
+++ b/slam_lib/CMakeLists.txt
@@ -33,7 +33,9 @@ add_library(LidarSlam
   src/RollingGrid.cxx
   src/ExternalSensorManagers.cxx
   src/Slam.cxx
+  src/KeypointExtractor.cxx
   src/SpinningSensorKeypointExtractor.cxx
+  src/DenseSpinningSensorKeypointExtractor.cxx
   src/Utilities.cxx
   src/VoxelGrid.cxx
   src/InterpolationModels.cxx
@@ -118,10 +120,12 @@ endif()
 set(target_public_header
     ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/CeresCostFunctions.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/ConfidenceEstimators.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/Enums.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/ExternalSensorManagers.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/InterpolationModels.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/KDTreePCLAdaptor.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/KeypointExtractor.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/KeypointsMatcher.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/LidarPoint.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/LidarSlam/LocalOptimizer.h
diff --git a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
new file mode 100644
index 000000000..f7874b406
--- /dev/null
+++ b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
@@ -0,0 +1,81 @@
+//==============================================================================
+// Copyright 2022 Kitware, Inc., Kitware SAS
+// Author: Jeanne Faure (Kitware SAS)
+// Creation date: 2023-10-12
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//==============================================================================
+
+#pragma once
+
+#include "Utilities.h"
+#include "LidarPoint.h"
+#include "Enums.h"
+#include "KeypointExtractor.h"
+#include <unordered_map>
+
+#define SetMacro(name,type) void Set##name (type _arg) { name = _arg; }
+#define GetMacro(name,type) type Get##name () const { return name; }
+
+namespace LidarSlam
+{
+class DenseSpinningSensorKeypointExtractor : public KeypointExtractor
+{
+public:
+  // Extract keypoints from the pointcloud. The key points
+  // will be separated in two classes : Edges keypoints which
+  // correspond to area with high curvature scan lines and
+  // planar keypoints which have small curvature.
+  // NOTE: This expects that the lowest/bottom laser_id is 0, and is increasing upward.
+  void ComputeKeyPoints(const PointCloud::Ptr& pc) override;
+
+  PointCloud::Ptr GetKeypoints(Keypoint k) override;
+
+  // Function to enable to have some inside on why a given point was detected as a keypoint
+  std::unordered_map<std::string, std::vector<float>> GetDebugArray() const override;
+
+private:
+  // Compute the curvature features within each scan line : depth
+  // space gap, intensity gap and line angle
+  void ComputeCurvature() override;
+
+  // Labelize points (unvalid, edge, plane)
+  // and extract them in correspondant pointcloud
+  // note : blobs are extracted identically in SSKE and DSSKE so ComputeBlobs() is defined in parent abstract class
+  void ComputePlanes() override;
+  void ComputeEdges() override;
+  void ComputeIntensityEdges() override;
+
+  // Add point to keypoint structure
+  void AddKeypoint(const Keypoint& k, const LidarPoint &pt) override;
+
+  // Add all keypoints of the type k that comply with the threshold criteria for these values
+  // The threshold can be a minimum or maximum value (threshIsMax)
+  // The weight basis allow to weight the keypoint depending on its certainty
+  void AddKptsUsingCriterion (Keypoint k);
+
+  // ---------------------------------------------------------------------------
+  //   Parameters
+  // ---------------------------------------------------------------------------
+
+  //TODO
+
+  // ---------------------------------------------------------------------------
+  //   Internal variables
+  // ---------------------------------------------------------------------------
+
+  //TODO
+
+};
+
+} // namespace LidarSlam
\ No newline at end of file
diff --git a/slam_lib/include/LidarSlam/Enums.h b/slam_lib/include/LidarSlam/Enums.h
index 1252e86bb..a18a96c3c 100644
--- a/slam_lib/include/LidarSlam/Enums.h
+++ b/slam_lib/include/LidarSlam/Enums.h
@@ -102,6 +102,19 @@ enum class EgoMotionMode
   EXTERNAL_OR_MOTION_EXTRAPOLATION = 5
 };
 
+enum KeypointExtractorMode
+{
+  //! Extract keypoints using SpinningSensorKeypointExtractor, convenient for all lidars.
+  SPARSE = 0,
+
+  //! Extract keypoints using DenseSpinningSensorKeypointExtractor, better option for lidars with 64 and 128 lasers.
+  DENSE = 1,
+
+  nbKeypointExtractorModes = 2
+};
+
+static const std::map<KeypointExtractorMode, std::string> KeypointExtractorModeNames = { {SPARSE, "sparse"}, {DENSE, "dense"} };
+
 //------------------------------------------------------------------------------
 namespace Interpolation
 {
diff --git a/slam_lib/include/LidarSlam/KeypointExtractor.h b/slam_lib/include/LidarSlam/KeypointExtractor.h
new file mode 100644
index 000000000..84f767dbc
--- /dev/null
+++ b/slam_lib/include/LidarSlam/KeypointExtractor.h
@@ -0,0 +1,274 @@
+//==============================================================================
+// Copyright 2022 Kitware, Inc., Kitware SAS
+// Author: Jeanne Faure (Kitware SAS)
+// Creation date: 2023-12-01
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//==============================================================================
+
+#pragma once
+
+#ifndef KEYPOINT_EXTRACTOR_H
+#define KEYPOINT_EXTRACTOR_H
+
+#include "Utilities.h"
+#include "LidarPoint.h"
+#include "Enums.h"
+#include <unordered_map>
+
+#define SetMacro(name,type) void Set##name (type _arg) { name = _arg; }
+#define GetMacro(name,type) type Get##name () const { return name; }
+
+namespace LidarSlam
+{
+
+struct LineFitting
+{
+  using Point = LidarPoint;
+  using PointCloud = pcl::PointCloud<Point>;
+
+  //! Fitting using very local line and check if this local
+  //! line is consistent in a more global neighborhood.
+  //! Warning : this implies factorial calculations relatively to the points number
+  bool FitLineAndCheckConsistency(const PointCloud& cloud,
+                                  const std::vector<int>& indices);
+
+  //! Compute the squared distance of a point to the fitted line
+  inline float DistanceToPoint(const Eigen::Vector3f& point) const
+  {
+    return ((point - this->Position).cross(this->Direction)).norm();
+  };
+
+  // Direction and position
+  Eigen::Vector3f Direction = Eigen::Vector3f::Zero();
+  Eigen::Vector3f Position = Eigen::Vector3f::Zero();
+
+  //! Ratio (squared) for squared distances of direct neighbors to consider a neighborhood as valid to fit line on
+  float SquaredRatio = 100.; // [.]
+
+  // Ratio between length and width to be trustworthy
+  float LengthWidthRatio = 10.; // [.]
+};
+
+class KeypointExtractor
+{
+public:
+  using Point = LidarPoint;
+  using PointCloud = pcl::PointCloud<Point>;
+
+  GetMacro(NbThreads, int)
+  SetMacro(NbThreads, int)
+
+  GetMacro(MaxPoints, int)
+  SetMacro(MaxPoints, int)
+
+  GetMacro(InputSamplingRatio, float)
+  SetMacro(InputSamplingRatio, float)
+
+  GetMacro(MinNeighNb, int)
+  SetMacro(MinNeighNb, int)
+
+  GetMacro(MinNeighRadius, float)
+  SetMacro(MinNeighRadius, float)
+
+  GetMacro(MinDistanceToSensor, float)
+  SetMacro(MinDistanceToSensor, float)
+
+  GetMacro(MaxDistanceToSensor, float)
+  SetMacro(MaxDistanceToSensor, float)
+
+  GetMacro(PlaneSinAngleThreshold, float)
+  SetMacro(PlaneSinAngleThreshold, float)
+
+  GetMacro(EdgeSinAngleThreshold, float)
+  SetMacro(EdgeSinAngleThreshold, float)
+
+  GetMacro(EdgeDepthGapThreshold, float)
+  SetMacro(EdgeDepthGapThreshold, float)
+
+  GetMacro(EdgeIntensityGapThreshold, float)
+  SetMacro(EdgeIntensityGapThreshold, float)
+
+  GetMacro(AzimuthalResolution, float)
+  SetMacro(AzimuthalResolution, float)
+
+  GetMacro(EdgeNbGapPoints, int)
+  SetMacro(EdgeNbGapPoints, int)
+
+  GetMacro(VoxelResolution, float)
+  SetMacro(VoxelResolution, float)
+
+  GetMacro(NbLaserRings, unsigned int)
+
+  void SetAzimuthMin(float angle)
+  {
+    float azimMin = Utils::Deg2Rad(angle);
+    while (azimMin < 0)
+      azimMin += 2 * M_PI;
+    this->AzimuthMin = azimMin;
+  };
+  float GetAzimuthMin()
+  {
+    return Utils::Rad2Deg(this->AzimuthMin);
+  };
+  void SetAzimuthMax(float angle)
+  {
+    float azimMax = Utils::Deg2Rad(angle);
+    while (azimMax < 0)
+      azimMax += 2 * M_PI;
+    this->AzimuthMax = azimMax;
+  };
+  float GetAzimuthMax()
+  {
+    return Utils::Rad2Deg(this->AzimuthMax);
+  };
+
+  void SetMinBeamSurfaceAngle(float angle)
+  {
+    this->MinBeamSurfaceAngle = std::cos(Utils::Deg2Rad(angle));
+  };
+  GetMacro(MinBeamSurfaceAngle, float)
+
+  // Select the keypoint types to extract
+  // This function resets the member map "Enabled"
+  void Enable(const std::vector<Keypoint>& kptTypes);
+
+  // Extract keypoints from the pointcloud. The key points
+  // will be separated in two classes : Edges keypoints which
+  // correspond to area with high curvature scan lines and
+  // planar keypoints which have small curvature.
+  // NOTE: This expects that the lowest/bottom laser_id is 0, and is increasing upward.
+  virtual void ComputeKeyPoints(const PointCloud::Ptr& pc) = 0;
+
+  virtual PointCloud::Ptr GetKeypoints(Keypoint k) = 0;
+
+  // Function to enable to have some inside on why a given point was detected as a keypoint
+  virtual std::unordered_map<std::string, std::vector<float>> GetDebugArray() const = 0;
+
+protected:
+
+  // Check if scanLine is almost empty
+  inline bool IsScanLineAlmostEmpty(int nScanLinePts) const { return nScanLinePts < 2 * this->MinNeighNb + 1; };
+
+  // Auto estimate azimuth angle resolution based on current ScanLines
+  // WARNING: to be correct, the points need to be in the LIDAR sensor
+  // coordinates system, where the sensor is spinning around Z axis.
+  void EstimateAzimuthalResolution();
+
+  // Compute the curvature and other features within each the scan line.
+  // The curvature is not the one of the surface that intersects the lines but
+  // the 1D curvature within each isolated scan line.
+  virtual void ComputeCurvature() = 0;
+
+  // Labelize points (unvalid, edge, plane, blob)
+  // and extract them in correspondant pointcloud
+  virtual void ComputePlanes() = 0;
+  virtual void ComputeEdges() = 0;
+  virtual void ComputeIntensityEdges() = 0;
+  void ComputeBlobs();
+
+  // Add point to the keypoint pointcloud
+  virtual void AddKeypoint(const Keypoint& k, const LidarPoint &pt) = 0;
+
+  // ---------------------------------------------------------------------------
+  //   Helpers
+  // ---------------------------------------------------------------------------
+
+  // Helpers for ComputeCurvature:
+  // Check if a point is inside the range of distance to sensor
+  inline bool CheckDistanceToSensor(float centralDepth) {return centralDepth >= this->MinDistanceToSensor && centralDepth < this->MaxDistanceToSensor;};
+  // Check if the azimuth of a point is inside the range of azimuth angles
+  bool CheckAzimuthAngle(const Eigen::Vector3f& centralPoint);
+  // Compute the cos of the azimuth between the central point and the point
+  inline float ComputeCosAngle(const Eigen::Vector3f& point, const Eigen::Vector3f& centralPt, float depth, float centralDepth)
+  {
+    return std::abs(point.dot(centralPt) / (depth * centralDepth));
+  };
+  // Compare the cos of the azimuth between the central point and the point to the max = azimuthal resolution
+  inline bool IsAngleValid(float cosAngle) {return (cosAngle >= std::cos(1.5 * this->AzimuthalResolution));};
+  // Compare the cos of the angle between the line and the central point to the threshold
+  bool IsBeamAngleValid(const Eigen::Vector3f& centralPt, float centralDepth, const LineFitting& line);
+
+  // ---------------------------------------------------------------------------
+  //   Parameters
+  // ---------------------------------------------------------------------------
+
+  // Keypoints activated
+  std::map<Keypoint, bool> Enabled = {{EDGE, true}, {INTENSITY_EDGE, true}, {PLANE, true}, {BLOB, false}};
+
+  // Max number of threads to use to process points in parallel
+  int NbThreads = 1;
+
+  // Maximum number of keypoints to extract
+  int MaxPoints = INT_MAX;
+
+  // Sampling ratio to perform for real time issues
+  float InputSamplingRatio = 1.;
+
+  // Minimum number of points used on each side of the studied point to compute its curvature
+  int MinNeighNb = 5;
+
+  // Minimum radius to define the neighborhood to compute curvature of a studied point
+  float MinNeighRadius = 0.10f;
+
+  // Minimal point/sensor sensor to consider a point as valid
+  float MinDistanceToSensor = 1.5;  // [m]
+
+  // Maximal point/sensor sensor to consider a point as valid
+  float MaxDistanceToSensor = 200.;  // [m]
+
+  // Minimum angle between laser beam and surface to consider a point as valid
+  float MinBeamSurfaceAngle = 10; // [°]
+
+  float AzimuthMin = 0.f;        // [rad]
+  float AzimuthMax = 2.f * M_PI; // [rad]
+
+  // Sharpness threshold to select a planar keypoint
+  float PlaneSinAngleThreshold = 0.5;  // sin(30°) (selected if sin angle is less than threshold)
+
+  // Sharpness threshold to select an edge keypoint
+  float EdgeSinAngleThreshold = 0.86;  // ~sin(60°) (selected, if sin angle is more than threshold)
+
+  // Threshold upon depth gap in neighborhood to select an edge keypoint
+  float EdgeDepthGapThreshold = 0.5;  // [m]
+
+  // Threshold upon intensity gap to select an edge keypoint
+  float EdgeIntensityGapThreshold = 50.;
+
+  // Nb of points missed to define a space gap
+  int EdgeNbGapPoints = 3; // [nb]
+
+  // Size of a voxel used to downsample the keypoints
+  // It corresponds approx to the mean distance between closest neighbors in the output keypoints cloud.
+  float VoxelResolution = 0.1; // [m]
+
+  // ---------------------------------------------------------------------------
+  //   Internal variables
+  // ---------------------------------------------------------------------------
+
+  // Azimuthal (= horizontal angle) resolution of the spinning lidar sensor
+  // If it is less or equal to 0, it will be auto-estimated from next frame.
+  // This angular resolution is used to compute an expected distance between two
+  // consecutives firings.
+  float AzimuthalResolution;
+
+  // Number of lasers scan lines composing the pointcloud
+  unsigned int NbLaserRings;
+
+  // Current point cloud
+  PointCloud::Ptr Scan;
+
+};
+} // end of namespace LidarSlam
+
+#endif // KEYPOINT_EXTRACTOR_H
diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h
index cc45ecb46..172611003 100644
--- a/slam_lib/include/LidarSlam/Slam.h
+++ b/slam_lib/include/LidarSlam/Slam.h
@@ -76,7 +76,9 @@
 #include "LidarSlam/Utilities.h"
 #include "LidarSlam/LidarPoint.h"
 #include "LidarSlam/Enums.h"
+#include "LidarSlam/KeypointExtractor.h"
 #include "LidarSlam/SpinningSensorKeypointExtractor.h"
+#include "LidarSlam/DenseSpinningSensorKeypointExtractor.h"
 #include "LidarSlam/KeypointsMatcher.h"
 #include "LidarSlam/LocalOptimizer.h"
 #include "LidarSlam/RollingGrid.h"
@@ -276,7 +278,7 @@ public:
   // Usefull types
   using Point = LidarPoint;
   using PointCloud = pcl::PointCloud<Point>;
-  using KeypointExtractorPtr = std::shared_ptr<SpinningSensorKeypointExtractor>;
+  using KeypointExtractorPtr = std::shared_ptr<KeypointExtractor>;
   using PCStorage = PointCloudStorage<LidarPoint>;
   using Maps = std::map<Keypoint, std::shared_ptr<RollingGrid>>;
 
diff --git a/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
index b085845d4..4c63503cd 100644
--- a/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
@@ -23,6 +23,7 @@
 #include "LidarSlam/LidarPoint.h"
 #include "LidarSlam/Enums.h"
 #include "LidarSlam/VoxelGrid.h"
+#include "KeypointExtractor.h"
 
 #include <pcl/point_cloud.h>
 
@@ -36,110 +37,23 @@
 
 namespace LidarSlam
 {
-
-namespace
-{
-//-----------------------------------------------------------------------------
-struct LineFitting
-{
-  using Point = LidarPoint;
-  using PointCloud = pcl::PointCloud<Point>;
-
-  //! Fitting using very local line and check if this local
-  //! line is consistent in a more global neighborhood.
-  //! Warning : this implies factorial calculations relatively to the points number
-  bool FitLineAndCheckConsistency(const PointCloud& cloud,
-                                  const std::vector<int>& indices);
-
-  //! Compute the squared distance of a point to the fitted line
-  inline float DistanceToPoint(Eigen::Vector3f const& point) const;
-
-  // Direction and position
-  Eigen::Vector3f Direction = Eigen::Vector3f::Zero();
-  Eigen::Vector3f Position = Eigen::Vector3f::Zero();
-
-  //! Ratio (squared) for squared distances of direct neighbors to consider a neighborhood as valid to fit line on
-  float SquaredRatio = 100.; // [.]
-
-  // Ratio between length and width to be trustworthy
-  float LengthWidthRatio = 10.; // [.]
-};
-} // End of anonymous namespace
-
-class SpinningSensorKeypointExtractor
+class SpinningSensorKeypointExtractor : public KeypointExtractor
 {
 public:
-  using Point = LidarPoint;
-  using PointCloud = pcl::PointCloud<Point>;
-
-  GetMacro(NbThreads, int)
-  SetMacro(NbThreads, int)
-
-  GetMacro(MaxPoints, int)
-  SetMacro(MaxPoints, int)
-
-  GetMacro(InputSamplingRatio, float)
-  SetMacro(InputSamplingRatio, float)
-
-  GetMacro(MinNeighNb, int)
-  SetMacro(MinNeighNb, int)
-
-  GetMacro(MinNeighRadius, float)
-  SetMacro(MinNeighRadius, float)
-
-  GetMacro(MinDistanceToSensor, float)
-  SetMacro(MinDistanceToSensor, float)
-
-  GetMacro(MaxDistanceToSensor, float)
-  SetMacro(MaxDistanceToSensor, float)
-
-  GetMacro(AzimuthMin, float)
-  SetMacro(AzimuthMin, float)
-
-  GetMacro(AzimuthMax, float)
-  SetMacro(AzimuthMax, float)
-
-  GetMacro(MinBeamSurfaceAngle, float)
-  SetMacro(MinBeamSurfaceAngle, float)
-
-  GetMacro(PlaneSinAngleThreshold, float)
-  SetMacro(PlaneSinAngleThreshold, float)
-
-  GetMacro(EdgeSinAngleThreshold, float)
-  SetMacro(EdgeSinAngleThreshold, float)
-
-  GetMacro(EdgeDepthGapThreshold, float)
-  SetMacro(EdgeDepthGapThreshold, float)
-
-  GetMacro(EdgeIntensityGapThreshold, float)
-  SetMacro(EdgeIntensityGapThreshold, float)
-
-  GetMacro(AzimuthalResolution, float)
-  SetMacro(AzimuthalResolution, float)
-
-  GetMacro(EdgeNbGapPoints, int)
-  SetMacro(EdgeNbGapPoints, int)
-
   GetMacro(VoxelResolution, float)
   SetMacro(VoxelResolution, float)
 
-  GetMacro(NbLaserRings, unsigned int)
-
-  // Select the keypoint types to extract
-  // This function resets the member map "Enabled"
-  void Enable(const std::vector<Keypoint>& kptTypes);
-
-  PointCloud::Ptr GetKeypoints(Keypoint k);
-
   // Extract keypoints from the pointcloud. The key points
   // will be separated in two classes : Edges keypoints which
   // correspond to area with high curvature scan lines and
   // planar keypoints which have small curvature.
   // NOTE: This expects that the lowest/bottom laser_id is 0, and is increasing upward.
-  void ComputeKeyPoints(const PointCloud::Ptr& pc);
+  void ComputeKeyPoints(const PointCloud::Ptr& pc) override;
+
+  PointCloud::Ptr GetKeypoints(Keypoint k) override;
 
   // Function to enable to have some inside on why a given point was detected as a keypoint
-  std::unordered_map<std::string, std::vector<float>> GetDebugArray() const;
+  std::unordered_map<std::string, std::vector<float>> GetDebugArray() const override;
 
 private:
 
@@ -151,30 +65,20 @@ private:
   // Reset all the features vectors and keypoints clouds
   void PrepareDataForNextFrame();
 
-  // Invalid the points with bad criteria from the list of possible future keypoints.
-  // These points correspond to planar surfaces roughly parallel to laser beam
-  // and points close to a gap created by occlusion.
-  void InvalidateNotUsablePoints();
-
   // Compute the curvature and other features within each the scan line.
   // The curvature is not the one of the surface that intersects the lines but
   // the 1D curvature within each isolated scan line.
-  void ComputeCurvature();
+  void ComputeCurvature() override;
 
-  // Labelize points (unvalid, edge, plane, blob)
+  // Labelize points (unvalid, edge, plane)
   // and extract them in correspondant pointcloud
-  void ComputePlanes();
-  void ComputeEdges();
-  void ComputeIntensityEdges();
-  void ComputeBlobs();
-
-  // Auto estimate azimuth angle resolution based on current ScanLines
-  // WARNING: to be correct, the points need to be in the LIDAR sensor
-  // coordinates system, where the sensor is spinning around Z axis.
-  void EstimateAzimuthalResolution();
+  // note : blobs are extracted identically in SSKE and DSSKE so ComputeBlobs() is defined in parent abstract class
+  void ComputePlanes() override;
+  void ComputeEdges() override;
+  void ComputeIntensityEdges() override;
 
-  // Check if scanLine is almost empty
-  inline bool IsScanLineAlmostEmpty(int nScanLinePts) const { return nScanLinePts < 2 * this->MinNeighNb + 1; }
+  // Add point to the voxel grid
+  void AddKeypoint(const Keypoint& k, const LidarPoint &pt) override;
 
   // Add all keypoints of the type k that comply with the threshold criteria for these values
   // The threshold can be a minimum or maximum value (threshIsMax)
@@ -193,51 +97,6 @@ private:
   //   Parameters
   // ---------------------------------------------------------------------------
 
-  // Keypoints activated
-  std::map<Keypoint, bool> Enabled = {{EDGE, true}, {INTENSITY_EDGE, true}, {PLANE, true}, {BLOB, false}};
-
-  // Max number of threads to use to process points in parallel
-  int NbThreads = 1;
-
-  // Maximum number of keypoints to extract
-  int MaxPoints = INT_MAX;
-
-  // Sampling ratio to perform for real time issues
-  float InputSamplingRatio = 1.;
-
-  // Minimum number of points used on each side of the studied point to compute its curvature
-  int MinNeighNb = 5;
-
-  // Minimum radius to define the neighborhood to compute curvature of a studied point
-  float MinNeighRadius = 0.10f;
-
-  // Minimal point/sensor sensor to consider a point as valid
-  float MinDistanceToSensor = 1.5;  // [m]
-
-  // Maximal point/sensor sensor to consider a point as valid
-  float MaxDistanceToSensor = 200.;  // [m]
-
-  // Minimum angle between laser beam and surface to consider a point as valid
-  float MinBeamSurfaceAngle = 10; // [°]
-
-  float AzimuthMin = 0; // [°]
-  float AzimuthMax = 360; // [°]
-
-  // Sharpness threshold to select a planar keypoint
-  float PlaneSinAngleThreshold = 0.5;  // sin(30°) (selected if sin angle is less than threshold)
-
-  // Sharpness threshold to select an edge keypoint
-  float EdgeSinAngleThreshold = 0.86;  // ~sin(60°) (selected, if sin angle is more than threshold)
-
-  // Threshold upon depth gap in neighborhood to select an edge keypoint
-  float EdgeDepthGapThreshold = 0.5;  // [m]
-
-  // Threshold upon intensity gap to select an edge keypoint
-  float EdgeIntensityGapThreshold = 50.;
-
-  // Nb of points missed to define a space gap
-  int EdgeNbGapPoints = 5; // [nb]
-
   // Size of a voxel used to downsample the keypoints
   // It corresponds approx to the mean distance between closest neighbors in the output keypoints cloud.
   float VoxelResolution = 0.1; // [m]
@@ -246,15 +105,6 @@ private:
   //   Internal variables
   // ---------------------------------------------------------------------------
 
-  // Azimuthal (= horizontal angle) resolution of the spinning lidar sensor
-  // If it is less or equal to 0, it will be auto-estimated from next frame.
-  // This angular resolution is used to compute an expected distance between two
-  // consecutives firings.
-  float AzimuthalResolution = 0.;  // [rad]
-
-  // Number of lasers scan lines composing the pointcloud
-  unsigned int NbLaserRings = 0;
-
   //! Label of a point as a keypoint
   //! We use binary flags as each point can have different keypoint labels.
   using KeypointFlags = std::bitset<Keypoint::nKeypointTypes>;
@@ -269,9 +119,7 @@ private:
   // Extracted keypoints of current frame
   std::map<Keypoint, VoxelGrid> Keypoints;
 
-  // Current point cloud stored in two differents formats
-  PointCloud::Ptr Scan;
+  // Map of the scan lines, sorted by their laser_id.
   std::unordered_map<int, PointCloud::Ptr> ScanLines;
 };
-
 } // end of LidarSlam namespace
\ No newline at end of file
diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
new file mode 100644
index 000000000..86cf98d99
--- /dev/null
+++ b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
@@ -0,0 +1,86 @@
+//==============================================================================
+// Copyright 2022 Kitware, Inc., Kitware SAS
+// Author: Jeanne Faure (Kitware SAS)
+// Creation date: 2023-10-12
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//==============================================================================
+
+#include "LidarSlam/DenseSpinningSensorKeypointExtractor.h"
+#include "LidarSlam/Utilities.h"
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <pcl/common/common.h>
+
+#include <random>
+
+namespace LidarSlam
+{
+
+//-----------------------------------------------------------------------------
+DenseSpinningSensorKeypointExtractor::PointCloud::Ptr DenseSpinningSensorKeypointExtractor::GetKeypoints(Keypoint k)
+{
+  //TODO
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::AddKeypoint(const Keypoint& k, const Point& pt)
+{
+  //TODO
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Ptr& pc)
+{
+  //TODO
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::ComputeCurvature()
+{
+  //TODO
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::AddKptsUsingCriterion(Keypoint k)
+{
+  //TODO
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::ComputePlanes()
+{
+  //TODO
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::ComputeEdges()
+{
+  //TODO
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::ComputeIntensityEdges()
+{
+  //TODO
+}
+
+//-----------------------------------------------------------------------------
+std::unordered_map<std::string, std::vector<float>> DenseSpinningSensorKeypointExtractor::GetDebugArray() const
+{
+  //TODO
+}
+
+}// end of LidarSlam namespace
\ No newline at end of file
diff --git a/slam_lib/src/KeypointExtractor.cxx b/slam_lib/src/KeypointExtractor.cxx
new file mode 100644
index 000000000..47c432990
--- /dev/null
+++ b/slam_lib/src/KeypointExtractor.cxx
@@ -0,0 +1,177 @@
+//==============================================================================
+// Copyright 2022 Kitware, Inc., Kitware SAS
+// Author: Jeanne Faure (Kitware SAS)
+// Creation date: 2023-12-01
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//==============================================================================
+
+#include "LidarSlam/Utilities.h"
+#include "LidarSlam/KeypointExtractor.h"
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <pcl/common/common.h>
+
+#include <random>
+
+namespace LidarSlam
+{
+
+//-----------------------------------------------------------------------------
+bool LineFitting::FitLineAndCheckConsistency(const KeypointExtractor::PointCloud& cloud,
+                                             const std::vector<int>& indices)
+{
+  // Check consistency of the line
+  if (indices.size() < 2)
+    return false;
+
+  float sqDistMax = 0.f;
+  float sqDistMin = std::numeric_limits<float>::max();
+  float sqDistCurr;
+  for (int i = 1; i < indices.size(); ++i)
+  {
+    sqDistCurr = (cloud[indices[i]].getVector3fMap() - cloud[indices[i-1]].getVector3fMap()).squaredNorm();
+    sqDistMax = std::max(sqDistMax, sqDistCurr);
+    sqDistMin = std::min(sqDistMin, sqDistCurr);
+  }
+  if (sqDistMax / sqDistMin > this->SquaredRatio)
+    return false;
+
+  Eigen::Vector3f diffVec = cloud[indices.back()].getVector3fMap() - cloud[indices.front()].getVector3fMap();
+  this->Direction = diffVec.normalized();
+  Eigen::Vector4f centroid;
+  pcl::compute3DCentroid(cloud, indices, centroid);
+  this->Position = centroid.head(3);
+
+  // Check line width
+  float lineLength = diffVec.norm();
+  float widthThreshold = lineLength / this->LengthWidthRatio;
+
+  for (auto idx : indices)
+  {
+    float error = (cloud[idx].getVector3fMap() - this->Position).cross(this->Direction).norm();
+    if (error > widthThreshold)
+      return false;
+  }
+
+  return true;
+}
+
+//-----------------------------------------------------------------------------
+void KeypointExtractor::Enable(const std::vector<Keypoint>& kptTypes)
+{
+  for (auto& en : this->Enabled)
+    en.second = false;
+  for (auto& k : kptTypes)
+    this->Enabled[k] = true;
+}
+
+//-----------------------------------------------------------------------------
+void KeypointExtractor::EstimateAzimuthalResolution()
+{
+  // Compute horizontal angle values between successive points
+  std::vector<float> angles;
+  angles.reserve(this->Scan->size());
+  // Container storing the last point encountered in each line
+  // to compute the angle difference with the new one in that line
+  std::map<int, LidarPoint> prevPtByLine;
+
+  for (const LidarPoint& point : *this->Scan)
+  {
+    if (prevPtByLine.count(point.laser_id))
+    {
+      Eigen::Map<const Eigen::Vector2f> pt1(point.data);
+      Eigen::Map<const Eigen::Vector2f> pt2(prevPtByLine[point.laser_id].data);
+      float angle = std::abs(std::acos(pt1.dot(pt2) / (pt1.norm() * pt2.norm())));
+      angles.push_back(angle);
+    }
+    prevPtByLine[point.laser_id] = point;
+  }
+
+  // A minimum number of angles is needed to get a trustable estimator
+  if (angles.size() < 100)
+  {
+    PRINT_WARNING("Not enough points to estimate azimuthal resolution");
+    return;
+  }
+
+  // Estimate azimuthal resolution from these angles
+  std::sort(angles.begin(), angles.end());
+  unsigned int maxInliersIdx = angles.size();
+  float maxAngle = Utils::Deg2Rad(5.);
+  float medianAngle = 0.;
+  // Iterate until only angles between direct LiDAR beam neighbors remain.
+  // The max resolution angle is decreased at each iteration.
+  while (maxAngle > 1.8 * medianAngle)
+  {
+    maxInliersIdx = std::upper_bound(angles.begin(), angles.begin() + maxInliersIdx, maxAngle) - angles.begin();
+    medianAngle = angles[maxInliersIdx / 2];
+    maxAngle = std::min(medianAngle * 2., maxAngle / 1.8);
+  }
+  this->AzimuthalResolution = medianAngle;
+  std::cout << "LiDAR's azimuthal resolution estimated to " << Utils::Rad2Deg(this->AzimuthalResolution) << "°" << std::endl;
+}
+
+//-----------------------------------------------------------------------------
+void KeypointExtractor::ComputeBlobs()
+{
+  // Init random distribution
+  std::mt19937 gen(2023); // Fix seed for deterministic processes
+  std::uniform_real_distribution<> dis(0.0, 1.0);
+
+  for (const auto& point : *this->Scan)
+  {
+    // Random sampling to decrease keypoints extraction
+    // computation time
+    if (this->InputSamplingRatio < 1.f && dis(gen) > this->InputSamplingRatio)
+      continue;
+
+    this->AddKeypoint(Keypoint::BLOB, point);
+  }
+}
+
+//-----------------------------------------------------------------------------
+bool KeypointExtractor::CheckAzimuthAngle(const Eigen::Vector3f& centralPoint)
+{
+  if (std::abs(std::abs(this->AzimuthMax - this->AzimuthMin) - 2.f * M_PI) < 1e-6)
+    return true;
+
+  if (std::abs(this->AzimuthMax - this->AzimuthMin) > 2.f * M_PI + 1e-6 ||
+      this->AzimuthMin == this->AzimuthMax)
+    return false;
+
+  const float cosAzimuth = centralPoint.x() / std::sqrt(std::pow(centralPoint.x(), 2) + std::pow(centralPoint.y(), 2));
+  const float azimuth = centralPoint.y() > 0 ? std::acos(cosAzimuth) : 2.f * M_PI - std::acos(cosAzimuth);
+
+  if (this->AzimuthMin < this->AzimuthMax &&
+      (azimuth < this->AzimuthMin || azimuth > this->AzimuthMax))
+    return false;
+
+  if (this->AzimuthMin > this->AzimuthMax &&
+      (azimuth < this->AzimuthMin && azimuth > this->AzimuthMax))
+    return false;
+
+  return true;
+}
+
+//-----------------------------------------------------------------------------
+bool KeypointExtractor::IsBeamAngleValid(const Eigen::Vector3f& centralPt,
+                                         float centralDepth,
+                                         const LineFitting& line)
+{
+  const float beamLineAngle = std::abs(line.Direction.dot(centralPt) / centralDepth);
+  return beamLineAngle <= this->MinBeamSurfaceAngle;
+}
+} // End of LidarSlam namespace
\ No newline at end of file
diff --git a/slam_lib/src/SpinningSensorKeypointExtractor.cxx b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
index 75425d308..aabfb324b 100644
--- a/slam_lib/src/SpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
@@ -31,64 +31,6 @@
 namespace LidarSlam
 {
 
-namespace
-{
-//-----------------------------------------------------------------------------
-bool LineFitting::FitLineAndCheckConsistency(const SpinningSensorKeypointExtractor::PointCloud& cloud,
-                                             const std::vector<int>& indices)
-{
-  // Check consistency of the line
-  if (indices.size() < 2)
-    return false;
-
-  float sqDistMax = 0.f;
-  float sqDistMin = std::numeric_limits<float>::max();
-  float sqDistCurr;
-  for (int i = 1; i < indices.size(); ++i)
-  {
-    sqDistCurr = (cloud[indices[i]].getVector3fMap() - cloud[indices[i-1]].getVector3fMap()).squaredNorm();
-    sqDistMax = std::max(sqDistMax, sqDistCurr);
-    sqDistMin = std::min(sqDistMin, sqDistCurr);
-  }
-  if (sqDistMax / sqDistMin > this->SquaredRatio)
-    return false;
-
-  Eigen::Vector3f diffVec = cloud[indices.back()].getVector3fMap() - cloud[indices.front()].getVector3fMap();
-  this->Direction = diffVec.normalized();
-  Eigen::Vector4f centroid;
-  pcl::compute3DCentroid(cloud, indices, centroid);
-  this->Position = centroid.head(3);
-
-  // Check line width
-  float lineLength = diffVec.norm();
-  float widthThreshold = lineLength / this->LengthWidthRatio;
-
-  for (auto idx : indices)
-  {
-    float error = (cloud[idx].getVector3fMap() - this->Position).cross(this->Direction).norm();
-    if (error > widthThreshold)
-      return false;
-  }
-
-  return true;
-}
-
-//-----------------------------------------------------------------------------
-inline float LineFitting::DistanceToPoint(Eigen::Vector3f const& point) const
-{
-  return ((point - this->Position).cross(this->Direction)).norm();
-}
-} // end of anonymous namespace
-
-//-----------------------------------------------------------------------------
-void SpinningSensorKeypointExtractor::Enable(const std::vector<Keypoint>& kptTypes)
-{
-  for (auto& en : this->Enabled)
-    en.second = false;
-  for (auto& k : kptTypes)
-    this->Enabled[k] = true;
-}
-
 //-----------------------------------------------------------------------------
 SpinningSensorKeypointExtractor::PointCloud::Ptr SpinningSensorKeypointExtractor::GetKeypoints(Keypoint k)
 {
@@ -103,31 +45,10 @@ SpinningSensorKeypointExtractor::PointCloud::Ptr SpinningSensorKeypointExtractor
   return keypoints;
 }
 
-
 //-----------------------------------------------------------------------------
-void SpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Ptr& pc)
+void SpinningSensorKeypointExtractor::AddKeypoint(const Keypoint& k, const Point& pt)
 {
-  this->Scan = pc;
-
-  // Split whole pointcloud into separate laser ring clouds
-  this->ConvertAndSortScanLines();
-
-  // Initialize the features vectors and keypoints
-  this->PrepareDataForNextFrame();
-
-  // Compute keypoints scores
-  this->ComputeCurvature();
-
-  // Labelize and extract keypoints
-  // Warning : order matters
-  if (this->Enabled[Keypoint::BLOB])
-    this->ComputeBlobs();
-  if (this->Enabled[Keypoint::PLANE])
-    this->ComputePlanes();
-  if (this->Enabled[Keypoint::EDGE])
-    this->ComputeEdges();
-  if (this->Enabled[Keypoint::INTENSITY_EDGE])
-    this->ComputeIntensityEdges();
+  this->Keypoints[k].AddPoint(pt);
 }
 
 //-----------------------------------------------------------------------------
@@ -161,6 +82,32 @@ void SpinningSensorKeypointExtractor::ConvertAndSortScanLines()
     this->EstimateAzimuthalResolution();
 }
 
+//-----------------------------------------------------------------------------
+void SpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Ptr& pc)
+{
+  this->Scan = pc;
+
+  // Split whole pointcloud into separate laser ring clouds
+  this->ConvertAndSortScanLines();
+
+  // Initialize the features vectors and keypoints
+  this->PrepareDataForNextFrame();
+
+  // Compute keypoints scores
+  this->ComputeCurvature();
+
+  // Labelize and extract keypoints
+  // Warning : order matters
+  if (this->Enabled[Keypoint::BLOB])
+    this->ComputeBlobs();
+  if (this->Enabled[Keypoint::PLANE])
+    this->ComputePlanes();
+  if (this->Enabled[Keypoint::EDGE])
+    this->ComputeEdges();
+  if (this->Enabled[Keypoint::INTENSITY_EDGE])
+    this->ComputeIntensityEdges();
+}
+
 //-----------------------------------------------------------------------------
 void SpinningSensorKeypointExtractor::PrepareDataForNextFrame()
 {
@@ -204,14 +151,6 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
   const float cosMinBeamSurfaceAngle = std::cos(Utils::Deg2Rad(this->MinBeamSurfaceAngle));
   const float cosMaxAzimuth = std::cos(1.5 * this->AzimuthalResolution);
   const float cosSpaceGapAngle = std::cos(this->EdgeNbGapPoints * this->AzimuthalResolution);
-  float azimuthMinRad = Utils::Deg2Rad(this->AzimuthMin);
-  float azimuthMaxRad = Utils::Deg2Rad(this->AzimuthMax);
-
-  // Rescale angles in [0, 2pi]
-  while (azimuthMinRad < 0)
-    azimuthMinRad += 2 * M_PI;
-  while (azimuthMaxRad < 0)
-    azimuthMaxRad += 2 * M_PI;
 
   // Init random distribution
   std::mt19937 gen(2023); // Fix seed for deterministic processes
@@ -240,26 +179,11 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
       const Eigen::Vector3f& centralPoint = scanLineCloud[index].getVector3fMap();
       float centralDepth = centralPoint.norm();
 
-      // Check distance to sensor
-      if (centralDepth < this->MinDistanceToSensor || centralDepth > this->MaxDistanceToSensor)
+      if (!this->CheckDistanceToSensor(centralDepth))
         continue;
 
-      // Check azimuth angle
-      if (std::abs(azimuthMaxRad - azimuthMinRad) < 2 * M_PI - 1e-6)
-      {
-        float cosAzimuth = centralPoint.x() / std::sqrt(std::pow(centralPoint.x(), 2) + std::pow(centralPoint.y(), 2));
-        float azimuth = centralPoint.y() > 0? std::acos(cosAzimuth) : 2*M_PI - std::acos(cosAzimuth);
-        if (azimuthMinRad == azimuthMaxRad)
-          continue;
-        if (azimuthMinRad < azimuthMaxRad &&
-            (azimuth < azimuthMinRad ||
-             azimuth > azimuthMaxRad))
-          continue;
-
-        if (azimuthMinRad > azimuthMaxRad &&
-            (azimuth < azimuthMinRad && azimuth > azimuthMaxRad))
-          continue;
-      }
+      if (!this->CheckAzimuthAngle(centralPoint))
+        continue;
 
       // Fill left and right neighbors
       // Those points must be more numerous than MinNeighNb and occupy more space than MinNeighRadius
@@ -297,11 +221,11 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
       const float rightDepth = rightPt.norm();
       const float leftDepth = leftPt.norm();
 
-      const float cosAngleRight = std::abs(rightPt.dot(centralPoint) / (rightDepth * centralDepth));
-      const float cosAngleLeft = std::abs(leftPt.dot(centralPoint) / (leftDepth * centralDepth));
+      const float cosAngleRight = this->ComputeCosAngle(rightPt, centralPoint, rightDepth, centralDepth);
+      const float cosAngleLeft = this->ComputeCosAngle(leftPt, centralPoint, leftDepth, centralDepth);
 
-      const Eigen::Vector3f diffVecRight = rightPt - centralPoint;
-      const Eigen::Vector3f diffVecLeft = leftPt - centralPoint;
+      const Eigen::Vector3f& diffVecRight = rightPt - centralPoint;
+      const Eigen::Vector3f& diffVecLeft = leftPt - centralPoint;
 
       const float diffRightNorm = diffVecRight.norm();
       const float diffLeftNorm = diffVecLeft.norm();
@@ -368,7 +292,8 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
         this->DepthGap[scanLine][index] = std::max(distLeft, distRight);
       }
 
-      if (cosAngleRight < cosMaxAzimuth || cosAngleLeft < cosMaxAzimuth)
+      if (!this->IsAngleValid(cosAngleRight) ||
+          !this->IsAngleValid(cosAngleLeft))
         continue;
 
       // Fit line on the left and right neighborhoods and
@@ -378,12 +303,8 @@ void SpinningSensorKeypointExtractor::ComputeCurvature()
           !rightLine.FitLineAndCheckConsistency(scanLineCloud, rightNeighbors))
         continue;
 
-      cosBeamLineAngleLeft = std::abs(leftLine.Direction.dot(centralPoint) / centralDepth);
-      if (cosBeamLineAngleLeft > cosMinBeamSurfaceAngle)
-        continue;
-
-      cosBeamLineAngleRight = std::abs(rightLine.Direction.dot(centralPoint) / centralDepth);
-      if (cosBeamLineAngleRight > cosMinBeamSurfaceAngle)
+      if (!this->IsBeamAngleValid(centralPoint, centralDepth, rightLine) ||
+          !this->IsBeamAngleValid(centralPoint, centralDepth, leftLine))
         continue;
 
       if (this->Enabled[INTENSITY_EDGE])
@@ -529,75 +450,6 @@ void SpinningSensorKeypointExtractor::ComputeIntensityEdges()
   this->AddKptsUsingCriterion(Keypoint::INTENSITY_EDGE, this->IntensityGap, this->EdgeIntensityGapThreshold, false);
 }
 
-//-----------------------------------------------------------------------------
-void SpinningSensorKeypointExtractor::ComputeBlobs()
-{
-  // Init random distribution
-  std::mt19937 gen(2023); // Fix seed for deterministic processes
-  std::uniform_real_distribution<> dis(0.0, 1.0);
-
-  for (unsigned int scanLine = 0; scanLine < this->NbLaserRings; ++scanLine)
-  {
-    const PointCloud& scanlineCloud = *this->GetScanlineCloud(scanLine);
-    for (unsigned int index = 0; index < scanlineCloud.size(); ++index)
-    {
-      // Random sampling to decrease keypoints extraction
-      // computation time
-      if (this->InputSamplingRatio < 1.f && dis(gen) > this->InputSamplingRatio)
-        continue;
-      this->Keypoints[Keypoint::BLOB].AddPoint(scanlineCloud[index]);
-    }
-  }
-}
-
-//-----------------------------------------------------------------------------
-void SpinningSensorKeypointExtractor::EstimateAzimuthalResolution()
-{
-  // Compute horizontal angle values between successive points
-  std::vector<float> angles;
-  angles.reserve(this->Scan->size());
-  for (unsigned int scanLineIdx = 0; scanLineIdx < this->NbLaserRings; ++scanLineIdx)
-  {
-    const auto& scanLineCloud = *this->GetScanlineCloud(scanLineIdx);
-    for (unsigned int index = 1; index < scanLineCloud.size(); ++index)
-    {
-      // Compute horizontal angle between two measurements
-      // WARNING: to be correct, the points need to be in the LIDAR sensor
-      // coordinates system, where the sensor is spinning around Z axis.
-      Eigen::Map<const Eigen::Vector2f> p1(scanLineCloud.at(index - 1).data);
-      Eigen::Map<const Eigen::Vector2f> p2(scanLineCloud.at(index).data);
-      float angle = std::abs(std::acos(p1.dot(p2) / (p1.norm() * p2.norm())));
-
-      // Keep only angles greater than 0 to avoid dual return issues
-      if (angle > 1e-4)
-        angles.push_back(angle);
-    }
-  }
-
-  // A minimum number of angles is needed to get a trustable estimator
-  if (angles.size() < 100)
-  {
-    PRINT_WARNING("Not enough points to estimate azimuthal resolution");
-    return;
-  }
-
-  // Estimate azimuthal resolution from these angles
-  std::sort(angles.begin(), angles.end());
-  unsigned int maxInliersIdx = angles.size();
-  float maxAngle = Utils::Deg2Rad(5.);
-  float medianAngle = 0.;
-  // Iterate until only angles between direct LiDAR beam neighbors remain.
-  // The max resolution angle is decreased at each iteration.
-  while (maxAngle > 1.8 * medianAngle)
-  {
-    maxInliersIdx = std::upper_bound(angles.begin(), angles.begin() + maxInliersIdx, maxAngle) - angles.begin();
-    medianAngle = angles[maxInliersIdx / 2];
-    maxAngle = std::min(medianAngle * 2., maxAngle / 1.8);
-  }
-  this->AzimuthalResolution = medianAngle;
-  std::cout << "LiDAR's azimuthal resolution estimated to " << Utils::Rad2Deg(this->AzimuthalResolution) << "°" << std::endl;
-}
-
 //-----------------------------------------------------------------------------
 std::unordered_map<std::string, std::vector<float>> SpinningSensorKeypointExtractor::GetDebugArray() const
 {
-- 
GitLab


From 60a2bc36a3b988dabece840d41939fdcc4710718 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Tue, 24 Oct 2023 17:58:20 +0200
Subject: [PATCH 06/13] [feat] Create Vertex Map

For now, the vertex map is a map of pointers to Point Features (PtFeat), whose index and depth only are initialized
---
 .../DenseSpinningSensorKeypointExtractor.h    | 48 +++++++++-
 slam_lib/include/LidarSlam/Utilities.h        | 16 ++++
 .../DenseSpinningSensorKeypointExtractor.cxx  | 87 ++++++++++++++++++-
 3 files changed, 148 insertions(+), 3 deletions(-)

diff --git a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
index f7874b406..240c38fac 100644
--- a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
@@ -29,6 +29,26 @@
 
 namespace LidarSlam
 {
+
+struct PtFeat
+{
+  int Index;
+  float Depth;
+  float SpaceGap;
+  float DepthGap;
+  float IntensityGap;
+  float Angle;
+  std::bitset<Keypoint::nKeypointTypes> KptTypes;
+
+  PtFeat() : Index(0), Depth(0.0f), SpaceGap(-1.0f), DepthGap(-1.0f), IntensityGap(-1.0f), Angle(-1.0f), KptTypes({}) {}
+};
+
+struct IdxVM
+{
+  int Row;
+  int Col;
+};
+
 class DenseSpinningSensorKeypointExtractor : public KeypointExtractor
 {
 public:
@@ -45,6 +65,20 @@ public:
   std::unordered_map<std::string, std::vector<float>> GetDebugArray() const override;
 
 private:
+
+  // Find the pointer to PtFeat of a point in the scan (designated by its index in the scan)
+  // (PtFeat containing the features of the point : space gap, depth...)
+  std::shared_ptr<PtFeat> GetPtFeat(int idxInScan) const;
+
+  // Count the number of non null ptr in a scanline
+  int GetScanLineSize(const std::vector<std::shared_ptr<PtFeat>>& scanLine);
+
+  // Initialize LaserIdMap, NbLaserRings, AzimuthalResolution and Pc2VmIndices
+  void InitInternalParameters();
+
+  // Create vertex map from input pointcloud using indices stored in Pc2VmIndices
+  void CreateVertexMap();
+
   // Compute the curvature features within each scan line : depth
   // space gap, intensity gap and line angle
   void ComputeCurvature() override;
@@ -62,7 +96,7 @@ private:
   // Add all keypoints of the type k that comply with the threshold criteria for these values
   // The threshold can be a minimum or maximum value (threshIsMax)
   // The weight basis allow to weight the keypoint depending on its certainty
-  void AddKptsUsingCriterion (Keypoint k);
+  void AddKptsUsingCriterion(Keypoint k);
 
   // ---------------------------------------------------------------------------
   //   Parameters
@@ -74,8 +108,18 @@ private:
   //   Internal variables
   // ---------------------------------------------------------------------------
 
-  //TODO
+  // Dimensions of the Vertex Map
+  int HeightVM;
+  int WidthVM;
+
+  // Map of laser_id to fit random laser_ids into {0, ..., NbLaserRings-1}
+  std::unordered_map<int, int> LaserIdMap;
+
+  // Vector linking the index of a point in the pointcloud to its index in the Vertex Map
+  std::vector<IdxVM> Pc2VmIndices;
 
+  // Vertex Map of points' indices in the pointcloud
+  std::vector<std::vector<std::shared_ptr<PtFeat>>> VertexMap;
 };
 
 } // namespace LidarSlam
\ No newline at end of file
diff --git a/slam_lib/include/LidarSlam/Utilities.h b/slam_lib/include/LidarSlam/Utilities.h
index e4dec8f09..ff74818bb 100644
--- a/slam_lib/include/LidarSlam/Utilities.h
+++ b/slam_lib/include/LidarSlam/Utilities.h
@@ -368,6 +368,22 @@ void ComputeMeanAndPCA(const pcl::PointCloud<PointT>& cloud,
   pcl::eigen33(covarianceMatrix, eigenVectors, eigenValues);
 }
 
+//----------------------------------------------------------------------------
+/*!
+ * @brief Deduce the rotation sense of the lidar
+ * @return true if the LiDAR rotates clockwise, false otherwise.
+ * @param cloud PointCloud published by lidar driver
+ * @param nbLasers Number of lasers of the lidar
+ */
+template<typename PointT>
+inline bool IsRotationClockwise(const pcl::PointCloud<PointT>& cloud, unsigned int nbLasers)
+{
+  Eigen::Vector2d firstPoint({cloud.front().x, cloud.front().y});
+  Eigen::Vector2d secondPoint({cloud[nbLasers].x, cloud[nbLasers].y});
+  double crossZ = firstPoint.x() * secondPoint.y() - firstPoint.y() * secondPoint.x();
+  return crossZ > 0;
+}
+
 //==============================================================================
 //   Covariance helpers
 //==========================================================================
diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
index 86cf98d99..1f8cd7958 100644
--- a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
@@ -41,10 +41,95 @@ void DenseSpinningSensorKeypointExtractor::AddKeypoint(const Keypoint& k, const
   //TODO
 }
 
+//-----------------------------------------------------------------------------
+std::shared_ptr<PtFeat> DenseSpinningSensorKeypointExtractor::GetPtFeat(int idxInScan) const
+{
+  auto& indices = this->Pc2VmIndices[idxInScan];
+  return this->VertexMap[indices.Row][indices.Col];
+}
+
+//-----------------------------------------------------------------------------
+int DenseSpinningSensorKeypointExtractor::GetScanLineSize(const std::vector<std::shared_ptr<PtFeat>>& scanLine)
+{
+  int nbPoints;
+  for (auto& point : scanLine)
+  {
+    if (point != nullptr)
+      nbPoints++;
+  }
+  return nbPoints;
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::InitInternalParameters()
+{
+  // Map laser_ids
+  for (const auto& point : *this->Scan)
+  {
+    if (this->LaserIdMap.count(point.laser_id) == 0)
+      this->LaserIdMap[point.laser_id] = this->LaserIdMap.size();
+  }
+
+  // Save the number of lasers
+  this->NbLaserRings = this->LaserIdMap.size();
+
+  bool rotationIsClockwise = Utils::IsRotationClockwise<Point>(*this->Scan, this->NbLaserRings);
+
+  // Estimate azimuthal resolution if not already done
+  // or if the previous value found is not plausible
+  // (because last scan was badly formed, e.g. lack of points)
+  if (this->AzimuthalResolution < 1e-6 || M_PI/4. < this->AzimuthalResolution)
+    this->EstimateAzimuthalResolution();
+
+  // Compute the indices of scan points in the future vertex map
+  this->Pc2VmIndices.clear(); // Clear previous indices
+  this->Pc2VmIndices.reserve(this->Scan->size());
+  Eigen::Vector3f firstPt(this->Scan->at(0).data);
+  for (const auto& point : *this->Scan)
+  {
+    double cross = firstPt.x() * point.y - firstPt.y() * point.x;
+    double angle = std::atan2(cross, firstPt.head(2).dot(point.getVector3fMap().head(2)));
+    float azimuth = rotationIsClockwise ? M_PI + angle : M_PI - angle;
+    this->Pc2VmIndices.emplace_back(IdxVM{this->LaserIdMap.find(point.laser_id)->second,
+                                          static_cast<int>(azimuth / this->AzimuthalResolution)});
+  }
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::CreateVertexMap()
+{
+  // Clear previous Vertex Map
+  for (auto& row : this->VertexMap)
+  {
+    for (auto& ptr : row)
+      ptr.reset(); // ptr = nullptr
+  }
+
+  this->WidthVM = 2.f * M_PI / this->AzimuthalResolution + 1;
+  this->HeightVM = this->NbLaserRings;
+
+  this->VertexMap.resize(this->HeightVM, std::vector<std::shared_ptr<PtFeat>>(this->WidthVM));
+  // Fill Vertex Map with index and depth values
+  #pragma omp parallel for num_threads(this->NbThreads) schedule(guided)
+  for (int i = 0; i < this->Scan->size(); ++i)
+  {
+    const Point& point = this->Scan->at(i);
+    auto& indices = this->Pc2VmIndices[i];
+
+    this->VertexMap[indices.Row][indices.Col] = std::make_shared<PtFeat>();
+    this->VertexMap[indices.Row][indices.Col]->Index = i;
+    this->VertexMap[indices.Row][indices.Col]->Depth = point.getVector3fMap().norm();
+  }
+}
+
 //-----------------------------------------------------------------------------
 void DenseSpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Ptr& pc)
 {
-  //TODO
+  this->Scan = pc;
+
+  this->InitInternalParameters();
+
+  this->CreateVertexMap();
 }
 
 //-----------------------------------------------------------------------------
-- 
GitLab


From f101e45448c6cd5af97c98b6f865805bdd9b6fab Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Wed, 25 Oct 2023 16:47:12 +0200
Subject: [PATCH 07/13] [debug] Output features as pgm images

Create pgm images in shades of grey for each feature
For now, only index and depth can be seen, but in the future
we will be able to display every point feature (depth gap, angle, space gap)
---
 .../DenseSpinningSensorKeypointExtractor.h    |  3 ++
 .../DenseSpinningSensorKeypointExtractor.cxx  | 42 +++++++++++++++++++
 2 files changed, 45 insertions(+)

diff --git a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
index 240c38fac..51ecb0078 100644
--- a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
@@ -79,6 +79,9 @@ private:
   // Create vertex map from input pointcloud using indices stored in Pc2VmIndices
   void CreateVertexMap();
 
+  // Output separate point features contained in Vertex Map in pgm format to visualize as 2D image
+  void OutputFeatures(std::string path);
+
   // Compute the curvature features within each scan line : depth
   // space gap, intensity gap and line angle
   void ComputeCurvature() override;
diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
index 1f8cd7958..df30cb5af 100644
--- a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
@@ -26,6 +26,8 @@
 
 #include <random>
 
+#include <fstream>
+
 namespace LidarSlam
 {
 
@@ -122,6 +124,46 @@ void DenseSpinningSensorKeypointExtractor::CreateVertexMap()
   }
 }
 
+//-----------------------------------------------------------------------------
+#define OUTPUT_FEATURE(filename, featName, maxValue, specialValue, minValue)                                      \
+{                                                                                                                 \
+  int maxPix = 255;                                                                                               \
+  int specialPix = 30;                                                                                            \
+  std::ofstream file(filename);                                                                                   \
+  file << "P2\n";                                                                                                 \
+  file << this->WidthVM << " " << this->HeightVM << "\n";                                                         \
+  file << maxPix << "\n";                                                                                         \
+  for (int i = 0; i < this->HeightVM; ++i)                                                                        \
+  {                                                                                                               \
+    for (int j = 0; j < this->WidthVM; ++j)                                                                       \
+    {                                                                                                             \
+      if (this->VertexMap[i][j] == nullptr)                                                                       \
+        file << 0 << " ";                                                                                         \
+      else                                                                                                        \
+      {                                                                                                           \
+        float value = this->VertexMap[i][j]->featName;                                                            \
+        if (std::abs(value - specialValue) < 1e-6)                                                                \
+          file << specialPix << " ";                                                                              \
+        else                                                                                                      \
+        {                                                                                                         \
+          value = std::min(static_cast<int>(value), static_cast<int>(maxValue));                                  \
+          value = std::max(static_cast<int>(value), static_cast<int>(minValue));                                  \
+          file << int((value - maxValue) * (maxPix - (specialPix + 1)) / (maxValue - minValue)) + maxPix << " ";  \
+        }                                                                                                         \
+      }                                                                                                           \
+    }                                                                                                             \
+    file << "\n";                                                                                                 \
+  }                                                                                                               \
+  file.close();                                                                                                   \
+}                                                                                                                 \
+
+void DenseSpinningSensorKeypointExtractor::OutputFeatures(std::string path)
+{
+  int totalSize = this->WidthVM * this->HeightVM;
+  OUTPUT_FEATURE(path + "Index.pgm", Index, totalSize, 0., 0.);
+  OUTPUT_FEATURE(path + "Depth.pgm", Depth, 20., 0., 0.);
+}
+
 //-----------------------------------------------------------------------------
 void DenseSpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Ptr& pc)
 {
-- 
GitLab


From cc406332e5051c047b553032aca6a891ed089106 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Wed, 6 Dec 2023 15:58:25 +0100
Subject: [PATCH 08/13] [feat] Extract features from horizontal neighborhood

For now, only horizontal neighborhood is extracted.
The features extracted lookalike SSKE features, except their technic of extraction is different, using the pros of Vertex Map structure. So the space gap and the depth gap are computed differently but applying the same theory.
The feature of angle is now the cosine of the angle instead of the absolute sine. We then decided to create a threshold that could be used for cosine in DSSKE and sine in SSKE : the angle threshold (one for planes and one for edges). See desc MR !324 for further explanation.
---
 .../lidar_slam/params/slam_config_indoor.yaml |  27 +-
 .../params/slam_config_outdoor.yaml           |  27 +-
 ros_wrapping/lidar_slam/src/LidarSlamNode.cxx |   4 +-
 .../DenseSpinningSensorKeypointExtractor.h    |  23 +-
 .../include/LidarSlam/KeypointExtractor.h     |  19 +-
 .../SpinningSensorKeypointExtractor.h         |  13 +
 .../DenseSpinningSensorKeypointExtractor.cxx  | 236 +++++++++++++++++-
 7 files changed, 320 insertions(+), 29 deletions(-)

diff --git a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
index 8b8df31e2..185ab1224 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
@@ -390,8 +390,13 @@ slam:
     min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
     neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
-    plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
-    edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
+    plane_angle_threshold: 150.        # [°] Threshold upon angle between left and right lines to consider a point as plane.
+                                       # It is used with its absolute sine value in SSKE for planes,
+                                       # with its cosine value in DSSKE for planes and
+                                       # with its opposite cosine value in DSSKE to filter too sharp edges.
+    edge_angle_threshold: 120.         # [°] Threshold upon angle between left and right lines to consider a point as edge.
+                                       # It is used with its absolute sine value in SSKE for edges and
+                                       # with its cosine value in DSSKE for edges.
     edge_depth_gap_threshold: 0.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     edge_intensity_gap_threshold: 20.  # [0-255] Threshold upon intensity gap to select an edge keypoint.
@@ -424,8 +429,13 @@ slam:
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
     #   neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     #   neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
-    #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
-    #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
+    #   plane_angle_threshold: 150.        # [°] Threshold upon angle between left and right lines to consider a point as plane.
+    #                                      # It is used with its absolute sine value in SSKE for planes,
+    #                                      # with its cosine value in DSSKE for planes and
+    #                                      # with its opposite cosine value in DSSKE to filter too sharp edges.
+    #   edge_angle_threshold: 120.         # [°] Threshold upon angle between left and right lines to consider a point as edge.
+    #                                      # It is used with its absolute sine value in SSKE for edges and
+    #                                      # with its cosine value in DSSKE for edges.
     #   edge_depth_gap_threshold: 0.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     #   edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     #   edge_intensity_gap_threshold: 20.  # [0-255] Threshold upon intensity gap to select an edge keypoint.
@@ -450,8 +460,13 @@ slam:
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
     #   neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     #   neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
-    #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
-    #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
+    #   plane_angle_threshold: 150.        # [°] Threshold upon angle between left and right lines to consider a point as plane.
+    #                                      # It is used with its absolute sine value in SSKE for planes,
+    #                                      # with its cosine value in DSSKE for planes and
+    #                                      # with its opposite cosine value in DSSKE to filter too sharp edges.
+    #   edge_angle_threshold: 120.         # [°] Threshold upon angle between left and right lines to consider a point as edge.
+    #                                      # It is used with its absolute sine value in SSKE for edges and
+    #                                      # with its cosine value in DSSKE for edges.
     #   edge_depth_gap_threshold: 0.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     #   edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     #   edge_intensity_gap_threshold: 200. # [0-255] Threshold upon intensity gap to select an edge keypoint.
diff --git a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
index ff0992b78..9508a684b 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
@@ -391,8 +391,13 @@ slam:
     min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
     neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
-    plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
-    edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
+    plane_angle_threshold: 150.        # [°] Threshold upon angle between left and right lines to consider a point as plane.
+                                       # It is used with its absolute sine value in SSKE for planes,
+                                       # with its cosine value in DSSKE for planes and
+                                       # with its opposite cosine value in DSSKE to filter too sharp edges.
+    edge_angle_threshold: 120.         # [°] Threshold upon angle between left and right lines to consider a point as edge.
+                                       # It is used with its absolute sine value in SSKE for edges and
+                                       # with its cosine value in DSSKE for edges.
     edge_depth_gap_threshold: 1.       # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     edge_intensity_gap_threshold: 50.  # [0-255] Threshold upon intensity gap to select an edge keypoint.
@@ -425,8 +430,13 @@ slam:
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
     #   neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     #   neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
-    #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
-    #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
+    #   plane_angle_threshold: 150.        # [°] Threshold upon angle between left and right lines to consider a point as plane.
+    #                                      # It is used with its absolute sine value in SSKE for planes,
+    #                                      # with its cosine value in DSSKE for planes and
+    #                                      # with its opposite cosine value in DSSKE to filter too sharp edges.
+    #   edge_angle_threshold: 120.         # [°] Threshold upon angle between left and right lines to consider a point as edge.
+    #                                      # It is used with its absolute sine value in SSKE for edges and
+    #                                      # with its cosine value in DSSKE for edges.
     #   edge_depth_gap_threshold: 1.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     #   edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     #   edge_intensity_gap_threshold: 30.  # [0-255] Threshold upon intensity gap to select an edge keypoint.
@@ -451,8 +461,13 @@ slam:
     #   min_beam_surface_angle: 10.        # [°] Minimal angle between the point surface and the laser beam to consider a point as valid.
     #   neighbors_side_nb: 5               # [>1] Minimum number of neighbors used on each side to estimate curvature and detect edges and planes.
     #   neighbors_radius: 0.10             # [m] Minimum radius to define a neighborhood on each side of the studied point.
-    #   plane_sin_angle_threshold: 0.5     # [0-1] Sharpness threshold to select a planar keypoint (selected if sin angle is less than threshold).
-    #   edge_sin_angle_threshold: 0.86     # [0-1] Sharpness threshold to select an edge keypoint (selected if sin angle is more than threshold).
+    #   plane_angle_threshold: 150.        # [°] Threshold upon angle between left and right lines to consider a point as plane.
+    #                                      # It is used with its absolute sine value in SSKE for planes,
+    #                                      # with its cosine value in DSSKE for planes and
+    #                                      # with its opposite cosine value in DSSKE to filter too sharp edges.
+    #   edge_angle_threshold: 120.         # [°] Threshold upon angle between left and right lines to consider a point as edge.
+    #                                      # It is used with its absolute sine value in SSKE for edges and
+    #                                      # with its cosine value in DSSKE for edges.
     #   edge_depth_gap_threshold: 1.5      # [m] Threshold upon depth gap in neighborhood to select an edge keypoint.
     #   edge_nb_gap_points: 10             # [points] Minimum number of successive missing points to consider a space gap.
     #   edge_intensity_gap_threshold: 200. # [0-255] Threshold upon intensity gap to select an edge keypoint.
diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
index 1444b2c3a..c48d46569 100644
--- a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
+++ b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
@@ -1760,8 +1760,8 @@ void LidarSlamNode::SetSlamParameters()
     SetKeypointsExtractorParam(float, prefix + "min_beam_surface_angle", MinBeamSurfaceAngle)
     SetKeypointsExtractorParam(float, prefix + "min_azimuth", AzimuthMin)
     SetKeypointsExtractorParam(float, prefix + "max_azimuth", AzimuthMax)
-    SetKeypointsExtractorParam(float, prefix + "plane_sin_angle_threshold", PlaneSinAngleThreshold)
-    SetKeypointsExtractorParam(float, prefix + "edge_sin_angle_threshold", EdgeSinAngleThreshold)
+    SetKeypointsExtractorParam(float, prefix + "edge_angle_threshold", EdgeAngleThreshold)
+    SetKeypointsExtractorParam(float, prefix + "plane_angle_threshold", PlaneAngleThreshold)
     SetKeypointsExtractorParam(float, prefix + "edge_depth_gap_threshold", EdgeDepthGapThreshold)
     SetKeypointsExtractorParam(float, prefix + "edge_nb_gap_points", EdgeNbGapPoints)
     SetKeypointsExtractorParam(float, prefix + "edge_intensity_gap_threshold", EdgeIntensityGapThreshold)
diff --git a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
index 51ecb0078..1af5f270c 100644
--- a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
@@ -40,7 +40,7 @@ struct PtFeat
   float Angle;
   std::bitset<Keypoint::nKeypointTypes> KptTypes;
 
-  PtFeat() : Index(0), Depth(0.0f), SpaceGap(-1.0f), DepthGap(-1.0f), IntensityGap(-1.0f), Angle(-1.0f), KptTypes({}) {}
+  PtFeat() : Index(0), Depth(0.0f), SpaceGap(-1.0f), DepthGap(0.0f), IntensityGap(-1.0f), Angle(1.0f), KptTypes({}) {}
 };
 
 struct IdxVM
@@ -52,6 +52,20 @@ struct IdxVM
 class DenseSpinningSensorKeypointExtractor : public KeypointExtractor
 {
 public:
+
+  GetMacro(PlaneCosAngleThreshold, float)
+  SetMacro(PlaneCosAngleThreshold, float)
+
+  GetMacro(EdgeCosAngleThreshold, float)
+  SetMacro(EdgeCosAngleThreshold, float)
+
+  // Set EdgeCosAngleThreshold and PlaneCosAngleThreshold from angle in degrees
+  void SetEdgeAngleThreshold(float angle) override {this->EdgeCosAngleThreshold = std::cos(Utils::Deg2Rad(angle));};
+  void SetPlaneAngleThreshold(float angle) override {this->PlaneCosAngleThreshold = std::cos(Utils::Deg2Rad(angle));};
+  // Associated getters
+  float GetEdgeAngleThreshold() const override {return this->EdgeCosAngleThreshold;};
+  float GetPlaneAngleThreshold() const override {return this->PlaneCosAngleThreshold;};
+
   // Extract keypoints from the pointcloud. The key points
   // will be separated in two classes : Edges keypoints which
   // correspond to area with high curvature scan lines and
@@ -105,7 +119,12 @@ private:
   //   Parameters
   // ---------------------------------------------------------------------------
 
-  //TODO
+  // Sharpness threshold to select a planar keypoint
+  // Also used, with its opposite value, to filter too sharp edges
+  float PlaneCosAngleThreshold = -0.86;  // ~cos(150°) (selected if cos angle is less than threshold)
+
+  // Sharpness threshold to select an edge keypoint
+  float EdgeCosAngleThreshold = -0.5; // ~cos(120°) (selected, if cos angle is more than threshold)
 
   // ---------------------------------------------------------------------------
   //   Internal variables
diff --git a/slam_lib/include/LidarSlam/KeypointExtractor.h b/slam_lib/include/LidarSlam/KeypointExtractor.h
index 84f767dbc..8b664aec1 100644
--- a/slam_lib/include/LidarSlam/KeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/KeypointExtractor.h
@@ -87,12 +87,6 @@ public:
   GetMacro(MaxDistanceToSensor, float)
   SetMacro(MaxDistanceToSensor, float)
 
-  GetMacro(PlaneSinAngleThreshold, float)
-  SetMacro(PlaneSinAngleThreshold, float)
-
-  GetMacro(EdgeSinAngleThreshold, float)
-  SetMacro(EdgeSinAngleThreshold, float)
-
   GetMacro(EdgeDepthGapThreshold, float)
   SetMacro(EdgeDepthGapThreshold, float)
 
@@ -139,6 +133,13 @@ public:
   };
   GetMacro(MinBeamSurfaceAngle, float)
 
+  // Methods to set sin or cos threshold, depending on the extractor
+  virtual void SetEdgeAngleThreshold(float angle) = 0;
+  virtual void SetPlaneAngleThreshold(float angle) = 0;
+  // Associated getters
+  virtual float GetEdgeAngleThreshold() const = 0;
+  virtual float GetPlaneAngleThreshold() const = 0;
+
   // Select the keypoint types to extract
   // This function resets the member map "Enabled"
   void Enable(const std::vector<Keypoint>& kptTypes);
@@ -233,12 +234,6 @@ protected:
   float AzimuthMin = 0.f;        // [rad]
   float AzimuthMax = 2.f * M_PI; // [rad]
 
-  // Sharpness threshold to select a planar keypoint
-  float PlaneSinAngleThreshold = 0.5;  // sin(30°) (selected if sin angle is less than threshold)
-
-  // Sharpness threshold to select an edge keypoint
-  float EdgeSinAngleThreshold = 0.86;  // ~sin(60°) (selected, if sin angle is more than threshold)
-
   // Threshold upon depth gap in neighborhood to select an edge keypoint
   float EdgeDepthGapThreshold = 0.5;  // [m]
 
diff --git a/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
index 4c63503cd..a446fcf99 100644
--- a/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
@@ -43,6 +43,13 @@ public:
   GetMacro(VoxelResolution, float)
   SetMacro(VoxelResolution, float)
 
+  // Set EdgeSinAngleThreshold and PlaneSinAngleThreshold from angle in degrees
+  void SetEdgeAngleThreshold(float angle) override {this->EdgeSinAngleThreshold = std::abs(std::sin(Utils::Deg2Rad(angle)));};
+  void SetPlaneAngleThreshold(float angle) override {this->PlaneSinAngleThreshold = std::abs(std::sin(Utils::Deg2Rad(angle)));};
+  // Associated getters
+  float GetEdgeAngleThreshold() const override {return this->EdgeSinAngleThreshold;};
+  float GetPlaneAngleThreshold() const override {return this->PlaneSinAngleThreshold;};
+
   // Extract keypoints from the pointcloud. The key points
   // will be separated in two classes : Edges keypoints which
   // correspond to area with high curvature scan lines and
@@ -101,6 +108,12 @@ private:
   // It corresponds approx to the mean distance between closest neighbors in the output keypoints cloud.
   float VoxelResolution = 0.1; // [m]
 
+  // Sharpness threshold to select a planar keypoint
+  float PlaneSinAngleThreshold = 0.5;  // sin(30°) (selected if sin angle is less than threshold)
+
+  // Sharpness threshold to select an edge keypoint
+  float EdgeSinAngleThreshold = 0.86; // ~sin(60°) (selected, if sin angle is more than threshold)
+
   // ---------------------------------------------------------------------------
   //   Internal variables
   // ---------------------------------------------------------------------------
diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
index df30cb5af..4b5f76105 100644
--- a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
@@ -162,6 +162,10 @@ void DenseSpinningSensorKeypointExtractor::OutputFeatures(std::string path)
   int totalSize = this->WidthVM * this->HeightVM;
   OUTPUT_FEATURE(path + "Index.pgm", Index, totalSize, 0., 0.);
   OUTPUT_FEATURE(path + "Depth.pgm", Depth, 20., 0., 0.);
+  OUTPUT_FEATURE(path + "SpaceGap.pgm", SpaceGap, 1., -1., 0.);
+  OUTPUT_FEATURE(path + "DepthGap.pgm", DepthGap, 10., 0., -15.);
+  OUTPUT_FEATURE(path + "IntensityGap.pgm", IntensityGap, 35., -1., 0.);
+  OUTPUT_FEATURE(path + "Angles.pgm", Angle, 1., 1., -1.);
 }
 
 //-----------------------------------------------------------------------------
@@ -172,12 +176,242 @@ void DenseSpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Pt
   this->InitInternalParameters();
 
   this->CreateVertexMap();
+
+  this->ComputeCurvature();
 }
 
 //-----------------------------------------------------------------------------
 void DenseSpinningSensorKeypointExtractor::ComputeCurvature()
 {
-  //TODO
+  // Init random distribution
+  std::mt19937 gen(2023); // Fix seed for deterministic processes
+  std::uniform_real_distribution<> dis(0.0, 1.0);
+
+  #pragma omp parallel for num_threads(this->NbThreads) schedule(guided)
+  for (int i = 0; i < this->HeightVM; ++i)
+  {
+    // If the line is almost empty, skip it
+    int nPoints = this->GetScanLineSize(this->VertexMap[i]);
+    if (this->IsScanLineAlmostEmpty(nPoints))
+      continue;
+
+    // Useful index to skip first and last points of the scan line
+    int idxInLine = 0;
+
+    for (unsigned int j = 0; j < this->WidthVM; ++j)
+    {
+      // PtFeat struct associated with current point
+      const auto& currentFeat = this->VertexMap[i][j];
+
+      // Ignore empty pixels
+      if (!currentFeat)
+        continue;
+
+      // Count every valid point in the scan line, to be compared with nPoints later
+      idxInLine++;
+
+      // Random sampling to decrease keypoints extraction
+      // computation time
+      if (this->InputSamplingRatio < 1.f && dis(gen) > this->InputSamplingRatio)
+        continue;
+
+      // Central point
+      const Point& currentPoint = this->Scan->at(currentFeat->Index);
+      const Eigen::Vector3f& centralPoint = currentPoint.getVector3fMap();
+
+      if (!this->CheckDistanceToSensor(currentFeat->Depth))
+        continue;
+
+      if (!this->CheckAzimuthAngle(centralPoint))
+        continue;
+
+      // Fill neighbors (vectors of indices) for each side (left and right)
+      // Those points must be more numerous than MinNeighNb and occupy more space than MinNeighRadius
+      auto getNeighbors = [&](bool right, std::vector<int>& neighbors)
+      {
+        neighbors.reserve(nPoints);
+        neighbors.emplace_back(currentFeat->Index);
+        int rightOrLeft = right ? 1 : -1;
+        int idxNeigh = 1;
+        float lineLength = 0.f;
+        while ((lineLength < this->MinNeighRadius ||
+               int(neighbors.size()) < this->MinNeighNb) &&
+               int(neighbors.size()) < nPoints)
+        {
+          const auto& ptrFeat = this->VertexMap[i][(j + rightOrLeft * idxNeigh + this->WidthVM) % this->WidthVM];
+          if (ptrFeat != nullptr)
+          {
+            neighbors.emplace_back(ptrFeat->Index);
+            if (lineLength < MinNeighRadius)
+              lineLength = (this->Scan->at(neighbors.back()).getVector3fMap() - this->Scan->at(neighbors.front()).getVector3fMap()).norm();
+          }
+          ++idxNeigh;
+        }
+        neighbors.shrink_to_fit();
+      };
+
+      std::vector<int> leftNeighbors, rightNeighbors;
+      getNeighbors(false, leftNeighbors);
+      getNeighbors(true, rightNeighbors);
+
+      if (this->Enabled[EDGE])
+      {
+        // ---- Compute horizontal space gap ----
+
+        // Find the first empty neighbor on the right and on the left
+        auto& idxRightNeigh = this->Pc2VmIndices[rightNeighbors[1]];
+        int nbEmptyRightNeigh = idxRightNeigh.Col - j;
+
+        auto& idxLeftNeigh = this->Pc2VmIndices[leftNeighbors[1]];
+        int nbEmptyLeftNeigh = j - idxLeftNeigh.Col;
+
+        const float cosMinBeamSurfaceAngle = std::cos(Utils::Deg2Rad(this->MinBeamSurfaceAngle));
+
+        float distRight = -1.f;
+        float distLeft = -1.f;
+
+        if (nbEmptyRightNeigh >= this->EdgeNbGapPoints)
+        {
+          const auto& rightPt = this->Scan->at(rightNeighbors[1]).getVector3fMap();
+          float diffRightNorm = (rightPt - centralPoint).norm();
+          float cosBeamLineAngleRight = std::abs((rightPt - centralPoint).dot(centralPoint) / (diffRightNorm * currentFeat->Depth));
+          if (cosBeamLineAngleRight < cosMinBeamSurfaceAngle)
+            distRight = diffRightNorm;
+        }
+        if (nbEmptyLeftNeigh >= this->EdgeNbGapPoints)
+        {
+          const auto& leftPt = this->Scan->at(leftNeighbors[1]).getVector3fMap();
+          float diffLeftNorm = (leftPt - centralPoint).norm();
+          float cosBeamLineAngleLeft = std::abs((leftPt - centralPoint).dot(centralPoint) / (diffLeftNorm * currentFeat->Depth));
+          if (cosBeamLineAngleLeft < cosMinBeamSurfaceAngle)
+            distLeft = diffLeftNorm;
+        }
+        currentFeat->SpaceGap = std::max(distLeft, distRight);
+      }
+
+      // Stop search for first and last points of the scan line
+      // because the discontinuity may alter the other criteria detection
+      if (idxInLine < int(leftNeighbors.size()) || idxInLine >= nPoints - int(rightNeighbors.size()))
+        continue;
+
+      if (this->Enabled[EDGE])
+      {
+        // ---- Compute horizontal depth gap ----
+
+        int idxNext = this->VertexMap[i][j + 1] ? j + 1 : this->VertexMap[i][j + 2] ? j + 2 : -1;
+        int idxPrev = this->VertexMap[i][j - 1] ? j - 1 : this->VertexMap[i][j - 2] ? j - 2 : -1;
+        if (idxNext > 0 && idxPrev > 0)
+        {
+          auto& directRightNeigh = this->VertexMap[i][idxNext];
+          float distRight = directRightNeigh->Depth - currentFeat->Depth;
+          auto& directLeftNeigh = this->VertexMap[i][idxPrev];
+          float distLeft = directLeftNeigh->Depth - currentFeat->Depth;
+
+          auto& postRightNeigh = this->VertexMap[i][idxNext + 1];
+          auto& preLeftNeigh = this->VertexMap[i][idxPrev - 1];
+          if (postRightNeigh != nullptr)
+          {
+            float distPostRight = postRightNeigh->Depth - directRightNeigh->Depth;
+            if (distRight < distPostRight)
+              distRight = 0.0f;
+          }
+          if (preLeftNeigh != nullptr)
+          {
+            float distPreLeft = preLeftNeigh->Depth - directLeftNeigh->Depth;
+            if (distLeft < distPreLeft)
+              distLeft = 0.0f;
+          }
+          currentFeat->DepthGap = std::abs(distLeft) > std::abs(distRight) ? distLeft : distRight;
+        }
+      }
+
+      if (currentFeat->SpaceGap > this->EdgeDepthGapThreshold || currentFeat->DepthGap > this->EdgeDepthGapThreshold)
+        continue;
+
+      // ---- Compute intensity gap ----
+
+      LineFitting leftLine, rightLine;
+      // Fit line on the left and right neighborhoods and
+      // skip point if they are not usable
+      if (!leftLine.FitLineAndCheckConsistency(*this->Scan, leftNeighbors) ||
+          !rightLine.FitLineAndCheckConsistency(*this->Scan, rightNeighbors))
+        continue;
+
+      if (!this->IsBeamAngleValid(centralPoint, currentFeat->Depth, rightLine) ||
+          !this->IsBeamAngleValid(centralPoint, currentFeat->Depth, leftLine))
+        continue;
+
+      if (this->Enabled[INTENSITY_EDGE])
+      {
+        if (std::abs(this->Scan->at(rightNeighbors[1]).intensity - this->Scan->at(leftNeighbors[1]).intensity) > this->EdgeIntensityGapThreshold)
+        {
+          // Compute mean intensity on the left
+          // We sample neighborhoods for computation time concerns
+          float meanIntensityLeft = 0.f;
+          int step = leftNeighbors.size() > this->MinNeighNb ? leftNeighbors.size() / this->MinNeighNb : 1;
+          int cptMean = 0;
+          // The first element of the neighborhood is the central point itself so we skip it
+          for (int i = 1; i < leftNeighbors.size(); i += step)
+          {
+            meanIntensityLeft += this->Scan->at(leftNeighbors[i]).intensity;
+            cptMean++;
+          }
+          meanIntensityLeft /= cptMean;
+          // Compute mean intensity on the right
+          float meanIntensityRight = 0.f;
+          step = rightNeighbors.size() > this->MinNeighNb ? rightNeighbors.size() / this->MinNeighNb : 1;
+          cptMean = 0;
+          for (int i = 1; i < rightNeighbors.size(); i += step)
+          {
+            meanIntensityRight += this->Scan->at(rightNeighbors[i]).intensity;
+            cptMean++;
+          }
+          meanIntensityRight /= cptMean;
+          currentFeat->IntensityGap = std::abs(meanIntensityLeft - meanIntensityRight);
+
+          // Remove neighbor points to get the best intensity discontinuity locally
+          auto neighPtr = this->GetPtFeat(leftNeighbors[1]);
+          if (neighPtr->IntensityGap < currentFeat->IntensityGap)
+            neighPtr->IntensityGap = -1;
+          else
+            currentFeat->IntensityGap = -1;
+        }
+      }
+
+      // ---- Compute angle ----
+
+      if (this->Enabled[PLANE] || this->Enabled[EDGE])
+      {
+        // Compute angles
+        currentFeat->Angle = leftLine.Direction.dot(rightLine.Direction);
+        // Remove angles too small to be edges
+        if (currentFeat->Angle > -this->PlaneCosAngleThreshold)
+        {
+          currentFeat->Angle = 1.f;
+          continue;
+        }
+
+        // Remove previous point from angle inspection if the angle is not maximal locally
+        if (this->Enabled[EDGE] && currentFeat->Angle > this->EdgeCosAngleThreshold)
+        {
+          // Check previously computed angle to keep only the maximal angle keypoint locally
+          for (int idx = 1; idx < leftNeighbors.size(); idx++)
+          {
+            const std::shared_ptr<PtFeat>& neighPtr = this->GetPtFeat(leftNeighbors[idx]);
+            if (neighPtr->Angle > this->EdgeCosAngleThreshold &&
+                neighPtr->Angle < -this->PlaneCosAngleThreshold)
+            {
+              if (neighPtr->Angle > currentFeat->Angle)
+                currentFeat->Angle = 1.f;
+              else
+                neighPtr->Angle = 1.f;
+              break;
+            }
+          }
+        }
+      }
+    }
+  }
 }
 
 //-----------------------------------------------------------------------------
-- 
GitLab


From 2c52a713e7b1aeb89e08487284c20a1e2d0e2cbb Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Thu, 9 Nov 2023 13:50:23 +0100
Subject: [PATCH 09/13] [feat] Compute keypoints

Create patches to divide the vertex map to extract keypoints in a uniform way.
The dense extractor is now able to extract planes, edges, intensity edges and blobs
---
 .../lidar_slam/params/slam_config_indoor.yaml |   6 +
 .../params/slam_config_outdoor.yaml           |   8 +-
 ros_wrapping/lidar_slam/src/LidarSlamNode.cxx |   9 +-
 .../DenseSpinningSensorKeypointExtractor.h    |  43 +++-
 .../DenseSpinningSensorKeypointExtractor.cxx  | 208 +++++++++++++++++-
 5 files changed, 254 insertions(+), 20 deletions(-)

diff --git a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
index 185ab1224..fbb7018d3 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
@@ -403,6 +403,8 @@ slam:
     downsampling:
       max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
       voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
+      # Additional parameter for the dense extractor (DSSKE)
+      patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
 
     # # Multi LiDAR inputs
     # # The multiple devices to use for SLAM.
@@ -442,6 +444,8 @@ slam:
     #   downsampling:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
     #     voxel_grid_resolution: 0.5       # [m/voxel] Size of a voxel to downsample the extracted keypoints.
+    #     # Additional parameter for the dense extractor (DSSKE)
+    #     patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
     # device_lidar_frame_1:
     #   # The extractor modes allow to decide which keypoint extractor to choose
     #   # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
@@ -473,3 +477,5 @@ slam:
     #   downsampling:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
     #     voxel_grid_resolution: 0.5       # [m/voxel] Size of a voxel to downsample the extracted keypoints.
+    #     # Additional parameter for the dense extractor (DSSKE)
+    #     patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
diff --git a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
index 9508a684b..93d1175f0 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
@@ -404,6 +404,8 @@ slam:
     downsampling:
       max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
       voxel_grid_resolution: 2.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
+      # Additional parameter for the dense extractor (DSSKE)
+      patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
 
     # # Multi LiDAR inputs
     # # The multiple devices to use for SLAM.
@@ -443,6 +445,8 @@ slam:
     #   downsampling:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
     #     voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
+    #     # Additional parameter for the dense extractor (DSSKE)
+    #     patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
     # device_lidar_frame_1:
     #   # The extractor modes allow to decide which keypoint extractor to choose
     #   # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
@@ -473,4 +477,6 @@ slam:
     #   edge_intensity_gap_threshold: 200. # [0-255] Threshold upon intensity gap to select an edge keypoint.
     #   downsampling:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
-    #     voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
\ No newline at end of file
+    #     voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
+    #     # Additional parameter for the dense extractor (DSSKE)
+    #     patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
\ No newline at end of file
diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
index c48d46569..55bc1ccd9 100644
--- a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
+++ b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
@@ -1820,7 +1820,14 @@ void LidarSlamNode::SetSlamParameters()
     if (mode == LidarSlam::KeypointExtractorMode::SPARSE)
       ke = std::make_shared<LidarSlam::SpinningSensorKeypointExtractor>();
     else
-      ke = std::make_shared<LidarSlam::DenseSpinningSensorKeypointExtractor>();
+    {
+      auto dsske = std::make_shared<LidarSlam::DenseSpinningSensorKeypointExtractor>();
+      // Set specific parameter for DSSKE
+      int patchSize;
+      if (this->PrivNh.getParam(prefix + "downsampling/patch_size", patchSize))
+        dsske->SetPatchSize(patchSize);
+      ke = dsske;
+    }
 
     // Set keypoints extractor parameters and enable keypoints
     initKeypointsExtractor(ke, prefix);
diff --git a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
index 1af5f270c..e4e998f31 100644
--- a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
@@ -53,19 +53,15 @@ class DenseSpinningSensorKeypointExtractor : public KeypointExtractor
 {
 public:
 
-  GetMacro(PlaneCosAngleThreshold, float)
-  SetMacro(PlaneCosAngleThreshold, float)
-
-  GetMacro(EdgeCosAngleThreshold, float)
-  SetMacro(EdgeCosAngleThreshold, float)
-
-  // Set EdgeCosAngleThreshold and PlaneCosAngleThreshold from angle in degrees
   void SetEdgeAngleThreshold(float angle) override {this->EdgeCosAngleThreshold = std::cos(Utils::Deg2Rad(angle));};
   void SetPlaneAngleThreshold(float angle) override {this->PlaneCosAngleThreshold = std::cos(Utils::Deg2Rad(angle));};
   // Associated getters
   float GetEdgeAngleThreshold() const override {return this->EdgeCosAngleThreshold;};
   float GetPlaneAngleThreshold() const override {return this->PlaneCosAngleThreshold;};
 
+  GetMacro(PatchSize, int)
+  SetMacro(PatchSize, int)
+
   // Extract keypoints from the pointcloud. The key points
   // will be separated in two classes : Edges keypoints which
   // correspond to area with high curvature scan lines and
@@ -93,6 +89,9 @@ private:
   // Create vertex map from input pointcloud using indices stored in Pc2VmIndices
   void CreateVertexMap();
 
+  // Clear Keypoints Poinclouds, reserve new size
+  void ClearKeypoints();
+
   // Output separate point features contained in Vertex Map in pgm format to visualize as 2D image
   void OutputFeatures(std::string path);
 
@@ -110,10 +109,18 @@ private:
   // Add point to keypoint structure
   void AddKeypoint(const Keypoint& k, const LidarPoint &pt) override;
 
-  // Add all keypoints of the type k that comply with the threshold criteria for these values
-  // The threshold can be a minimum or maximum value (threshIsMax)
-  // The weight basis allow to weight the keypoint depending on its certainty
-  void AddKptsUsingCriterion(Keypoint k);
+  // Create square division of the image using 2 dimensions
+  void CreatePatchGrid(std::function<bool(const std::shared_ptr<PtFeat>&)> isPtFeatValid);
+
+  // Clear patch grid and reset the number of points in the grid
+  // To be called at each keypoint computation (in ComputeEdges, ComputePlanes, etc.))
+  void ClearPatchGrid();
+
+  // Add keypoints of type k to a keypoint pointcloud
+  // Using patches to have a uniform distribution of keypoints
+  void AddKptsUsingPatchGrid(Keypoint k,
+                             std::function<bool(const std::shared_ptr<PtFeat>&,
+                                                const std::shared_ptr<PtFeat>&)> comparePtFeats);
 
   // ---------------------------------------------------------------------------
   //   Parameters
@@ -126,6 +133,11 @@ private:
   // Sharpness threshold to select an edge keypoint
   float EdgeCosAngleThreshold = -0.5; // ~cos(120°) (selected, if cos angle is more than threshold)
 
+  // Size of a patch (nb of points in one dimension, a patch is a square)
+  // Patches are used for 2D grid construction to downsample the keypoints
+  // A patch with size 32 means that the patch will contain at most 32x32 points
+  int PatchSize = 32; // [nb]
+
   // ---------------------------------------------------------------------------
   //   Internal variables
   // ---------------------------------------------------------------------------
@@ -142,6 +154,15 @@ private:
 
   // Vertex Map of points' indices in the pointcloud
   std::vector<std::vector<std::shared_ptr<PtFeat>>> VertexMap;
+
+  // Patch grid used to downsample the keypoints to reduce global computation time
+  std::unordered_map<int, std::vector<std::shared_ptr<PtFeat>>> PatchGrid;
+
+  // Struct to store the number of points in each voxel/patch of the used grid;
+  int NbPointsInGrid;
+
+  // Extracted keypoints of current frame
+  std::map<Keypoint, std::vector<LidarPoint>> Keypoints;
 };
 
 } // namespace LidarSlam
\ No newline at end of file
diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
index 4b5f76105..1679adfa6 100644
--- a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
@@ -34,13 +34,27 @@ namespace LidarSlam
 //-----------------------------------------------------------------------------
 DenseSpinningSensorKeypointExtractor::PointCloud::Ptr DenseSpinningSensorKeypointExtractor::GetKeypoints(Keypoint k)
 {
-  //TODO
+  if (!this->Enabled.count(k) || !this->Enabled[k])
+  {
+    PRINT_ERROR("Unable to get keypoints of type " << KeypointTypeNames.at(k));
+    return PointCloud::Ptr();
+  }
+
+  PointCloud::Ptr keypointsCloud(new PointCloud);
+  Utils::CopyPointCloudMetadata(*this->Scan, *keypointsCloud);
+
+  std::vector<LidarPoint> points = this->Keypoints.at(k);
+  keypointsCloud->reserve(points.size());
+  for (const auto& pt : points)
+    keypointsCloud->push_back(pt);
+
+  return keypointsCloud;
 }
 
 //-----------------------------------------------------------------------------
 void DenseSpinningSensorKeypointExtractor::AddKeypoint(const Keypoint& k, const Point& pt)
 {
-  //TODO
+  this->Keypoints.at(k).push_back(pt);
 }
 
 //-----------------------------------------------------------------------------
@@ -124,6 +138,19 @@ void DenseSpinningSensorKeypointExtractor::CreateVertexMap()
   }
 }
 
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::ClearKeypoints()
+{
+  // Clear keypoints
+  this->Keypoints.clear();
+  // Initialize keypoints
+  for (const auto& k : KeypointTypes)
+  {
+    if (this->Enabled[k])
+      this->Keypoints[k].reserve(this->Scan->size());
+  }
+}
+
 //-----------------------------------------------------------------------------
 #define OUTPUT_FEATURE(filename, featName, maxValue, specialValue, minValue)                                      \
 {                                                                                                                 \
@@ -175,9 +202,22 @@ void DenseSpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Pt
 
   this->InitInternalParameters();
 
+  this->ClearKeypoints();
+
   this->CreateVertexMap();
 
   this->ComputeCurvature();
+
+  // Label and extract keypoints
+  //! Warning : order matters for computation time concerns
+  if (this->Enabled[Keypoint::PLANE])
+    this->ComputePlanes();
+  if (this->Enabled[Keypoint::EDGE])
+    this->ComputeEdges();
+  if (this->Enabled[Keypoint::INTENSITY_EDGE])
+    this->ComputeIntensityEdges();
+  if (this->Enabled[Keypoint::BLOB])
+    this->ComputeBlobs();
 }
 
 //-----------------------------------------------------------------------------
@@ -415,27 +455,181 @@ void DenseSpinningSensorKeypointExtractor::ComputeCurvature()
 }
 
 //-----------------------------------------------------------------------------
-void DenseSpinningSensorKeypointExtractor::AddKptsUsingCriterion(Keypoint k)
+void DenseSpinningSensorKeypointExtractor::CreatePatchGrid(std::function<bool(const std::shared_ptr<PtFeat>&)> isPtFeatValid)
 {
-  //TODO
+  this->ClearPatchGrid();
+  int nbPatchesX = std::ceil(this->WidthVM / this->PatchSize);
+  int nbPatchesY = std::ceil(this->HeightVM / this->PatchSize);
+  this->PatchGrid.reserve(nbPatchesX * nbPatchesY);
+
+  // Fill grid with smart pointers to PtFeat
+  for (unsigned int i = 0; i < this->HeightVM; ++i)
+  {
+    for (unsigned int j = 0; j < this->WidthVM; ++j)
+    {
+      const auto& ptFeat = this->VertexMap[i][j];
+      if (!isPtFeatValid(ptFeat))
+        continue;
+      int idxPatchY = ((i + this->HeightVM) % this->HeightVM) / this->PatchSize;
+      int idxPatchX = ((j + this->WidthVM)  % this->WidthVM)  / this->PatchSize;
+      int indexPatch = idxPatchY * nbPatchesX + idxPatchX;
+      this->PatchGrid[indexPatch].emplace_back(ptFeat);
+      this->NbPointsInGrid++;
+    }
+  }
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::ClearPatchGrid()
+{
+  this->PatchGrid.clear();
+  this->NbPointsInGrid = 0;
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::AddKptsUsingPatchGrid(Keypoint k,
+                                                                 std::function<bool(const std::shared_ptr<PtFeat>&,
+                                                                                    const std::shared_ptr<PtFeat>&)> comparePtFeat)
+{
+  // If we have less candidates than the max keypoints number
+  if (this->NbPointsInGrid < this->MaxPoints)
+  {
+    for (auto& cell : this->PatchGrid)
+    {
+      auto& vec = cell.second;
+      for (auto& pt : vec)
+      {
+        this->AddKeypoint(k, this->Scan->at(pt->Index));
+        pt->KptTypes.set(static_cast<int>(k));
+      }
+    }
+    return;
+  }
+
+  // While nbkeypointextracted < nbpointmax && remains keypoints candidates
+  int ptIdx = 0;
+  while (ptIdx < this->MaxPoints)
+  {
+    bool remainKptCandidate = false;
+    for (auto& cell : this->PatchGrid)
+    {
+      auto& vec = cell.second;
+
+      // Check if cell has point that could be keypoints.
+      // Reminder : cells only contain valid points, so here we only check for points that have not been extracted as keypoints for this type
+      bool hasKptCandidate = std::any_of(vec.begin(), vec.end(), [&](const std::shared_ptr<PtFeat>& pt) {return !pt->KptTypes[static_cast<int>(k)];});
+      if (!hasKptCandidate)
+        continue;
+      remainKptCandidate = true;
+
+      // Search for max element in patch using comparePtFeat to compare all feats simultaneously
+      auto maxPtIt = std::max_element(vec.begin(), vec.end(), comparePtFeat);
+      const auto& maxPt = *maxPtIt;
+
+      // Add to keypoints
+      this->AddKeypoint(k, this->Scan->at(maxPt->Index));
+      maxPt->KptTypes.set(static_cast<int>(k));
+      ++ptIdx;
+
+      if (ptIdx >= this->MaxPoints)
+        break;
+    }
+    if (!remainKptCandidate)
+      break;
+  }
 }
 
 //-----------------------------------------------------------------------------
 void DenseSpinningSensorKeypointExtractor::ComputePlanes()
 {
-  //TODO
+  auto isPtValid = [this](const std::shared_ptr<PtFeat>& pt)
+  {
+    return pt != nullptr &&
+           (!pt->KptTypes[static_cast<int>(PLANE)] && !pt->KptTypes[static_cast<int>(EDGE)]) &&
+           pt->Angle < this->PlaneCosAngleThreshold;
+  };
+  this->CreatePatchGrid(isPtValid);
+  this->AddKptsUsingPatchGrid(Keypoint::PLANE,
+                              [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
+                              {
+                                if (!isPtValid(a) && isPtValid(b))
+                                  return true;  // b is considered greater when a is nullptr
+                                else if (isPtValid(a) && !isPtValid(b))
+                                  return false;   // a is considered greater when b is nullptr
+                                else if (!isPtValid(a) && !isPtValid(b))
+                                  return true;  // Both are nullptr, no preference
+
+                                return (a->Angle > b->Angle);
+                              });
 }
 
 //-----------------------------------------------------------------------------
 void DenseSpinningSensorKeypointExtractor::ComputeEdges()
 {
-  //TODO
+  auto isPtValid = [this](const std::shared_ptr<PtFeat>& pt)
+  {
+    return pt != nullptr &&
+           (!pt->KptTypes[static_cast<int>(PLANE)] && !pt->KptTypes[static_cast<int>(EDGE)])&&
+           ((pt->DepthGap - (-1.0f) > 1e-6 && pt->DepthGap > this->EdgeDepthGapThreshold) ||
+            (pt->Angle < -this->PlaneCosAngleThreshold && pt->Angle > this->EdgeCosAngleThreshold) ||
+            (pt->SpaceGap - (-1.0f) > 1e-6 && pt->SpaceGap > this->EdgeDepthGapThreshold));
+  };
+  this->CreatePatchGrid(isPtValid);
+  this->AddKptsUsingPatchGrid(Keypoint::EDGE,
+                              [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
+                              {
+                                if (!isPtValid(a) && isPtValid(b))
+                                  return true;  // b is considered greater when a is nullptr
+                                else if (isPtValid(a) && !isPtValid(b))
+                                  return false;   // a is considered greater when b is nullptr
+                                else if (!isPtValid(a) && !isPtValid(b))
+                                  return true;  // Both are nullptr, no preference
+
+                                if (a->DepthGap < b->DepthGap && b->DepthGap > this->EdgeDepthGapThreshold)
+                                  return true;
+                                if (b->DepthGap < a->DepthGap && a->DepthGap > this->EdgeDepthGapThreshold)
+                                  return false;
+
+                                if (a->Angle < b->Angle && b->Angle < -this->PlaneCosAngleThreshold && b->Angle > this->EdgeCosAngleThreshold)
+                                  return true;
+                                if (b->Angle < a->Angle && a->Angle < -this->PlaneCosAngleThreshold && a->Angle > this->EdgeCosAngleThreshold)
+                                  return false;
+
+                                if (a->SpaceGap < b->SpaceGap && b->SpaceGap > this->EdgeDepthGapThreshold)
+                                  return true;
+                                if (b->SpaceGap < a->SpaceGap && a->SpaceGap > this->EdgeDepthGapThreshold)
+                                  return false;
+
+                                return true;
+                              });
 }
 
 //-----------------------------------------------------------------------------
 void DenseSpinningSensorKeypointExtractor::ComputeIntensityEdges()
 {
-  //TODO
+  auto isPtValid = [this](const std::shared_ptr<PtFeat>& pt)
+  {
+    return pt != nullptr &&
+           (!pt->KptTypes[static_cast<int>(EDGE)] && !pt->KptTypes[static_cast<int>(INTENSITY_EDGE)]) &&
+           (pt->IntensityGap - (-1.0f) > 1e-6 && pt->IntensityGap > this->EdgeIntensityGapThreshold);
+  };
+  this->CreatePatchGrid(isPtValid);
+  this->AddKptsUsingPatchGrid(Keypoint::INTENSITY_EDGE,
+                              [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
+                              {
+                                if (!isPtValid(a) && isPtValid(b))
+                                  return true;  // b is considered greater when a is nullptr
+                                else if (isPtValid(a) && !isPtValid(b))
+                                  return false;   // a is considered greater when b is nullptr
+                                else if (!isPtValid(a) && !isPtValid(b))
+                                  return true;  // Both are nullptr, no preference
+
+                                if (a->IntensityGap < b->IntensityGap && b->IntensityGap > this->EdgeIntensityGapThreshold)
+                                  return true;
+                                if (b->IntensityGap < a->IntensityGap && a->IntensityGap > this->EdgeIntensityGapThreshold)
+                                  return false;
+                                return true;
+                              });
 }
 
 //-----------------------------------------------------------------------------
-- 
GitLab


From d9cada4978752dd805b77d9a462192d21c212581 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Tue, 23 Jan 2024 16:11:21 +0100
Subject: [PATCH 10/13] [debug] Add GetDebugArray in DSSKE

---
 .../DenseSpinningSensorKeypointExtractor.cxx  | 25 ++++++++++++++++++-
 1 file changed, 24 insertions(+), 1 deletion(-)

diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
index 1679adfa6..bc52be602 100644
--- a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
@@ -635,7 +635,30 @@ void DenseSpinningSensorKeypointExtractor::ComputeIntensityEdges()
 //-----------------------------------------------------------------------------
 std::unordered_map<std::string, std::vector<float>> DenseSpinningSensorKeypointExtractor::GetDebugArray() const
 {
-  //TODO
+  std::unordered_map<std::string, std::vector<float>> debugArray;
+  debugArray["space_gap"]               = std::vector<float>(this->Scan->size(), 0.f);
+  debugArray["depth_gap"]               = std::vector<float>(this->Scan->size(), 0.f);
+  debugArray["intensity_gap"]           = std::vector<float>(this->Scan->size(), 0.f);
+  debugArray["cos_angle"]               = std::vector<float>(this->Scan->size(), 0.f);
+  debugArray["edge_keypoint"]           = std::vector<float>(this->Scan->size(), 0.f);
+  debugArray["plane_keypoint"]          = std::vector<float>(this->Scan->size(), 0.f);
+  debugArray["intensity_edge_keypoint"] = std::vector<float>(this->Scan->size(), 0.f);
+  debugArray["blob_keypoint"]           = std::vector<float>(this->Scan->size(), 0.f);
+
+  for (int i = 0; i < this->Scan->size(); i++)
+  {
+    const std::shared_ptr<PtFeat>& ptrFeat = this->GetPtFeat(i);
+    debugArray["space_gap"][i]               = ptrFeat->SpaceGapH;
+    debugArray["depth_gap"][i]               = ptrFeat->DepthGapH;
+    debugArray["intensity_gap"][i]           = ptrFeat->IntensityGapH;
+    debugArray["cos_angle"][i]               = ptrFeat->Angle;
+    debugArray["edge_keypoint"][i]           = ptrFeat->KptTypes[static_cast<int>(Keypoint::EDGE)];
+    debugArray["plane_keypoint"][i]          = ptrFeat->KptTypes[static_cast<int>(Keypoint::PLANE)];
+    debugArray["intensity_edge_keypoint"][i] = ptrFeat->KptTypes[static_cast<int>(Keypoint::INTENSITY_EDGE)];
+    debugArray["blob_keypoint"][i]           = ptrFeat->KptTypes[static_cast<int>(Keypoint::BLOB)];
+  }
+
+  return debugArray;
 }
 
 }// end of LidarSlam namespace
\ No newline at end of file
-- 
GitLab


From 033f9b7c285210304e926502ba54bdf2685c20c5 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Fri, 10 Nov 2023 13:32:40 +0100
Subject: [PATCH 11/13] [debug] Output keypoints in csv and pgm format

For each keypoint type, gives two outputs :
-csv : 3D pointcloud to be visualised in CloudCompare or LidarView
-pgm : 2D image to be used in GIMP or to compare with 2D images of the point feats
---
 .../DenseSpinningSensorKeypointExtractor.h    |  4 ++
 .../DenseSpinningSensorKeypointExtractor.cxx  | 37 +++++++++++++++++++
 2 files changed, 41 insertions(+)

diff --git a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
index e4e998f31..4c93dd46f 100644
--- a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
@@ -95,6 +95,10 @@ private:
   // Output separate point features contained in Vertex Map in pgm format to visualize as 2D image
   void OutputFeatures(std::string path);
 
+  // Output Keypoints in csv format to use as 3D pointcloud
+  // and in pgm format to visualize as 2D image
+  bool OutputKeypoints(std::string path);
+
   // Compute the curvature features within each scan line : depth
   // space gap, intensity gap and line angle
   void ComputeCurvature() override;
diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
index bc52be602..63e255ddb 100644
--- a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
@@ -195,6 +195,43 @@ void DenseSpinningSensorKeypointExtractor::OutputFeatures(std::string path)
   OUTPUT_FEATURE(path + "Angles.pgm", Angle, 1., 1., -1.);
 }
 
+//-----------------------------------------------------------------------------
+bool DenseSpinningSensorKeypointExtractor::OutputKeypoints(std::string path)
+{
+  for (const auto& type : this->Keypoints)
+  {
+    const Keypoint& k = type.first;
+    // In case a keypoint type has been disabled during the process but not changed in this->Keypoints
+    if (!this->Enabled[k])
+      continue;
+
+    std::ofstream fileCsv(path + "keypoints_" + KeypointTypeNames.at(k) + ".csv");
+    fileCsv << "x,y,z,kpt\n";
+
+    std::ofstream filePgm(path + "keypoints_" + KeypointTypeNames.at(k) + ".pgm");
+    filePgm << "P2\n";
+    filePgm << this->WidthVM << " " << this->HeightVM << "\n";
+    filePgm << 1 << "\n";
+    for (int i = 0; i < this->HeightVM; ++i)
+    {
+      for (int j = 0; j < this->WidthVM; ++j)
+      {
+        const auto& ptFeat = this->VertexMap[i][j];
+        if (ptFeat == nullptr)
+          continue;
+        const Point& point = this->Scan->at(ptFeat->Index);
+        fileCsv << point.x << "," << point.y << "," << point.z << "," << ptFeat->KptTypes[static_cast<int>(k)] << "\n";
+        filePgm << ptFeat->KptTypes[static_cast<int>(k)] << " ";
+      }
+      filePgm << "\n";
+    }
+    fileCsv.close();
+    filePgm.close();
+  }
+
+  return true;
+}
+
 //-----------------------------------------------------------------------------
 void DenseSpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Ptr& pc)
 {
-- 
GitLab


From 1e71f54db1b48f47310a3a74473c4abddc9dee27 Mon Sep 17 00:00:00 2001
From: Jeanne Faure <jeanne.faure@kitware.com>
Date: Wed, 29 Nov 2023 09:47:36 +0100
Subject: [PATCH 12/13] [feat] Add 3D grid for keypoint extraction

Building a sampling grid on the 2D VertexMap was sometimes unefficient to extract far points as keypoints.
We decided to add a 3D mode in case the keypoints extracted by the 2D Patch grid is unsatisfying.
---
 .../lidar_slam/params/slam_config_indoor.yaml |  15 +-
 .../params/slam_config_outdoor.yaml           |  17 +-
 ros_wrapping/lidar_slam/src/LidarSlamNode.cxx |  23 +-
 .../DenseSpinningSensorKeypointExtractor.h    |  25 +-
 slam_lib/include/LidarSlam/Enums.h            |  15 +
 .../DenseSpinningSensorKeypointExtractor.cxx  | 273 +++++++++++++-----
 6 files changed, 285 insertions(+), 83 deletions(-)

diff --git a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
index fbb7018d3..34304daf1 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_indoor.yaml
@@ -403,8 +403,11 @@ slam:
     downsampling:
       max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
       voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
-      # Additional parameter for the dense extractor (DSSKE)
+      # Additional parameters for the dense extractor (DSSKE)
       patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
+      sampling_mode: 0                 # {0, 1} Sampling mode for keypoint extraction :
+                                       # 0) Patch mode --> for a 2D grid built on the vertex map.
+                                       # 1) Voxel mode --> for a 3D grid built on the scan cloud, use voxel_grid_resolution common to SSKE.
 
     # # Multi LiDAR inputs
     # # The multiple devices to use for SLAM.
@@ -444,8 +447,11 @@ slam:
     #   downsampling:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
     #     voxel_grid_resolution: 0.5       # [m/voxel] Size of a voxel to downsample the extracted keypoints.
-    #     # Additional parameter for the dense extractor (DSSKE)
+    #     # Additional parameters for the dense extractor (DSSKE)
     #     patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
+    #     sampling_mode: 0                 # {0, 1} Sampling mode for keypoint extraction :
+    #                                      # 0) Patch mode --> for a 2D grid built on the vertex map.
+    #                                      # 1) Voxel mode --> for a 3D grid built on the scan cloud, use voxel_grid_resolution common to SSKE.
     # device_lidar_frame_1:
     #   # The extractor modes allow to decide which keypoint extractor to choose
     #   # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
@@ -477,5 +483,8 @@ slam:
     #   downsampling:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
     #     voxel_grid_resolution: 0.5       # [m/voxel] Size of a voxel to downsample the extracted keypoints.
-    #     # Additional parameter for the dense extractor (DSSKE)
+    #     # Additional parameters for the dense extractor (DSSKE)
     #     patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
+    #     sampling_mode: 0                 # {0, 1} Sampling mode for keypoint extraction :
+    #                                      # 0) Patch mode --> for a 2D grid built on the vertex map.
+    #                                      # 1) Voxel mode --> for a 3D grid built on the scan cloud, use voxel_grid_resolution common to SSKE.
\ No newline at end of file
diff --git a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
index 93d1175f0..bbad4b346 100644
--- a/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config_outdoor.yaml
@@ -404,8 +404,11 @@ slam:
     downsampling:
       max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
       voxel_grid_resolution: 2.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
-      # Additional parameter for the dense extractor (DSSKE)
+      # Additional parameters for the dense extractor (DSSKE)
       patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
+      sampling_mode: 0                 # {0, 1} Sampling mode for keypoint extraction :
+                                        # 0) Patch mode --> for a 2D grid built on the vertex map.
+                                        # 1) Voxel mode --> for a 3D grid built on the scan cloud, use voxel_grid_resolution common to SSKE.
 
     # # Multi LiDAR inputs
     # # The multiple devices to use for SLAM.
@@ -445,8 +448,11 @@ slam:
     #   downsampling:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
     #     voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
-    #     # Additional parameter for the dense extractor (DSSKE)
+    #     # Additional parameters for the dense extractor (DSSKE)
     #     patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
+    #     sampling_mode: 0                 # {0, 1} Sampling mode for keypoint extraction :
+    #                                      # 0) Patch mode --> for a 2D grid built on the vertex map.
+    #                                      # 1) Voxel mode --> for a 3D grid built on the scan cloud, use voxel_grid_resolution common to SSKE.
     # device_lidar_frame_1:
     #   # The extractor modes allow to decide which keypoint extractor to choose
     #   # 0) Sparse extractor called SpinningSensorKeypointExtractor. Convenient for all lidars.
@@ -478,5 +484,8 @@ slam:
     #   downsampling:
     #     max_points: 1000                 # [points] Maximum number of keypoints of each type to extract.
     #     voxel_grid_resolution: 1.        # [m/voxel] Size of a voxel to downsample the extracted keypoints.
-    #     # Additional parameter for the dense extractor (DSSKE)
-    #     patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
\ No newline at end of file
+    #     # Additional parameters for the dense extractor (DSSKE)
+    #     patch_size: 32                   # [nb] Size of a patch. It will contain at much patch_size*patch_size keypoints candidates.
+    #     sampling_mode: 0                 # {0, 1} Sampling mode for keypoint extraction :
+    #                                      # 0) Patch mode --> for a 2D grid built on the vertex map.
+    #                                      # 1) Voxel mode --> for a 3D grid built on the scan cloud, use voxel_grid_resolution common to SSKE.
\ No newline at end of file
diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
index 55bc1ccd9..ee506e77b 100644
--- a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
+++ b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx
@@ -1749,6 +1749,25 @@ void LidarSlamNode::SetSlamParameters()
     }
   };
 
+  auto setSamplingMode = [this](auto& dsske,
+                                const std::string& prefix,
+                                const std::string& message = "")
+  {
+    int samplingMode;
+    if (this->PrivNh.getParam(prefix + "downsampling/sampling_mode", samplingMode))
+    {
+      LidarSlam::SamplingModeDSSKE sampling = static_cast<LidarSlam::SamplingModeDSSKE>(samplingMode);
+      if (sampling != LidarSlam::SamplingModeDSSKE::PATCH &&
+          sampling != LidarSlam::SamplingModeDSSKE::VOXEL)
+      {
+        ROS_ERROR_STREAM("Invalid DSSKE sampling mode (" << samplingMode << ")"
+                         << message << ". Setting it to 'PATCH'.");
+        sampling = LidarSlam::SamplingModeDSSKE::PATCH;
+      }
+      dsske->SetSamplingDSSKE(sampling);
+    }
+  };
+
   auto initKeypointsExtractor = [this](auto& ke, const std::string& prefix)
   {
     #define SetKeypointsExtractorParam(type, rosParam, keParam) {type val; if (this->PrivNh.getParam(rosParam, val)) ke->Set##keParam(val);}
@@ -1822,10 +1841,12 @@ void LidarSlamNode::SetSlamParameters()
     else
     {
       auto dsske = std::make_shared<LidarSlam::DenseSpinningSensorKeypointExtractor>();
-      // Set specific parameter for DSSKE
+      // Set specific parameters for DSSKE
       int patchSize;
       if (this->PrivNh.getParam(prefix + "downsampling/patch_size", patchSize))
         dsske->SetPatchSize(patchSize);
+      deviceIds.size() == 1 ? setSamplingMode(dsske, prefix)
+                            : setSamplingMode(dsske, prefix, " for LiDAR device " + deviceId);
       ke = dsske;
     }
 
diff --git a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
index 4c93dd46f..966f20fc9 100644
--- a/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/DenseSpinningSensorKeypointExtractor.h
@@ -62,6 +62,9 @@ public:
   GetMacro(PatchSize, int)
   SetMacro(PatchSize, int)
 
+  GetMacro(SamplingDSSKE, LidarSlam::SamplingModeDSSKE)
+  SetMacro(SamplingDSSKE, LidarSlam::SamplingModeDSSKE)
+
   // Extract keypoints from the pointcloud. The key points
   // will be separated in two classes : Edges keypoints which
   // correspond to area with high curvature scan lines and
@@ -114,17 +117,20 @@ private:
   void AddKeypoint(const Keypoint& k, const LidarPoint &pt) override;
 
   // Create square division of the image using 2 dimensions
-  void CreatePatchGrid(std::function<bool(const std::shared_ptr<PtFeat>&)> isPtFeatValid);
+  void Create2DGrid(std::function<bool(const std::shared_ptr<PtFeat>&)> isPtFeatValid);
+
+  // Create cubic division of the pointcloud using 3 dimensions
+  void Create3DGrid(std::function<bool(const std::shared_ptr<PtFeat>&)> isPtFeatValid);
 
-  // Clear patch grid and reset the number of points in the grid
+  // Clear grid and reset the number of points in the grid
   // To be called at each keypoint computation (in ComputeEdges, ComputePlanes, etc.))
-  void ClearPatchGrid();
+  void ClearGrid();
 
   // Add keypoints of type k to a keypoint pointcloud
   // Using patches to have a uniform distribution of keypoints
-  void AddKptsUsingPatchGrid(Keypoint k,
-                             std::function<bool(const std::shared_ptr<PtFeat>&,
-                                                const std::shared_ptr<PtFeat>&)> comparePtFeats);
+  void AddKptsUsingGrid(Keypoint k,
+                        std::function<bool(const std::shared_ptr<PtFeat>&,
+                                           const std::shared_ptr<PtFeat>&)> comparePtFeats);
 
   // ---------------------------------------------------------------------------
   //   Parameters
@@ -137,6 +143,9 @@ private:
   // Sharpness threshold to select an edge keypoint
   float EdgeCosAngleThreshold = -0.5; // ~cos(120°) (selected, if cos angle is more than threshold)
 
+  // Downsampling mode
+  SamplingModeDSSKE SamplingDSSKE = SamplingModeDSSKE::PATCH;
+
   // Size of a patch (nb of points in one dimension, a patch is a square)
   // Patches are used for 2D grid construction to downsample the keypoints
   // A patch with size 32 means that the patch will contain at most 32x32 points
@@ -159,8 +168,8 @@ private:
   // Vertex Map of points' indices in the pointcloud
   std::vector<std::vector<std::shared_ptr<PtFeat>>> VertexMap;
 
-  // Patch grid used to downsample the keypoints to reduce global computation time
-  std::unordered_map<int, std::vector<std::shared_ptr<PtFeat>>> PatchGrid;
+  // Grid (voxel or patch grid) used to downsample the keypoints
+  std::unordered_map<int, std::vector<std::shared_ptr<PtFeat>>> Grid;
 
   // Struct to store the number of points in each voxel/patch of the used grid;
   int NbPointsInGrid;
diff --git a/slam_lib/include/LidarSlam/Enums.h b/slam_lib/include/LidarSlam/Enums.h
index a18a96c3c..1cb7729b6 100644
--- a/slam_lib/include/LidarSlam/Enums.h
+++ b/slam_lib/include/LidarSlam/Enums.h
@@ -156,6 +156,21 @@ enum class MappingMode
   UPDATE = 2,
 };
 
+//------------------------------------------------------------------------------
+//! How to downsample for DSSKE
+enum class SamplingModeDSSKE
+{
+  //! Use 2D downsampling to extract keypoints
+  //! The grid is built using the VertexMap (like patches of an image)
+  PATCH = 0,
+
+  //! Use 3D downsampling to extract keypoints
+  //! The grid is built using the original Scan cloud (like voxel grid in SSKE)
+  VOXEL = 1,
+
+  nbSamplingModes = 2
+};
+
 //------------------------------------------------------------------------------
 //! How to downsample the map
 // A voxel grid is used and various downsampling modes
diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
index 63e255ddb..c00af537e 100644
--- a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
@@ -255,6 +255,8 @@ void DenseSpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Pt
     this->ComputeIntensityEdges();
   if (this->Enabled[Keypoint::BLOB])
     this->ComputeBlobs();
+
+  this->ClearGrid();
 }
 
 //-----------------------------------------------------------------------------
@@ -492,12 +494,12 @@ void DenseSpinningSensorKeypointExtractor::ComputeCurvature()
 }
 
 //-----------------------------------------------------------------------------
-void DenseSpinningSensorKeypointExtractor::CreatePatchGrid(std::function<bool(const std::shared_ptr<PtFeat>&)> isPtFeatValid)
+void DenseSpinningSensorKeypointExtractor::Create2DGrid(std::function<bool(const std::shared_ptr<PtFeat>&)> isPtFeatValid)
 {
-  this->ClearPatchGrid();
+  this->ClearGrid();
   int nbPatchesX = std::ceil(this->WidthVM / this->PatchSize);
   int nbPatchesY = std::ceil(this->HeightVM / this->PatchSize);
-  this->PatchGrid.reserve(nbPatchesX * nbPatchesY);
+  this->Grid.reserve(nbPatchesX * nbPatchesY);
 
   // Fill grid with smart pointers to PtFeat
   for (unsigned int i = 0; i < this->HeightVM; ++i)
@@ -510,28 +512,60 @@ void DenseSpinningSensorKeypointExtractor::CreatePatchGrid(std::function<bool(co
       int idxPatchY = ((i + this->HeightVM) % this->HeightVM) / this->PatchSize;
       int idxPatchX = ((j + this->WidthVM)  % this->WidthVM)  / this->PatchSize;
       int indexPatch = idxPatchY * nbPatchesX + idxPatchX;
-      this->PatchGrid[indexPatch].emplace_back(ptFeat);
+      this->Grid[indexPatch].emplace_back(ptFeat);
       this->NbPointsInGrid++;
     }
   }
 }
 
 //-----------------------------------------------------------------------------
-void DenseSpinningSensorKeypointExtractor::ClearPatchGrid()
+void DenseSpinningSensorKeypointExtractor::Create3DGrid(std::function<bool(const std::shared_ptr<PtFeat>&)> isPtFeatValid)
+{
+  this->ClearGrid();
+  // Find width and height of the scan (in meters)
+  Eigen::Vector4f ptMin, ptMax;
+  pcl::getMinMax3D(*this->Scan, ptMin, ptMax);
+  int xSize = std::abs(ptMax.x() - ptMin.x());
+  int ySize = std::abs(ptMax.y() - ptMin.y());
+  int zSize = std::abs(ptMax.z() - ptMin.z());
+
+  // Compute the number of voxels in each direction
+  int nbVoxelsX = std::ceil(xSize / this->VoxelResolution);
+  int nbVoxelsY = std::ceil(ySize / this->VoxelResolution);
+  int nbVoxelsZ = std::ceil(zSize / this->VoxelResolution);
+  // Reserve memory for the grid
+  this->Grid.reserve(std::min(nbVoxelsX * nbVoxelsY * nbVoxelsZ, static_cast<int>(this->Scan->size())));
+
+  // Fill grid with smart pointers to PtFeat
+  for (unsigned int i = 0; i < this->Scan->size(); i++)
+  {
+    const LidarPoint& point = this->Scan->at(i);
+    auto ptFeat = this->GetPtFeat(i);
+    if (!isPtFeatValid(ptFeat))
+      continue;
+    Eigen::Vector3i voxel = ((point.getVector3fMap() - ptMin.head(3)) / this->VoxelResolution).cast<int>();
+    int index1D = voxel.z() * nbVoxelsX * nbVoxelsY + voxel.y() * nbVoxelsX + voxel.x();
+    this->Grid[index1D].push_back(ptFeat);
+    this->NbPointsInGrid++;
+  }
+}
+
+//-----------------------------------------------------------------------------
+void DenseSpinningSensorKeypointExtractor::ClearGrid()
 {
-  this->PatchGrid.clear();
+  this->Grid.clear();
   this->NbPointsInGrid = 0;
 }
 
 //-----------------------------------------------------------------------------
-void DenseSpinningSensorKeypointExtractor::AddKptsUsingPatchGrid(Keypoint k,
-                                                                 std::function<bool(const std::shared_ptr<PtFeat>&,
-                                                                                    const std::shared_ptr<PtFeat>&)> comparePtFeat)
+void DenseSpinningSensorKeypointExtractor::AddKptsUsingGrid(Keypoint k,
+                                                            std::function<bool(const std::shared_ptr<PtFeat>&,
+                                                                               const std::shared_ptr<PtFeat>&)> comparePtFeat)
 {
   // If we have less candidates than the max keypoints number
   if (this->NbPointsInGrid < this->MaxPoints)
   {
-    for (auto& cell : this->PatchGrid)
+    for (auto& cell : this->Grid)
     {
       auto& vec = cell.second;
       for (auto& pt : vec)
@@ -548,7 +582,7 @@ void DenseSpinningSensorKeypointExtractor::AddKptsUsingPatchGrid(Keypoint k,
   while (ptIdx < this->MaxPoints)
   {
     bool remainKptCandidate = false;
-    for (auto& cell : this->PatchGrid)
+    for (auto& cell : this->Grid)
     {
       auto& vec = cell.second;
 
@@ -585,19 +619,47 @@ void DenseSpinningSensorKeypointExtractor::ComputePlanes()
            (!pt->KptTypes[static_cast<int>(PLANE)] && !pt->KptTypes[static_cast<int>(EDGE)]) &&
            pt->Angle < this->PlaneCosAngleThreshold;
   };
-  this->CreatePatchGrid(isPtValid);
-  this->AddKptsUsingPatchGrid(Keypoint::PLANE,
-                              [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
-                              {
-                                if (!isPtValid(a) && isPtValid(b))
-                                  return true;  // b is considered greater when a is nullptr
-                                else if (isPtValid(a) && !isPtValid(b))
-                                  return false;   // a is considered greater when b is nullptr
-                                else if (!isPtValid(a) && !isPtValid(b))
-                                  return true;  // Both are nullptr, no preference
-
-                                return (a->Angle > b->Angle);
-                              });
+  switch (this->SamplingDSSKE)
+  {
+    // If Patch mode activated : use a 2D grid built on vertex map
+    // to extract keypoints
+    case SamplingModeDSSKE::PATCH:
+    {
+      this->Create2DGrid(isPtValid);
+      this->AddKptsUsingGrid(Keypoint::PLANE,
+                             [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
+                             {
+                               if (!isPtValid(a) && isPtValid(b))
+                                 return true;  // b is considered greater when a is nullptr
+                               else if (isPtValid(a) && !isPtValid(b))
+                                 return false;   // a is considered greater when b is nullptr
+                               else if (!isPtValid(a) && !isPtValid(b))
+                                 return true;  // Both are nullptr, no preference
+
+                               return (a->Angle > b->Angle);
+                             });
+      break;
+    }
+    // If Voxel mode activated : use a 3D grid built on scan cloud
+    // to extract keypoints
+    case SamplingModeDSSKE::VOXEL:
+    {
+      this->Create3DGrid(isPtValid);
+      this->AddKptsUsingGrid(Keypoint::PLANE,
+                             [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
+                             {
+                               if (!isPtValid(a) && isPtValid(b))
+                                 return true;  // b is considered greater when a is nullptr
+                               else if (isPtValid(a) && !isPtValid(b))
+                                 return false;   // a is considered greater when b is nullptr
+                               else if (!isPtValid(a) && !isPtValid(b))
+                                 return true;  // Both are nullptr, no preference
+
+                               return (a->Angle > b->Angle);
+                             });
+      break;
+    }
+  }
 }
 
 //-----------------------------------------------------------------------------
@@ -611,34 +673,77 @@ void DenseSpinningSensorKeypointExtractor::ComputeEdges()
             (pt->Angle < -this->PlaneCosAngleThreshold && pt->Angle > this->EdgeCosAngleThreshold) ||
             (pt->SpaceGap - (-1.0f) > 1e-6 && pt->SpaceGap > this->EdgeDepthGapThreshold));
   };
-  this->CreatePatchGrid(isPtValid);
-  this->AddKptsUsingPatchGrid(Keypoint::EDGE,
-                              [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
-                              {
-                                if (!isPtValid(a) && isPtValid(b))
-                                  return true;  // b is considered greater when a is nullptr
-                                else if (isPtValid(a) && !isPtValid(b))
-                                  return false;   // a is considered greater when b is nullptr
-                                else if (!isPtValid(a) && !isPtValid(b))
-                                  return true;  // Both are nullptr, no preference
-
-                                if (a->DepthGap < b->DepthGap && b->DepthGap > this->EdgeDepthGapThreshold)
-                                  return true;
-                                if (b->DepthGap < a->DepthGap && a->DepthGap > this->EdgeDepthGapThreshold)
-                                  return false;
-
-                                if (a->Angle < b->Angle && b->Angle < -this->PlaneCosAngleThreshold && b->Angle > this->EdgeCosAngleThreshold)
+  switch (this->SamplingDSSKE)
+  {
+    // If Patch mode activated : use a 2D grid built on vertex map
+    // to extract keypoints
+    case SamplingModeDSSKE::PATCH:
+    {
+      this->Create2DGrid(isPtValid);
+      this->AddKptsUsingGrid(Keypoint::EDGE,
+                             [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
+                             {
+                               if (!isPtValid(a) && isPtValid(b))
+                                 return true;  // b is considered greater when a is nullptr
+                               else if (isPtValid(a) && !isPtValid(b))
+                                 return false;   // a is considered greater when b is nullptr
+                               else if (!isPtValid(a) && !isPtValid(b))
+                                 return true;  // Both are nullptr, no preference
+
+                               if (a->DepthGap < b->DepthGap && b->DepthGap > this->EdgeDepthGapThreshold)
+                                 return true;
+                               if (b->DepthGap < a->DepthGap && a->DepthGap > this->EdgeDepthGapThreshold)
+                                 return false;
+
+                               if (a->Angle < b->Angle && b->Angle < -this->PlaneCosAngleThreshold && b->Angle > this->EdgeCosAngleThreshold)
+                                 return true;
+                               if (b->Angle < a->Angle && a->Angle < -this->PlaneCosAngleThreshold && a->Angle > this->EdgeCosAngleThreshold)
+                                 return false;
+
+                               if (a->SpaceGap < b->SpaceGap && b->SpaceGap > this->EdgeDepthGapThreshold)
+                                 return true;
+                               if (b->SpaceGap < a->SpaceGap && a->SpaceGap > this->EdgeDepthGapThreshold)
+                                 return false;
+
+                               return true;
+                              });
+      break;
+    }
+    // If Voxel mode activated : use a 3D grid built on scan cloud
+    // to extract keypoints
+    case SamplingModeDSSKE::VOXEL:
+    {
+      this->Create3DGrid(isPtValid);
+      this->AddKptsUsingGrid(Keypoint::EDGE,
+                             [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
+                             {
+                               if (!isPtValid(a) && isPtValid(b))
+                                 return true;  // b is considered greater when a is nullptr
+                               else if (isPtValid(a) && !isPtValid(b))
+                                 return false;   // a is considered greater when b is nullptr
+                               else if (!isPtValid(a) && !isPtValid(b))
+                                 return true;  // Both are nullptr, no preference
+
+                               if (a->DepthGap < b->DepthGap && b->DepthGap > this->EdgeDepthGapThreshold)
+                                 return true;
+                               if (b->DepthGap < a->DepthGap && a->DepthGap > this->EdgeDepthGapThreshold)
+                                 return false;
+
+                               if (a->Angle < b->Angle && b->Angle < -this->PlaneCosAngleThreshold && b->Angle > this->EdgeCosAngleThreshold)
                                   return true;
                                 if (b->Angle < a->Angle && a->Angle < -this->PlaneCosAngleThreshold && a->Angle > this->EdgeCosAngleThreshold)
                                   return false;
 
-                                if (a->SpaceGap < b->SpaceGap && b->SpaceGap > this->EdgeDepthGapThreshold)
-                                  return true;
-                                if (b->SpaceGap < a->SpaceGap && a->SpaceGap > this->EdgeDepthGapThreshold)
-                                  return false;
+                               if (a->SpaceGap < b->SpaceGap && b->SpaceGap > this->EdgeDepthGapThreshold)
+                                 return true;
+                               if (b->SpaceGap < a->SpaceGap && a->SpaceGap > this->EdgeDepthGapThreshold)
+                                 return false;
 
-                                return true;
-                              });
+                               return true;
+                             });
+      break;
+    }
+  }
 }
 
 //-----------------------------------------------------------------------------
@@ -650,23 +755,57 @@ void DenseSpinningSensorKeypointExtractor::ComputeIntensityEdges()
            (!pt->KptTypes[static_cast<int>(EDGE)] && !pt->KptTypes[static_cast<int>(INTENSITY_EDGE)]) &&
            (pt->IntensityGap - (-1.0f) > 1e-6 && pt->IntensityGap > this->EdgeIntensityGapThreshold);
   };
-  this->CreatePatchGrid(isPtValid);
-  this->AddKptsUsingPatchGrid(Keypoint::INTENSITY_EDGE,
-                              [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
-                              {
-                                if (!isPtValid(a) && isPtValid(b))
-                                  return true;  // b is considered greater when a is nullptr
-                                else if (isPtValid(a) && !isPtValid(b))
-                                  return false;   // a is considered greater when b is nullptr
-                                else if (!isPtValid(a) && !isPtValid(b))
-                                  return true;  // Both are nullptr, no preference
-
-                                if (a->IntensityGap < b->IntensityGap && b->IntensityGap > this->EdgeIntensityGapThreshold)
-                                  return true;
-                                if (b->IntensityGap < a->IntensityGap && a->IntensityGap > this->EdgeIntensityGapThreshold)
-                                  return false;
-                                return true;
-                              });
+  switch (this->SamplingDSSKE)
+  {
+    // If Patch mode activated : use a 2D grid built on vertex map
+    // to extract keypoints
+    case SamplingModeDSSKE::PATCH:
+    {
+      this->Create2DGrid(isPtValid);
+      this->AddKptsUsingGrid(Keypoint::INTENSITY_EDGE,
+                             [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
+                             {
+                               if (!isPtValid(a) && isPtValid(b))
+                                 return true;  // b is considered greater when a is nullptr
+                               else if (isPtValid(a) && !isPtValid(b))
+                                 return false;   // a is considered greater when b is nullptr
+                               else if (!isPtValid(a) && !isPtValid(b))
+                                 return true;  // Both are nullptr, no preference
+
+                               if (a->IntensityGap < b->IntensityGap && b->IntensityGap > this->EdgeIntensityGapThreshold)
+                                 return true;
+                               if (b->IntensityGap < a->IntensityGap && a->IntensityGap > this->EdgeIntensityGapThreshold)
+                                 return false;
+
+                               return true;
+                             });
+      break;
+    }
+    // If Voxel mode activated : use a 3D grid built on scan cloud
+    // to extract keypoints
+    case SamplingModeDSSKE::VOXEL:
+    {
+      this->Create3DGrid(isPtValid);
+      this->AddKptsUsingGrid(Keypoint::INTENSITY_EDGE,
+                             [&](const std::shared_ptr<PtFeat>& a, const std::shared_ptr<PtFeat>& b)
+                             {
+                               if (!isPtValid(a) && isPtValid(b))
+                                 return true;  // b is considered greater when a is nullptr
+                               else if (isPtValid(a) && !isPtValid(b))
+                                 return false;   // a is considered greater when b is nullptr
+                               else if (!isPtValid(a) && !isPtValid(b))
+                                 return true;  // Both are nullptr, no preference
+
+                               if (a->IntensityGap < b->IntensityGap && b->IntensityGap > this->EdgeIntensityGapThreshold)
+                                 return true;
+                               if (b->IntensityGap < a->IntensityGap && a->IntensityGap > this->EdgeIntensityGapThreshold)
+                                 return false;
+
+                               return true;
+                             });
+      break;
+    }
+  }
 }
 
 //-----------------------------------------------------------------------------
@@ -685,9 +824,9 @@ std::unordered_map<std::string, std::vector<float>> DenseSpinningSensorKeypointE
   for (int i = 0; i < this->Scan->size(); i++)
   {
     const std::shared_ptr<PtFeat>& ptrFeat = this->GetPtFeat(i);
-    debugArray["space_gap"][i]               = ptrFeat->SpaceGapH;
-    debugArray["depth_gap"][i]               = ptrFeat->DepthGapH;
-    debugArray["intensity_gap"][i]           = ptrFeat->IntensityGapH;
+    debugArray["space_gap"][i]               = ptrFeat->SpaceGap;
+    debugArray["depth_gap"][i]               = ptrFeat->DepthGap;
+    debugArray["intensity_gap"][i]           = ptrFeat->IntensityGap;
     debugArray["cos_angle"][i]               = ptrFeat->Angle;
     debugArray["edge_keypoint"][i]           = ptrFeat->KptTypes[static_cast<int>(Keypoint::EDGE)];
     debugArray["plane_keypoint"][i]          = ptrFeat->KptTypes[static_cast<int>(Keypoint::PLANE)];
-- 
GitLab


From d0d5a52cf8afadbfacea22f0822a36ca9773ab00 Mon Sep 17 00:00:00 2001
From: "julia.sanchez" <julia.sanchez@kitware.com>
Date: Tue, 6 Feb 2024 10:07:11 +0100
Subject: [PATCH 13/13] [PV] Update PV wrapper with new extractor

---
 .../Plugin/vtkLidarSlam/Slam.xml              | 49 +++++++++++++------
 .../vtkSpinningSensorKeypointExtractor.cxx    | 12 +++--
 .../vtkSpinningSensorKeypointExtractor.h      | 24 +++++++--
 3 files changed, 61 insertions(+), 24 deletions(-)

diff --git a/paraview_wrapping/Plugin/vtkLidarSlam/Slam.xml b/paraview_wrapping/Plugin/vtkLidarSlam/Slam.xml
index 8a63db703..beb1cec9a 100644
--- a/paraview_wrapping/Plugin/vtkLidarSlam/Slam.xml
+++ b/paraview_wrapping/Plugin/vtkLidarSlam/Slam.xml
@@ -2477,6 +2477,23 @@
         Extract edges, planes and blobs keypoints from a spinning LiDAR sensor pointcloud.
       </Documentation>
 
+      <IntVectorProperty name="Mode"
+                         command="SetMode"
+                         number_of_elements="1"
+                         default_values="0">
+        <EnumerationDomain name="enum">
+          <Entry value="0" text="Sparse"/>
+          <Entry value="1" text="Dense"/>
+        </EnumerationDomain>
+        <Documentation>
+          How to extract the keypoints.
+
+          If DENSE, a vertex map is built to analyse the frame.
+
+          If SPARSE, the lines are analysed independently.
+        </Documentation>
+      </IntVectorProperty>
+
       <IntVectorProperty name="Min neighbors nb"
                          command="SetMinNeighNb"
                          number_of_elements="1"
@@ -2558,29 +2575,28 @@
         </Documentation>
       </DoubleVectorProperty>
 
-      <DoubleVectorProperty name="Plane max sinus angle"
-                            command="SetPlaneSinAngleThreshold"
+      <DoubleVectorProperty name="Plane threshold angle"
+                            command="SetPlaneAngleThreshold"
                             number_of_elements="1"
-                            default_values="0.5"
+                            default_values="150."
                             panel_visibility="advanced">
         <Documentation>
-          One strategy to consider a point as a planar keypoint is to compute
-          the angle between the two lines that fit its previous and next neighborhoods.
-          If the angle is close enough to 0° or 180° (sin(Angle) low enough), we consider
-          the point as a planar keypoint.
+          Threshold upon angle in degrees between left and right lines to consider a point as plane.
+          It is used with its absolute sine value in SSKE for planes,
+          with its cosine value in DSSKE for planes and
+          with its opposite cosine value in DSSKE to filter too sharp edges.
         </Documentation>
       </DoubleVectorProperty>
 
-      <DoubleVectorProperty name="Edge min sinus angle"
-                            command="SetEdgeSinAngleThreshold"
+      <DoubleVectorProperty name="Edge threshold angle"
+                            command="SetEdgeAngleThreshold"
                             number_of_elements="1"
-                            default_values="0.86"
+                            default_values="120."
                             panel_visibility="advanced">
         <Documentation>
-          One strategy to consider a point as an edge keypoint is to compute
-          the angle between the two lines that fit its previous and next neighborhoods.
-          If the angle is close enough to 90° (sin(Angle) high enough), we consider
-          the point as an edge keypoint.
+          Threshold upon angle in degrees between left and right lines to consider a point as edge.
+          Used with its absolute sine value in SSKE for edges and
+          with its cosine value in DSSKE for edges.
         </Documentation>
       </DoubleVectorProperty>
 
@@ -2655,6 +2671,7 @@
       </DoubleVectorProperty>
 
       <PropertyGroup label="Spinning Sensor Keypoints Extractor parameters">
+        <Property name="Mode" />
         <Property name="Min neighbors nb" />
         <Property name="Min neighborhood radius" />
         <Property name="Min distance to sensor" />
@@ -2662,8 +2679,8 @@
         <Property name="Min azimuth angle" />
         <Property name="Max azimuth angle" />
         <Property name="Min laser beam to surface angle" />
-        <Property name="Plane max sinus angle" />
-        <Property name="Edge min sinus angle" />
+        <Property name="Plane threshold angle" />
+        <Property name="Edge threshold angle" />
         <Property name="Edge min depth gap" />
         <Property name="Edge min nb of missing points" />
         <Property name="Edge min intensity gap" />
diff --git a/paraview_wrapping/Plugin/vtkLidarSlam/vtkSpinningSensorKeypointExtractor.cxx b/paraview_wrapping/Plugin/vtkLidarSlam/vtkSpinningSensorKeypointExtractor.cxx
index 3428009b9..db17491fb 100644
--- a/paraview_wrapping/Plugin/vtkLidarSlam/vtkSpinningSensorKeypointExtractor.cxx
+++ b/paraview_wrapping/Plugin/vtkLidarSlam/vtkSpinningSensorKeypointExtractor.cxx
@@ -26,8 +26,12 @@ vtkStandardNewMacro(vtkSpinningSensorKeypointExtractor)
 
 //-----------------------------------------------------------------------------
 vtkSpinningSensorKeypointExtractor::vtkSpinningSensorKeypointExtractor()
-  : Extractor(std::make_shared<LidarSlam::SpinningSensorKeypointExtractor>())
-{}
+{
+  if (this->Mode != LidarSlam::KeypointExtractorMode::DENSE)
+    this->Extractor = std::make_shared<LidarSlam::SpinningSensorKeypointExtractor>();
+  else
+    this->Extractor = std::make_shared<LidarSlam::DenseSpinningSensorKeypointExtractor>();
+}
 
 void vtkSpinningSensorKeypointExtractor::PrintSelf(std::ostream& os, vtkIndent indent)
 {
@@ -46,9 +50,9 @@ void vtkSpinningSensorKeypointExtractor::PrintSelf(std::ostream& os, vtkIndent i
   PrintParameter(AzimuthMin)
   PrintParameter(AzimuthMax)
 
-  PrintParameter(PlaneSinAngleThreshold)
+  PrintParameter(PlaneAngleThreshold)
 
-  PrintParameter(EdgeSinAngleThreshold)
+  PrintParameter(EdgeAngleThreshold)
   PrintParameter(EdgeDepthGapThreshold)
   PrintParameter(EdgeIntensityGapThreshold)
   PrintParameter(EdgeNbGapPoints)
diff --git a/paraview_wrapping/Plugin/vtkLidarSlam/vtkSpinningSensorKeypointExtractor.h b/paraview_wrapping/Plugin/vtkLidarSlam/vtkSpinningSensorKeypointExtractor.h
index 6dc011a25..33d964617 100644
--- a/paraview_wrapping/Plugin/vtkLidarSlam/vtkSpinningSensorKeypointExtractor.h
+++ b/paraview_wrapping/Plugin/vtkLidarSlam/vtkSpinningSensorKeypointExtractor.h
@@ -21,6 +21,7 @@
 #define VTK_SPINNING_SENSOR_KEYPOINT_EXTRACTOR_H
 
 #include <LidarSlam/SpinningSensorKeypointExtractor.h>
+#include <LidarSlam/DenseSpinningSensorKeypointExtractor.h>
 #include <vtkObject.h>
 
 //
@@ -72,9 +73,9 @@ public:
 
   vtkCustomSetMacro(MinBeamSurfaceAngle, float)
 
-  vtkCustomSetMacro(PlaneSinAngleThreshold, float)
+  vtkCustomSetMacro(PlaneAngleThreshold, float)
 
-  vtkCustomSetMacro(EdgeSinAngleThreshold, float)
+  vtkCustomSetMacro(EdgeAngleThreshold, float)
 
   vtkCustomSetMacro(EdgeDepthGapThreshold, float)
 
@@ -82,16 +83,31 @@ public:
 
   vtkCustomSetMacro(EdgeNbGapPoints, int)
 
-  std::shared_ptr<LidarSlam::SpinningSensorKeypointExtractor> GetExtractor() const { return Extractor; }
+  void SetMode(int mode)
+  {
+    vtkDebugMacro(<< this->GetClassName() << " (" << this << "): setting mode to " << mode);
+    LidarSlam::KeypointExtractorMode extractMode = static_cast<LidarSlam::KeypointExtractorMode>(mode);
+    if (this->Mode == extractMode)
+      return;
+
+    if (this->Mode != LidarSlam::KeypointExtractorMode::DENSE)
+      this->Extractor = std::make_shared<LidarSlam::SpinningSensorKeypointExtractor>();
+    else
+      this->Extractor = std::make_shared<LidarSlam::DenseSpinningSensorKeypointExtractor>();
+  }
+
+  std::shared_ptr<LidarSlam::KeypointExtractor> GetExtractor() const { return Extractor; }
 
 protected:
   vtkSpinningSensorKeypointExtractor();
 
-  std::shared_ptr<LidarSlam::SpinningSensorKeypointExtractor> Extractor;
+  std::shared_ptr<LidarSlam::KeypointExtractor> Extractor;
 
 private:
   vtkSpinningSensorKeypointExtractor(const vtkSpinningSensorKeypointExtractor&) = delete;
   void operator=(const vtkSpinningSensorKeypointExtractor&) = delete;
+
+  LidarSlam::KeypointExtractorMode Mode = LidarSlam::KeypointExtractorMode::SPARSE;
 };
 
 #endif // VTK_SPINNING_SENSOR_KEYPOINT_EXTRACTOR_H
-- 
GitLab