From 03cdb0080f4ca501f63cb1673dc1c7beecb733df Mon Sep 17 00:00:00 2001
From: Tong Fu <tong.fu@kitware.com>
Date: Fri, 24 Jan 2025 15:37:33 +0100
Subject: [PATCH 1/2] [minor][fix] Remove double header file

---
 slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
index a3ebc21d6..ca7d74a1a 100644
--- a/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
+++ b/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
@@ -30,7 +30,6 @@
 #include <unordered_map>
 #include <map>
 #include <bitset>
-#include <map>
 
 namespace LidarSlam
 {
-- 
GitLab


From d448f4d0b309698603a53d078de840adbf020839 Mon Sep 17 00:00:00 2001
From: Tong Fu <tong.fu@kitware.com>
Date: Fri, 24 Jan 2025 15:41:29 +0100
Subject: [PATCH 2/2] [fix] Check nan value for azimuthal resolution

---
 slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx | 6 +++---
 slam_lib/src/SpinningSensorKeypointExtractor.cxx      | 3 ++-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
index e6ebc5672..9ecfe91c1 100644
--- a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
@@ -24,9 +24,9 @@
 
 #include <pcl/common/common.h>
 
-#include <random>
-
+#include <cmath>
 #include <fstream>
+#include <random>
 
 namespace LidarSlam
 {
@@ -108,7 +108,7 @@ void DenseSpinningSensorKeypointExtractor::InitInternalParameters()
   // 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)
+  if (this->AzimuthalResolution < 1e-6 || M_PI/4. < this->AzimuthalResolution || std::isnan(this->AzimuthalResolution))
     this->EstimateAzimuthalResolution();
 
   // Compute the indices of scan points in the future vertex map
diff --git a/slam_lib/src/SpinningSensorKeypointExtractor.cxx b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
index e27a4e4c8..782f45024 100644
--- a/slam_lib/src/SpinningSensorKeypointExtractor.cxx
+++ b/slam_lib/src/SpinningSensorKeypointExtractor.cxx
@@ -26,6 +26,7 @@
 
 #include <pcl/common/common.h>
 
+#include <cmath>
 #include <random>
 
 namespace LidarSlam
@@ -92,7 +93,7 @@ void SpinningSensorKeypointExtractor::ConvertAndSortScanLines()
   // 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)
+  if (this->AzimuthalResolution < 1e-6 || M_PI/4. < this->AzimuthalResolution || std::isnan(this->AzimuthalResolution))
     this->EstimateAzimuthalResolution();
 }
 
-- 
GitLab