fix: error in point cloud conversion (#166)

* fix: error in point cloud convertion

* fix: pre-commit

* fix: apply improvements

packages/icp_odometry/src/conversion.cpp

@@ -1,5 +1,7 @@
 #include <icp_odometry/conversion.h>
 
+#include "common/math.h"
+
 namespace truck::icp_odometry {
 
 namespace {
@@ -9,29 +11,35 @@ constexpr bool isBigendian() { return __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; }
 }  // namespace
 
 DataPoints toDataPoints(const sensor_msgs::msg::LaserScan& scan) {
+    Limits range_limit{scan.range_min, scan.range_max};
+
     DataPoints::Labels feature_labels;
     feature_labels.push_back(DataPoints::Label("x", 1));
     feature_labels.push_back(DataPoints::Label("y", 1));
     feature_labels.push_back(DataPoints::Label("w", 1));
 
     const DataPoints::Labels descriptor_labels;
 
+    auto is_valid = [&](double range) { return std::isnormal(range) && range_limit.isMet(range); };
+
     const size_t point_count = std::count_if(
-        scan.ranges.begin(), scan.ranges.end(), [](float range) { return std::isfinite(range); });
+        scan.ranges.begin(), scan.ranges.end(), [&](float range) { return is_valid(range); });
 
     DataPoints result(feature_labels, descriptor_labels, point_count);
 
-    for (size_t i = 0; i < scan.ranges.size(); ++i) {
+    for (size_t i = 0, j = 0; i < scan.ranges.size(); ++i) {
         const double range = scan.ranges[i];
-        if (!std::isfinite(range)) {
+
+        if (!is_valid(range)) {
             continue;
         }
 
         const double angle = scan.angle_min + i * scan.angle_increment;
 
-        result.features(0, i) = range * std::cos(angle);
-        result.features(1, i) = range * std::sin(angle);
-        result.features(2, i) = 1.0f;
+        result.features(0, j) = range * std::cos(angle);
+        result.features(1, j) = range * std::sin(angle);
+        result.features(2, j) = 1.0f;
+        j++;
     }
 
     return result;