From f6fd8f479956e6937316c6ce49ef0776c0c07c00 Mon Sep 17 00:00:00 2001
From: Benedikt Schwab <benedikt.schwab@gmail.com>
Date: Thu, 12 Dec 2024 11:59:58 +0100
Subject: [PATCH] added optional lidar sensor id remapping per camera view

---
 a2d2_ros_preparer/src/converter.cpp           |  1 +
 .../src/lidar/lidar_scan_stream.cpp           | 10 +++++-
 .../src/lidar/lidar_scan_stream.h             |  1 +
 .../lidar/lidar_scan_timeseries_per_view.cpp  |  9 +++--
 .../lidar/lidar_scan_timeseries_per_view.h    |  2 ++
 .../src/lidar/npz_file_reader.cpp             | 18 +++++++++-
 a2d2_ros_preparer/src/lidar/npz_file_reader.h |  2 +-
 a2d2_ros_preparer/src/options.h               |  9 +++++
 a2d2_ros_preparer/src/options_parser.cpp      | 36 +++++++++++++++++++
 a2d2_ros_preparer/src/options_parser.h        |  1 +
 10 files changed, 83 insertions(+), 6 deletions(-)

diff --git a/a2d2_ros_preparer/src/converter.cpp b/a2d2_ros_preparer/src/converter.cpp
index ff85337..f5ca6b3 100644
--- a/a2d2_ros_preparer/src/converter.cpp
+++ b/a2d2_ros_preparer/src/converter.cpp
@@ -45,6 +45,7 @@ namespace a2d2_ros_preparer {
         auto lidar_scan_stream = LidarScanStream(vehicle_configuration.GetLidarDataDirectoryPath(),
                                                  vehicle_configuration.GetCameraIdentifiers(),
                                                  vehicle_configuration.GetLidarIdentifiers(),
+                                                 options.lidar_sensor_id_remappings(),
                                                  vehicle_configuration.GetCameraToBaseAffineTransforms(),
                                                  vehicle_configuration.GetLidarToBaseAffineTransforms(),
                                                  options.filter_start_timestamp(),
diff --git a/a2d2_ros_preparer/src/lidar/lidar_scan_stream.cpp b/a2d2_ros_preparer/src/lidar/lidar_scan_stream.cpp
index a92f0d9..4349852 100644
--- a/a2d2_ros_preparer/src/lidar/lidar_scan_stream.cpp
+++ b/a2d2_ros_preparer/src/lidar/lidar_scan_stream.cpp
@@ -28,6 +28,7 @@ namespace a2d2_ros_preparer {
     LidarScanStream::LidarScanStream(const std::filesystem::path& lidar_data_directory,
                                      const std::vector<CameraDirectionIdentifier>& camera_identifiers,
                                      std::vector<LidarDirectionIdentifier> lidar_identifiers,
+                                     const std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> sensor_id_remappings,
                                      std::map<CameraDirectionIdentifier, Eigen::Affine3d> camera_to_base_affine_transformation,
                                      std::map<LidarDirectionIdentifier, Eigen::Affine3d> lidar_to_base_affine_transformation,
                                      std::optional<Time> filter_start_timestamp,
@@ -42,7 +43,14 @@ namespace a2d2_ros_preparer {
                 continue;
             }
             auto directory_path = GetSubdirectoryPath(lidar_data_directory, current_camera_identifier);
-            auto sensor_data_timeseries = LidarScanTimeseriesPerView(directory_path, current_camera_identifier, filter_start_timestamp, filter_stop_timestamp);
+
+            std::map<uint64_t, uint64_t> current_sensor_id_remappings = {};
+            if (sensor_id_remappings.count(current_camera_identifier))
+            {
+                current_sensor_id_remappings = sensor_id_remappings.at(current_camera_identifier);
+            }
+
+            auto sensor_data_timeseries = LidarScanTimeseriesPerView(directory_path, current_camera_identifier, current_sensor_id_remappings, filter_start_timestamp, filter_stop_timestamp);
             lidar_scan_timeseries_per_view_.insert(std::make_pair(current_camera_identifier, sensor_data_timeseries));
         }
 
diff --git a/a2d2_ros_preparer/src/lidar/lidar_scan_stream.h b/a2d2_ros_preparer/src/lidar/lidar_scan_stream.h
index 4f8b98a..6b81071 100644
--- a/a2d2_ros_preparer/src/lidar/lidar_scan_stream.h
+++ b/a2d2_ros_preparer/src/lidar/lidar_scan_stream.h
@@ -34,6 +34,7 @@ namespace a2d2_ros_preparer {
         explicit LidarScanStream(const std::filesystem::path& lidar_data_directory,
                                  const std::vector<CameraDirectionIdentifier>& camera_identifiers,
                                  std::vector<LidarDirectionIdentifier> lidar_identifiers,
+                                 const std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> sensor_id_remappings,
                                  std::map<CameraDirectionIdentifier, Eigen::Affine3d> camera_to_base_affine_transformation,
                                  std::map<LidarDirectionIdentifier, Eigen::Affine3d> lidar_to_base_affine_transformation,
                                  std::optional<Time> filter_start_timestamp,
diff --git a/a2d2_ros_preparer/src/lidar/lidar_scan_timeseries_per_view.cpp b/a2d2_ros_preparer/src/lidar/lidar_scan_timeseries_per_view.cpp
index e54110e..c520a32 100644
--- a/a2d2_ros_preparer/src/lidar/lidar_scan_timeseries_per_view.cpp
+++ b/a2d2_ros_preparer/src/lidar/lidar_scan_timeseries_per_view.cpp
@@ -30,8 +30,11 @@ namespace a2d2_ros_preparer {
 
     LidarScanTimeseriesPerView::LidarScanTimeseriesPerView(const std::filesystem::path& directory_path,
                                                            const std::string& frame_id,
+                                                           const std::map<uint64_t, uint64_t>& sensor_id_remappings,
                                                            std::optional<Time> filter_start_timestamp,
-                                                           std::optional<Time> filter_stop_timestamp): frame_id_(frame_id) {
+                                                           std::optional<Time> filter_stop_timestamp):
+    frame_id_(frame_id), sensor_id_remappings_(sensor_id_remappings) {
+
         LOG(INFO) << "Reading lidar data for view: " << frame_id;
 
         // get all filepaths
@@ -77,7 +80,7 @@ namespace a2d2_ros_preparer {
         std::mutex m;
         std::for_each(std::execution::par_unseq, std::begin(filtered_filepaths), std::end(filtered_filepaths), [&](std::filesystem::path& current_filepath) {
             DataSequenceId current_id = ExtractLastIntegerFromString(current_filepath.stem());
-            auto point_cloud = ReadFile(current_filepath, frame_id_, current_id);
+            auto point_cloud = ReadFile(current_filepath, frame_id_, current_id, sensor_id_remappings_);
 
             auto time_min = point_cloud.GetStartTime();
             auto time_max = point_cloud.GetStopTime();
@@ -103,7 +106,7 @@ namespace a2d2_ros_preparer {
     TimedPointCloudData LidarScanTimeseriesPerView::GetTimedPointCloudData(DataSequenceId id) const {
         CHECK(HasData(id)) << "No lidar data for view '" << frame_id_ << "' and id '" << id << "'";
 
-        return ReadFile(filepaths_.at(id), frame_id_, id);
+        return ReadFile(filepaths_.at(id), frame_id_, id, sensor_id_remappings_);
     }
 
     std::vector<DataSequenceId> LidarScanTimeseriesPerView::GetDataSequenceIds() const {
diff --git a/a2d2_ros_preparer/src/lidar/lidar_scan_timeseries_per_view.h b/a2d2_ros_preparer/src/lidar/lidar_scan_timeseries_per_view.h
index 9806e09..7efbbc7 100644
--- a/a2d2_ros_preparer/src/lidar/lidar_scan_timeseries_per_view.h
+++ b/a2d2_ros_preparer/src/lidar/lidar_scan_timeseries_per_view.h
@@ -30,6 +30,7 @@ namespace a2d2_ros_preparer {
     public:
         explicit LidarScanTimeseriesPerView(const std::filesystem::path& directory_path,
                                             const std::string& frame_id,
+                                            const std::map<uint64_t, uint64_t>& sensor_id_remappings,
                                             std::optional<Time> filter_start_timestamp,
                                             std::optional<Time> filter_stop_timestamp);
 
@@ -54,6 +55,7 @@ namespace a2d2_ros_preparer {
     private:
         std::string frame_id_;
         std::map<DataSequenceId, std::filesystem::path> filepaths_;
+        const std::map<uint64_t, uint64_t> sensor_id_remappings_;
 
         std::map<DataSequenceId, Time> point_cloud_time_min_;
         std::map<DataSequenceId, Time> point_cloud_time_max_;
diff --git a/a2d2_ros_preparer/src/lidar/npz_file_reader.cpp b/a2d2_ros_preparer/src/lidar/npz_file_reader.cpp
index 0cca543..b746855 100644
--- a/a2d2_ros_preparer/src/lidar/npz_file_reader.cpp
+++ b/a2d2_ros_preparer/src/lidar/npz_file_reader.cpp
@@ -22,7 +22,7 @@
 
 namespace a2d2_ros_preparer {
 
-    TimedPointCloudData ReadFile(const std::filesystem::path& filepath, const std::string& view_id, const DataSequenceId& sequence_id) {
+    TimedPointCloudData ReadFile(const std::filesystem::path& filepath, const std::string& view_id, const DataSequenceId& sequence_id, const std::map<uint64_t, uint64_t>& sensor_id_remappings) {
         CHECK(exists(filepath)) << "File not found at: " << filepath;
 
         cnpy::npz_t npz_file = cnpy::npz_load(filepath);
@@ -41,6 +41,22 @@ namespace a2d2_ros_preparer {
         auto distance_data = npz_file["pcloud_attr.distance"].as_vec<double>();
 
         auto sensor_id_data = npz_file["pcloud_attr.lidar_id"].as_vec<uint64_t>();
+        // remapping the lidar sensor ids in case they have been provided inconsistently per camera view
+        if (!sensor_id_remappings.empty()) {
+            std::vector<uint64_t> remapped_sensor_id_data;
+            remapped_sensor_id_data.reserve(sensor_id_data.size());
+
+            for (const auto id : sensor_id_data) {
+                if (sensor_id_remappings.count(id) == 1) {
+                    remapped_sensor_id_data.push_back(sensor_id_remappings.at(id));
+                } else {
+                    remapped_sensor_id_data.push_back(id); // Retain original if no mapping exists
+                }
+            }
+
+            sensor_id_data = std::move(remapped_sensor_id_data);
+        }
+        
 
         std::vector<TimedRangefinderPoint> ranges;
         for (int i=0; i < number_points; i++)
diff --git a/a2d2_ros_preparer/src/lidar/npz_file_reader.h b/a2d2_ros_preparer/src/lidar/npz_file_reader.h
index 8677cb2..fa7a5ac 100644
--- a/a2d2_ros_preparer/src/lidar/npz_file_reader.h
+++ b/a2d2_ros_preparer/src/lidar/npz_file_reader.h
@@ -23,7 +23,7 @@
 
 namespace a2d2_ros_preparer {
 
-    TimedPointCloudData ReadFile(const std::filesystem::path& , const std::string& view_id, const DataSequenceId& sequence_id);
+    TimedPointCloudData ReadFile(const std::filesystem::path& , const std::string& view_id, const DataSequenceId& sequence_id, const std::map<uint64_t, uint64_t>& sensor_id_remappings = {});
 
 }
 
diff --git a/a2d2_ros_preparer/src/options.h b/a2d2_ros_preparer/src/options.h
index dc7ebc4..78e9fb9 100644
--- a/a2d2_ros_preparer/src/options.h
+++ b/a2d2_ros_preparer/src/options.h
@@ -58,6 +58,12 @@ namespace a2d2_ros_preparer {
         [[nodiscard]] std::optional<Time> filter_stop_timestamp() const { return filter_stop_timestamp_; }
         [[nodiscard]] std::set<CameraDirectionIdentifier> filter_camera_identifiers() const { return filter_camera_identifiers_; }
 
+        // lidar sensors
+        void SetLidarSensorsIdRemappings(std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> remappings) { lidar_sensor_id_remappings_ = std::move(remappings); }
+
+        [[nodiscard]] std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> lidar_sensor_id_remappings() const { return lidar_sensor_id_remappings_; }
+
+
         // camera sensors
         void SetFieldNameImageTime(std::string field_name) { field_name_image_time_ = std::move(field_name); }
         void SetCameraSensorsTimeEstimationLidarTimeframeToImageRatio(double ratio) { camera_sensors_time_estimation_lidar_timeframe_to_image_ratio_ = ratio; }
@@ -137,6 +143,9 @@ namespace a2d2_ros_preparer {
         std::optional<Time> filter_stop_timestamp_ = {};
         std::set<CameraDirectionIdentifier> filter_camera_identifiers_ = {};
 
+        // lidar sensors
+        std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> lidar_sensor_id_remappings_;
+
         // camera sensors
         std::string field_name_image_time_ = "cam_tstamp";
 
diff --git a/a2d2_ros_preparer/src/options_parser.cpp b/a2d2_ros_preparer/src/options_parser.cpp
index 57aa413..74d1137 100644
--- a/a2d2_ros_preparer/src/options_parser.cpp
+++ b/a2d2_ros_preparer/src/options_parser.cpp
@@ -18,6 +18,7 @@
 #include "options_parser.h"
 
 #include <iostream>
+#include <boost/algorithm/string/split.hpp>
 #include <glog/logging.h>
 
 namespace a2d2_ros_preparer {
@@ -30,6 +31,7 @@ namespace a2d2_ros_preparer {
 
         ParsePaths(node, prefix + "/paths", options);
         ParseFilter(node, prefix + "/filter", options);
+        ParseLidarSensors(node, prefix + "/lidar_sensors", options);
         ParseCameraSensors(node, prefix + "/camera_sensors", options);
         ParseBusSignals(node, prefix + "/bus_signals", options);
         ParsePublish(node, prefix + "/publish", options);
@@ -62,6 +64,40 @@ namespace a2d2_ros_preparer {
             options.SetFilterCameraIdentifiers(GetParam<std::vector<std::string>>(node, prefix + "/camera_identifiers"));
     }
 
+    void OptionsParser::ParseLidarSensors(const ros::NodeHandle& node, const std::string& prefix, Options& options) {
+        std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> parsed_lidar_id_remappings = {};
+
+        std::vector<std::string> all_keys;
+        node.getParamNames(all_keys);
+
+        std::string remapping_prefix = prefix + "/lidar_id_remappings";
+        for (auto const &key : all_keys)
+        {
+            if (key.find(remapping_prefix) != 0)
+                continue;
+
+            std::string remaining_key = key.substr(remapping_prefix.length() + 1); // Skip the prefix and '/'
+            std::stringstream remaining_key_stringstream(remaining_key);
+            std::string segment;
+            std::vector<std::string> param_names;
+            while(std::getline(remaining_key_stringstream, segment, '/'))
+            {
+                param_names.push_back(segment);
+            }
+
+            std::string current_camera_identifier = param_names.at(0);
+            uint64_t mapping_from = std::stoi(param_names.at(1));
+            uint64_t mapping_to = GetParam<double>(node, key);
+            // std::cout << "current key " << key << " current_camera_identifier " << current_camera_identifier <<
+            // " mapping_from " << mapping_from << " mapping_to " << mapping_to << std::endl;
+
+            parsed_lidar_id_remappings[current_camera_identifier][mapping_from] = mapping_to;
+        }
+        options.SetLidarSensorsIdRemappings(parsed_lidar_id_remappings);
+
+    }
+
+
     void OptionsParser::ParseCameraSensors(const ros::NodeHandle& node, const std::string& prefix, Options& options) {
         if (node.hasParam(prefix + "/fieldnames/image_timestamp"))
             options.SetFieldNameImageTime(GetParam<std::string>(node, prefix + "/fieldnames/image_timestamp"));
diff --git a/a2d2_ros_preparer/src/options_parser.h b/a2d2_ros_preparer/src/options_parser.h
index be89fb0..fc9cd76 100644
--- a/a2d2_ros_preparer/src/options_parser.h
+++ b/a2d2_ros_preparer/src/options_parser.h
@@ -39,6 +39,7 @@ namespace a2d2_ros_preparer {
     private:
         static void ParsePaths(const ros::NodeHandle& node, const std::string& prefix, Options& options);
         static void ParseFilter(const ros::NodeHandle& node, const std::string& prefix, Options& options);
+        static void ParseLidarSensors(const ros::NodeHandle& node, const std::string& prefix, Options& options);
         static void ParseCameraSensors(const ros::NodeHandle& node, const std::string& prefix, Options& options);
         static void ParseBusSignals(const ros::NodeHandle& node, std::string prefix, Options& options);
         static void ParsePublish(const ros::NodeHandle& node, const std::string& prefix, Options& options);