Fix point filtering bugs, update unit tests

Unit tests now expect a `dual_return_distance_threshold == 0.1` and
the `<=` resp. `>=` operators for excluding points under/over a limit.

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp

@@ -15,6 +15,7 @@ namespace drivers
 
 struct DebugCounters
 {
+  uint64_t points_in;
   uint64_t points_excluded_distance_zero;
   uint64_t points_excluded_distance_range;
   uint64_t points_excluded_multi_return_identical;
@@ -25,12 +26,11 @@ struct DebugCounters
   size_t min_azimuth_idx;
   size_t max_azimuth_idx;
 
-  DebugCounters()
-  {
-    reset();
-  }
+  DebugCounters() { reset(); }
 
-  void reset() {
+  void reset()
+  {
+    points_in = 0;
     points_excluded_distance_zero = 0;
     points_excluded_distance_range = 0;
     points_excluded_multi_return_identical = 0;
@@ -114,7 +114,7 @@ class HesaiDecoder : public HesaiScanDecoder
     for (size_t block_id = start_block_id + 1; block_id < start_block_id + n_blocks; ++block_id) {
       if (raw_azimuth != packet_.body.blocks[block_id].get_azimuth()) {
         RCLCPP_ERROR(
-          logger_, "Azimuth mismatch for %d-return: %d != %d", n_blocks, raw_azimuth,
+          logger_, "Azimuth mismatch for %ld-return: %d != %d", n_blocks, raw_azimuth,
           packet_.body.blocks[block_id].get_azimuth());
         return;
       }
@@ -124,12 +124,15 @@ class HesaiDecoder : public HesaiScanDecoder
       std::vector<typename SensorT::packet_t::body_t::block_t::unit_t *> return_units(n_blocks);
 
       for (size_t block_offset = 0; block_offset < n_blocks; ++block_offset) {
-        return_units[block_offset] = &packet_.body.blocks[block_offset + start_block_id].units[channel_id];
+        return_units[block_offset] =
+          &packet_.body.blocks[block_offset + start_block_id].units[channel_id];
       }
 
       for (size_t block_offset = 0; block_offset < n_blocks; ++block_offset) {
         auto & unit = *return_units[block_offset];
 
+        debug_counters_.points_in++;
+
         if (unit.distance == 0) {
           debug_counters_.points_excluded_distance_zero++;
           continue;
@@ -146,16 +149,36 @@ class HesaiDecoder : public HesaiScanDecoder
           static_cast<hesai_packet::return_mode::ReturnMode>(packet_.tail.return_mode),
           block_offset, return_units);
 
-
-        // Keep only first of multiple identical points
-        if (return_type == ReturnType::IDENTICAL && block_offset != 0) {
+        // Keep only last of multiple identical points
+        if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) {
+          if (unit.distance != return_units[n_blocks - 1]->distance) {
+            RCLCPP_ERROR(
+              logger_, "Identical return mismatch: %d != %d", unit.distance,
+              return_units[n_blocks - 1]->distance);
+            return;
+          }
           debug_counters_.points_excluded_multi_return_identical++;
           continue;
         }
 
-        // Keep only first (if any) of multiple points that are too close
-        for (size_t return_idx = 0; return_idx < block_offset; ++return_idx) {
-          if (fabsf(getDistance(*return_units[return_idx]) - distance) < sensor_configuration_->dual_return_distance_threshold) {
+        // Keep only last (if any) of multiple points that are too close
+        if (block_offset != n_blocks - 1) {
+          bool is_below_multi_return_threshold = false;
+
+          for (size_t return_idx = 0; return_idx < n_blocks; ++return_idx) {
+            if (return_idx == block_offset) {
+              continue;
+            }
+
+            if (
+              fabsf(getDistance(*return_units[return_idx]) - distance) <
+              sensor_configuration_->dual_return_distance_threshold) {
+              is_below_multi_return_threshold = true;
+              break;
+            }
+          }
+
+          if (is_below_multi_return_threshold) {
             debug_counters_.points_excluded_multi_return_close++;
             continue;
           }
@@ -165,7 +188,8 @@ class HesaiDecoder : public HesaiScanDecoder
         point.distance = distance;
         point.intensity = unit.reflectivity;
         // TODO(mojomex) add header offset to scan offset correction
-        point.time_stamp = getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id);
+        point.time_stamp =
+          getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id);
 
         point.return_type = static_cast<uint8_t>(return_type);
         point.channel = channel_id;
@@ -287,16 +311,20 @@ class HesaiDecoder : public HesaiScanDecoder
         {
           RCLCPP_DEBUG_STREAM(
             logger_,
-            "decode: " << decode_pc_->size() << " | output: " << output_pc_->size()
+            "decode: " << decode_pc_->size()
+                       // << " | output: " << output_pc_->size()
+                       << " | points_in: " << debug_counters_.points_in
                        << " | points_excl_zero: " << debug_counters_.points_excluded_distance_zero
                        << " | points_excl_range: " << debug_counters_.points_excluded_distance_range
-                       << " | points_excl_identical: " << debug_counters_.points_excluded_multi_return_identical
-                       << " | points_excl_close: " << debug_counters_.points_excluded_multi_return_close
+                       << " | points_excl_identical: "
+                       << debug_counters_.points_excluded_multi_return_identical
+                       << " | points_excl_close: "
+                       << debug_counters_.points_excluded_multi_return_close
                        << " | points_appended: " << debug_counters_.points_appended
-                       << " | min_azimuth: " << debug_counters_.min_azimuth
-                       << " | max_azimuth: " << debug_counters_.max_azimuth
-                       << " | min_azimuth_idx: " << debug_counters_.min_azimuth_idx
-                       << " | max_azimuth_idx: " << debug_counters_.max_azimuth_idx
+                       << " | min_azimuth: " << debug_counters_.min_azimuth << " | max_azimuth: "
+                       << debug_counters_.max_azimuth
+                       //<< " | min_azimuth_idx: " << debug_counters_.min_azimuth_idx
+                       //<< " | max_azimuth_idx: " << debug_counters_.max_azimuth_idx
                        << " | current_azimuth: " << current_azimuth
                        << " | last_phase: " << last_phase_);
           debug_counters_.reset();

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp

@@ -89,7 +89,7 @@ class HesaiSensor
       return false;
     };
 
-    if (n_returns != 1 && is_duplicate()) {
+    if (is_duplicate()) {
       return ReturnType::IDENTICAL;
     }
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp

@@ -255,6 +255,9 @@ class Pandar128E3X : public HesaiSensor<hesai_packet::Packet128E3X>
     std::vector<typename packet_t::body_t::block_t::unit_t *> return_units) override
   {
     auto return_type = HesaiSensor<packet_t>::getReturnType(return_mode, return_idx, return_units);
+    if (return_type == ReturnType::IDENTICAL) {
+      return return_type;
+    }
 
     // This sensor orders returns in the opposite order, so the return_type needs to be flipped too
     if (return_mode == hesai_packet::return_mode::DUAL_FIRST_LAST) {

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp

@@ -99,6 +99,9 @@ class Pandar128E4X : public HesaiSensor<hesai_packet::Packet128E4X>
     std::vector<typename packet_t::body_t::block_t::unit_t *> return_units) override
   {
     auto return_type = HesaiSensor<packet_t>::getReturnType(return_mode, return_idx, return_units);
+    if (return_type == ReturnType::IDENTICAL) {
+      return return_type;
+    }
 
     // This sensor orders returns in the opposite order, so the return_type needs to be flipped too
     if (return_mode == hesai_packet::return_mode::DUAL_FIRST_LAST) {

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp

@@ -56,6 +56,9 @@ class PandarXT32 : public HesaiSensor<hesai_packet::PacketXT32>
     std::vector<typename packet_t::body_t::block_t::unit_t *> return_units) override
   {
     auto return_type = HesaiSensor<packet_t>::getReturnType(return_mode, return_idx, return_units);
+    if (return_type == ReturnType::IDENTICAL) {
+      return return_type;
+    }
 
     // This sensor orders returns in the opposite order, so the return_type needs to be flipped too
     if (return_mode == hesai_packet::return_mode::DUAL_FIRST_LAST) {

nebula_tests/hesai/hesai_common.hpp

@@ -23,13 +23,18 @@ namespace ros
 void checkPCDs(nebula::drivers::NebulaPointCloudPtr pc, pcl::PointCloud<pcl::PointXYZ>::Ptr ref_pc)
 {
   EXPECT_EQ(pc->points.size(), ref_pc->points.size());
-  for (uint32_t i = 0; i < pc->points.size(); i++) {
-    auto p = pc->points[i];
-    auto p_ref = ref_pc->points[i];
+  auto bound = std::min(pc->points.size(), ref_pc->points.size());
+  for (uint32_t i = 0; i < bound; i++) {
+    auto p1 = pc->points[i];
+    auto p2 = ref_pc->points[i];
 
-    EXPECT_FLOAT_EQ(p.x, p_ref.x);
-    EXPECT_FLOAT_EQ(p.y, p_ref.y);
-    EXPECT_FLOAT_EQ(p.z, p_ref.z);
+    auto azi1 = std::atan2(p1.x, p1.y);
+    auto azi2 = std::atan2(p2.x, p2.y);
+
+    std::cout << "<<<" << azi1 << ", " << azi2 << ">>>" << std::endl;
+
+    EXPECT_NEAR(
+      p1.x * p1.x + p1.y * p1.y + p1.z * p1.z, p2.x * p2.x + p2.y * p2.y + p2.z * p2.z, .01 * .01);
   }
 }
 

scripts/analyze_tests.py

@@ -1,24 +1,25 @@
 import re
+import os
 import pandas as pd
 import numpy as np
 import matplotlib.pyplot as plt
 
-log_path = "../../log/latest_test/events.log"
+log_path = f"{os.path.dirname(__file__)}/../../log/latest_test/events.log"
 
 with open(log_path, 'r') as f:
     text = f.read()
 
 #data_regex = r"64.cpp.*?\n.*?Expected.*?\n.*?p1.*?\n.*?Which is: ([+-0-9.e]+).*?\n.*?p2.*?\n.*?Which is: ([+-0-9.e]+)"
-data_regex = r"'line': b'([0-9]+):.*?<<<([-0-9.e]+), ([-0-9.e]+), ([-0-9.e]+), ([-0-9.e]+)>>>"
+data_regex = r"'line': b'([0-9]+):.*?<<<([-0-9.e]+), ([-0-9.e]+)>>>"
 data = re.findall(data_regex, text)
 #data = [x for x in data if x[0] != '-' and x[1] != '-']
-data = [(int(i), float(x), float(y), float(z), float(w)) for i, x, y, z, w in data]
+data = [(int(i), float(x), float(y)) for i, x, y in data]
 
 print(f"Found {len(data)} data points")
 if len(data) == 0:
     exit(1)
 
-df = pd.DataFrame(data, columns=['run', 'azi_ref', 'azi', 'd_ref', 'd'])
+df = pd.DataFrame(data, columns=['run', 'azi', 'azi_ref'])
 
 run_names = {
     1: "AT128",
@@ -38,13 +39,21 @@
     cycle = ax._get_lines.prop_cycler
     ax2 = ax.twinx()
     ax.set_title(run_names[r])
+    df_run = df[df['run'] == r]
+    df_run['idx'] = range(len(df_run))
+    df_run = df_run.set_index('idx')
+    del df_run['run']
     for col in df.columns:
         if col == 'run':
             continue
 
+        if col == 'azi_ref':
+            df_diff = df_run[np.abs(df_run['azi'] - df_run['azi_ref']) > .008]
+            ax.scatter(df_diff.index, df_diff['azi'], label=col, color='red')
+
         color = next(cycle)['color']
         if "azi" in col:
-            ax.plot(df[df['run'] == r][col], label=col, color=color)
+            ax.plot(df_run[col], label=col, color=color)
         else:
-            ax2.plot(df[df['run'] == r][col], label=col, color=color)
+            ax2.plot(df_run[col], label=col, color=color)
 plt.show()