Merge pull request #11 from MOLAorg/10-bad-first-icp-re-starting-from…

…-scratch-with-a-new-local-map Fix NaN pointcloud radius in doInitializeEstimatedMaxSensorRange()
MOLAorg · Jan 11, 2025 · 8cc4efd · 8cc4efd
2 parents 8b21ea1 + feb422f
commit 8cc4efd
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/module/src/LidarOdometry.cpp b/module/src/LidarOdometry.cpp
@@ -1577,7 +1577,13 @@ void LidarOdometry::doInitializeEstimatedMaxSensorRange(const mrpt::obs::CObserv
   const auto bb = pts->boundingBox();
   double radius = std::max(bb.max.norm(), bb.min.norm());
 
-  mrpt::keep_max(radius, params_.absolute_minimum_sensor_range);
+  // check for NaN: See: https://github.com/MOLAorg/mola_lidar_odometry/issues/10
+  if (radius != radius) {
+    MRPT_LOG_WARN_STREAM("NaN bounding box for sensor point cloud. Using default sensor range.");
+    radius = params_.absolute_minimum_sensor_range;
+  } else {
+    mrpt::keep_max(radius, params_.absolute_minimum_sensor_range);
+  }
 
   maxRange = radius;