adding sensor-specific correction transformations

a2d2_ros_preparer/src/converter.cpp

@@ -48,6 +48,7 @@ namespace a2d2_ros_preparer {
                                                  options.lidar_sensor_id_remappings(),
                                                  vehicle_configuration.GetCameraToBaseAffineTransforms(),
                                                  vehicle_configuration.GetLidarToBaseAffineTransforms(),
+                                                 options.lidar_correction_transformation(),
                                                  options.filter_start_timestamp(),
                                                  options.filter_stop_timestamp());
 

a2d2_ros_preparer/src/lidar/lidar_scan_stream.cpp

@@ -28,14 +28,16 @@ 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, 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::map<LidarDirectionIdentifier, Eigen::Matrix4d> lidar_correction_transformation,
                                      std::optional<Time> filter_start_timestamp,
                                      std::optional<Time> filter_stop_timestamp) :
             lidar_identifiers_(std::move(lidar_identifiers)),
             camera_to_base_affine_transformation_(std::move(camera_to_base_affine_transformation)),
-            lidar_to_base_affine_transformation_(std::move(lidar_to_base_affine_transformation)) {
+            lidar_to_base_affine_transformation_(std::move(lidar_to_base_affine_transformation)),
+            lidar_correction_transformation_(std::move(lidar_correction_transformation)) {
 
         for (auto const& current_camera_identifier: camera_identifiers) {
             if (!IsSubdirectoryContained(lidar_data_directory, current_camera_identifier)) {
@@ -84,14 +86,22 @@ namespace a2d2_ros_preparer {
         std::vector<TimedPointCloudData> point_clouds;
 
         std::for_each(std::execution::par_unseq, std::begin(camera_identifiers), std::end(camera_identifiers), [&](const std::string& current_camera_identifier) {
-            auto point_cloud = lidar_scan_timeseries_per_view_.at(current_camera_identifier).GetTimedPointCloudData(timestamp_min, timestamp_max);
-            auto transformed_point_cloud = TransformPointCloud(point_cloud, camera_to_base_affine_transformation_.at(current_camera_identifier), "base_link");
+            const auto point_cloud = lidar_scan_timeseries_per_view_.at(current_camera_identifier).GetTimedPointCloudData(timestamp_min, timestamp_max);
+            const auto transformed_point_cloud = TransformPointCloud(point_cloud, camera_to_base_affine_transformation_.at(current_camera_identifier), "base_link");
 
             std::lock_guard<std::mutex> guard(m);
             point_clouds.push_back(transformed_point_cloud);
         });
+        auto combined_point_cloud = TimedPointCloudData(point_clouds);
 
-        return TimedPointCloudData(point_clouds);
+        // apply post corrections if defined
+        if (!lidar_correction_transformation_.empty())
+        {
+            auto lidar_correction_transformation_per_index = GetLidarCorrectionTransformationPerIndex();
+            combined_point_cloud = TransformPointCloudSensorSpecific(combined_point_cloud, lidar_correction_transformation_per_index);
+        }
+
+        return combined_point_cloud;
     }
 
     std::vector<DataSequenceId> LidarScanStream::GetAllDataSequenceIds() const {
@@ -116,6 +126,14 @@ namespace a2d2_ros_preparer {
         return lidar_scan_timeseries.GetDataSequenceIds(start_timestamp, stop_timestamp);
     }
 
+    uint64_t LidarScanStream::GetLidarIndex(const LidarDirectionIdentifier& id) const {
+        const auto it = std::find(lidar_identifiers_.begin(), lidar_identifiers_.end(), id);
+        if (it == lidar_identifiers_.end()) {
+            throw std::out_of_range("LidarDirectionIdentifier not found");
+        }
+        return std::distance(lidar_identifiers_.begin(), it);
+    }
+
     bool LidarScanStream::IsSensorDataAvailable(DataSequenceId id) const {
         for (const auto& [_, current_sensor_data_timeseries] : lidar_scan_timeseries_per_view_) {
             if (!current_sensor_data_timeseries.HasData(id))
@@ -124,6 +142,15 @@ namespace a2d2_ros_preparer {
         return true;
     }
 
+    std::map<uint64_t, Eigen::Matrix4d> LidarScanStream::GetLidarCorrectionTransformationPerIndex() const {
+        auto lidar_correction_transformation_per_index = std::map<uint64_t, Eigen::Matrix4d>();
+        for(const auto & [lidar_direction_identifier, matrix] : lidar_correction_transformation_) {
+            lidar_correction_transformation_per_index.insert(std::make_pair(GetLidarIndex(lidar_direction_identifier), matrix));
+        }
+
+        return lidar_correction_transformation_per_index;
+    }
+
     Time LidarScanStream::GetStartTimestamp() const {
         std::vector<Time> start_timestamps;
         for (auto& [_, current_sensor_data_timeseries] : lidar_scan_timeseries_per_view_)
@@ -193,9 +220,7 @@ namespace a2d2_ros_preparer {
         complete_point_cloud = FilterPointDuplicates(complete_point_cloud);
 
         for (auto it = lidar_identifiers_.begin(); it != lidar_identifiers_.end(); ++it) {
-            int index = std::distance(lidar_identifiers_.begin(), it);
-
-            auto current_point_cloud = FilterPointCloud(complete_point_cloud, index);
+            auto current_point_cloud = FilterPointCloud(complete_point_cloud, GetLidarIndex(*it));
             if (!current_point_cloud.empty()) {
                 auto transform = lidar_to_base_affine_transformation_.at(*it);
                 auto transform_inverse = transform.inverse(Eigen::Affine);
@@ -224,9 +249,7 @@ namespace a2d2_ros_preparer {
         for (auto it = lidar_identifiers_.begin(); it != lidar_identifiers_.end(); ++it) {
             auto individual_file_path = individual_directory_path / (*it + ".xyz");
 
-            int index = std::distance(lidar_identifiers_.begin(), it);
-
-            auto current_point_cloud = FilterPointCloud(complete_point_cloud, index);
+            auto current_point_cloud = FilterPointCloud(complete_point_cloud, GetLidarIndex(*it));
             if (!current_point_cloud.empty()) {
                 auto transform = lidar_to_base_affine_transformation_.at(*it);
                 auto transform_inverse = transform.inverse(Eigen::Affine);

a2d2_ros_preparer/src/lidar/lidar_scan_stream.h

@@ -34,9 +34,10 @@ 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, 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::map<LidarDirectionIdentifier, Eigen::Matrix4d> lidar_correction_transformation,
                                  std::optional<Time> filter_start_timestamp,
                                  std::optional<Time> filter_stop_timestamp);
 
@@ -53,7 +54,9 @@ namespace a2d2_ros_preparer {
         [[nodiscard]] std::map<CameraDirectionIdentifier, std::map<DataSequenceId, Time>> GetPointCloudTimeMaxsPerCameraDirection() const;
         [[nodiscard]] std::map<CameraDirectionIdentifier, std::vector<DataSequenceId>> GetAllDataSequenceIdsPerCameraDirection() const;
 
+        [[nodiscard]] uint64_t GetLidarIndex(const LidarDirectionIdentifier& id) const;
         [[nodiscard]] bool IsSensorDataAvailable(DataSequenceId id) const;
+        [[nodiscard]] std::map<uint64_t, Eigen::Matrix4d> GetLidarCorrectionTransformationPerIndex() const;
 
         void WriteAllDataToRosbag(rosbag::Bag &bag, Time start_timestamp, Time stop_timestamp);
         void WriteAllDataToXYZFile(const std::filesystem::path& directory_path, const std::string& filename, Time start_timestamp, Time stop_timestamp);
@@ -70,6 +73,8 @@ namespace a2d2_ros_preparer {
 
         std::map<CameraDirectionIdentifier, Eigen::Affine3d> camera_to_base_affine_transformation_;
         std::map<LidarDirectionIdentifier, Eigen::Affine3d> lidar_to_base_affine_transformation_;
+
+        std::map<LidarDirectionIdentifier, Eigen::Matrix4d> lidar_correction_transformation_;
     };
 }
 

a2d2_ros_preparer/src/lidar/timed_point_cloud_data.cpp

@@ -100,7 +100,26 @@ namespace a2d2_ros_preparer {
 
             transformed_ranges.push_back(result_range);
         }
-        TimedPointCloudData transformed_point_cloud = TimedPointCloudData(transformed_ranges, target_frame_id);
+        auto transformed_point_cloud = TimedPointCloudData(transformed_ranges, target_frame_id);
+        return transformed_point_cloud;
+    }
+
+    TimedPointCloudData TransformPointCloudSensorSpecific(const TimedPointCloudData& point_cloud, const std::map<uint64_t, Eigen::Matrix4d>& lidar_specific_matrices) {
+
+        std::vector<TimedRangefinderPoint> transformed_ranges;
+        for (const auto& current_range: point_cloud.ranges()) {
+            TimedRangefinderPoint result_range = current_range;
+
+            if (lidar_specific_matrices.count(current_range.sensor_id) > 0)
+            {
+                auto position_4d = Eigen::Vector4d(current_range.position.x(), current_range.position.y(), current_range.position.z(), 1.0);
+                Eigen::Vector4d transformed_position_4d = lidar_specific_matrices.at(current_range.sensor_id) * position_4d;
+                result_range.position = Eigen::Vector3d(transformed_position_4d.x(), transformed_position_4d.y(), transformed_position_4d.z());
+            }
+
+            transformed_ranges.push_back(result_range);
+        }
+        auto transformed_point_cloud = TimedPointCloudData(transformed_ranges, point_cloud.frame_id());
         return transformed_point_cloud;
     }
 

a2d2_ros_preparer/src/lidar/timed_point_cloud_data.h

@@ -68,6 +68,7 @@ namespace a2d2_ros_preparer {
     };
 
     TimedPointCloudData TransformPointCloud(const TimedPointCloudData& point_cloud, const Eigen::Affine3d& affine, const std::string& target_frame_id);
+    TimedPointCloudData TransformPointCloudSensorSpecific(const TimedPointCloudData& point_cloud, const std::map<uint64_t, Eigen::Matrix4d>& lidar_specific_matrices);
     TimedPointCloudData FilterPointCloud(const TimedPointCloudData& point_cloud, Time time_min, Time time_max);
     TimedPointCloudData FilterPointCloud(const TimedPointCloudData& point_cloud, uint64_t sensor_id);
     TimedPointCloudData FilterPointDuplicates(const TimedPointCloudData& point_cloud);

a2d2_ros_preparer/src/options.h

@@ -22,6 +22,7 @@
 #include <cmath>
 #include <utility>
 #include <glog/logging.h>
+#include <Eigen/StdVector>
 #include "common/timing.h"
 #include "common/types.h"
 
@@ -60,9 +61,10 @@ namespace a2d2_ros_preparer {
 
         // lidar sensors
         void SetLidarSensorsIdRemappings(std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> remappings) { lidar_sensor_id_remappings_ = std::move(remappings); }
+        void SetLidarCorrectionTransformation(std::map<LidarDirectionIdentifier, Eigen::Matrix4d> matrices) { lidar_correction_transformation_ = std::move(matrices); }
 
         [[nodiscard]] std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> lidar_sensor_id_remappings() const { return lidar_sensor_id_remappings_; }
-
+        [[nodiscard]] std::map<LidarDirectionIdentifier, Eigen::Matrix4d> lidar_correction_transformation() const { return lidar_correction_transformation_; }
 
         // camera sensors
         void SetFieldNameImageTime(std::string field_name) { field_name_image_time_ = std::move(field_name); }
@@ -145,6 +147,7 @@ namespace a2d2_ros_preparer {
 
         // lidar sensors
         std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> lidar_sensor_id_remappings_;
+        std::map<LidarDirectionIdentifier, Eigen::Matrix4d> lidar_correction_transformation_;
 
         // camera sensors
         std::string field_name_image_time_ = "cam_tstamp";

a2d2_ros_preparer/src/options_parser.cpp

@@ -65,12 +65,12 @@ namespace a2d2_ros_preparer {
     }
 
     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);
 
+        // parse lidar id remappings
         std::string remapping_prefix = prefix + "/lidar_id_remappings";
+        std::map<CameraDirectionIdentifier, std::map<uint64_t, uint64_t>> parsed_lidar_id_remappings = {};
         for (auto const &key : all_keys)
         {
             if (key.find(remapping_prefix) != 0)
@@ -95,6 +95,33 @@ namespace a2d2_ros_preparer {
         }
         options.SetLidarSensorsIdRemappings(parsed_lidar_id_remappings);
 
+
+        // parse lidar id remappings
+        auto lidar_correction_transformation = std::map<LidarDirectionIdentifier, Eigen::Matrix4d>();
+        std::string correction_prefix = prefix + "/lidar_transformation_corrections";
+        for (auto const &key : all_keys)
+        {
+            if (key.find(correction_prefix) != 0)
+                continue;
+
+            std::string remaining_key = key.substr(correction_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_lidar_identifier = param_names.at(0);
+            auto matrix_values = GetParam<std::vector<double>>(node, key);
+            Eigen::Matrix4d matrix = Eigen::Map<Eigen::Matrix4d>(matrix_values.data());
+            // std::cout << "Matrix:\n" << matrix << std::endl;
+
+            lidar_correction_transformation.insert(std::make_pair(current_lidar_identifier, matrix));
+        }
+
+        options.SetLidarCorrectionTransformation(lidar_correction_transformation);
     }