From 52acc428a09291d38855d341bc2f4f6939748365 Mon Sep 17 00:00:00 2001 From: Chuanhong Guo Date: Tue, 25 Oct 2022 20:36:02 +0800 Subject: [PATCH] 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. --- lib/karto_sdk/include/karto_sdk/Karto.h | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/lib/karto_sdk/include/karto_sdk/Karto.h b/lib/karto_sdk/include/karto_sdk/Karto.h index c03f6757..36b6978d 100644 --- a/lib/karto_sdk/include/karto_sdk/Karto.h +++ b/lib/karto_sdk/include/karto_sdk/Karto.h @@ -5658,16 +5658,9 @@ 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 point; - point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); - point.SetY(scanPose.GetY() + (rangeReading * sin(angle))); - - m_UnfilteredPointReadings.push_back(point); + if (rangeReading < pLaserRangeFinder->GetMinimumRange() || + rangeReading > pLaserRangeFinder->GetMaximumRange()) continue; - } kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution; @@ -5675,10 +5668,11 @@ class LocalizedRangeScan : public LaserRangeScan 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