ignore measurements outside lidar range

According to the comment in sensor_msgs/LaserScan, values
< range_min or > range_max should be discarded.
Without this, scan matching will fail for lidars publishing
0 for invalid measurements.

Author: 981213

lib/karto_sdk/include/karto_sdk/Karto.h

@@ -5658,27 +5658,21 @@ class LocalizedRangeScan : public LaserRangeScan
       kt_int32u beamNum = 0;
       for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++) {
         kt_double rangeReading = GetRangeReadings()[i];
-        if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold)) {
-          kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
-
-          Vector2<kt_double> point;
-          point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
-          point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
-
-          m_UnfilteredPointReadings.push_back(point);
+        if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(),
+                           pLaserRangeFinder->GetMaximumRange()))
           continue;
-        }
 
         kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
 
         Vector2<kt_double> point;
         point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
         point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
 
-        m_PointReadings.push_back(point);
+        if (rangeReading < rangeThreshold) {
+            m_PointReadings.push_back(point);
+            rangePointsSum += point;
+        }
         m_UnfilteredPointReadings.push_back(point);
-
-        rangePointsSum += point;
       }
 
       // compute barycenter