diff --git a/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h b/slam_lib/include/LidarSlam/SpinningSensorKeypointExtractor.h
index a3ebc21d66b38de813ea8d95fc9914e7c2e26a18..ca7d74a1a8ca25622d4339ed617a9fe90ea64974 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
 {
diff --git a/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx b/slam_lib/src/DenseSpinningSensorKeypointExtractor.cxx
index e6ebc5672e964ea1fd160cab891d9ccf449311d4..9ecfe91c198603c176133b1b01108ba5a8a100a2 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 e27a4e4c84e3abe3a15bf9aed7a1520a77d4069a..782f450243fa3a82ce4367bc66bcb024b5ca1096 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();
 }