From 5505535538aa8c3638c76a9396f94aeedcd92335 Mon Sep 17 00:00:00 2001 From: Benedikt Schwab Date: Fri, 20 Oct 2023 11:18:27 +0200 Subject: [PATCH] added XYZ point cloud writing for individual LiDAR sensors --- .../src/lidar/lidar_scan_stream.cpp | 25 +++++++++++++++-- .../src/lidar/lidar_scan_stream.h | 2 +- .../src/lidar/timed_point_cloud_data.cpp | 27 +++---------------- .../src/lidar/timed_point_cloud_data.h | 1 - a2d2_ros_preparer/src/vehicle/vehicle.cpp | 5 ++-- 5 files changed, 30 insertions(+), 30 deletions(-) diff --git a/a2d2_ros_preparer/src/lidar/lidar_scan_stream.cpp b/a2d2_ros_preparer/src/lidar/lidar_scan_stream.cpp index fef7519..a92f0d9 100644 --- a/a2d2_ros_preparer/src/lidar/lidar_scan_stream.cpp +++ b/a2d2_ros_preparer/src/lidar/lidar_scan_stream.cpp @@ -201,11 +201,32 @@ namespace a2d2_ros_preparer { } void - LidarScanStream::WriteAllDataToXYZFile(const std::filesystem::path& filepath, Time start_timestamp, Time stop_timestamp) { + LidarScanStream::WriteAllDataToXYZFile(const std::filesystem::path& directory_path, const std::string& filename, Time start_timestamp, Time stop_timestamp) { auto complete_point_cloud = GetTimedPointCloudData(start_timestamp, stop_timestamp); complete_point_cloud = FilterPointDuplicates(complete_point_cloud); - WriteToXYZFileVerbose(complete_point_cloud, filepath); + // individual non-transformed point cloud + auto complete_file_path = directory_path / (filename + ".xyz"); + WriteToXYZFile(complete_point_cloud, complete_file_path); + + // back-transformed point clouds differentiated by each sensor + auto individual_directory_path = directory_path / filename; + std::filesystem::create_directory(individual_directory_path); + + 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); + if (!current_point_cloud.empty()) { + auto transform = lidar_to_base_affine_transformation_.at(*it); + auto transform_inverse = transform.inverse(Eigen::Affine); + auto transformed_point_cloud = TransformPointCloud(current_point_cloud, transform_inverse, "lidar_" + *it); + + WriteToXYZFile(transformed_point_cloud, individual_file_path); + } + } } Time LidarScanStream::GetTime(const CameraDirectionIdentifier& camera_identifier, DataSequenceId sequence_id) const { diff --git a/a2d2_ros_preparer/src/lidar/lidar_scan_stream.h b/a2d2_ros_preparer/src/lidar/lidar_scan_stream.h index 7e2f17c..4f8b98a 100644 --- a/a2d2_ros_preparer/src/lidar/lidar_scan_stream.h +++ b/a2d2_ros_preparer/src/lidar/lidar_scan_stream.h @@ -55,7 +55,7 @@ namespace a2d2_ros_preparer { [[nodiscard]] bool IsSensorDataAvailable(DataSequenceId id) const; void WriteAllDataToRosbag(rosbag::Bag &bag, Time start_timestamp, Time stop_timestamp); - void WriteAllDataToXYZFile(const std::filesystem::path& filepath, Time start_timestamp, Time stop_timestamp); + void WriteAllDataToXYZFile(const std::filesystem::path& directory_path, const std::string& filename, Time start_timestamp, Time stop_timestamp); TimedPointCloudData GetTimedPointCloudData(const std::map>& ids); TimedPointCloudData GetTimedPointCloudData(DataSequenceId sequence_id, const std::set& view_ids = std::set()); diff --git a/a2d2_ros_preparer/src/lidar/timed_point_cloud_data.cpp b/a2d2_ros_preparer/src/lidar/timed_point_cloud_data.cpp index 87a1512..6fbbe80 100644 --- a/a2d2_ros_preparer/src/lidar/timed_point_cloud_data.cpp +++ b/a2d2_ros_preparer/src/lidar/timed_point_cloud_data.cpp @@ -141,13 +141,13 @@ namespace a2d2_ros_preparer { } - void WriteToXYZFileVerbose(const TimedPointCloudData& point_cloud, const std::filesystem::path& filepath) { + void WriteToXYZFile(const TimedPointCloudData& point_cloud, const std::filesystem::path& filepath) { auto start_time = point_cloud.GetStartTime(); std::ofstream file; file.open (filepath); - file << "X Y Z intensity time time_since_start_in_ms timestamp rectime boundary azimuth col row depth distance sensor_id dataset_view_id dataset_sequence_id\n"; + file << "x y z intensity time time_since_start_in_ms rectime boundary azimuth col row depth distance sensor_id dataset_view_id dataset_sequence_id\n"; for (const auto& range: point_cloud.ranges()) { file << std::setprecision(std::numeric_limits::digits10 + 2) << range.position.x() << " " << @@ -165,28 +165,7 @@ namespace a2d2_ros_preparer { range.distance << " " << range.sensor_id << " " << range.dataset_view_id << " " << - range.dataset_sequence_id << "" << - "\n"; - } - file.close(); - } - - void WriteToXYZFile(const TimedPointCloudData& point_cloud, const std::filesystem::path& filepath) { - - auto start_time = point_cloud.GetStartTime(); - - std::ofstream file; - file.open (filepath); - file << "X Y Z intensity time time_since_start_in_ms\n"; - for (const auto& range: point_cloud.ranges()) { - file << std::setprecision(std::numeric_limits::digits10 + 2) << - range.position.x() << " " << - range.position.y() << " " << - range.position.z() << " " << - range.intensity << " " << - range.time << " " << - static_cast(range.time-start_time)/1e6 << " " << - "\n"; + range.dataset_sequence_id << "\n"; } file.close(); } diff --git a/a2d2_ros_preparer/src/lidar/timed_point_cloud_data.h b/a2d2_ros_preparer/src/lidar/timed_point_cloud_data.h index 8671014..b62f3c5 100644 --- a/a2d2_ros_preparer/src/lidar/timed_point_cloud_data.h +++ b/a2d2_ros_preparer/src/lidar/timed_point_cloud_data.h @@ -72,7 +72,6 @@ namespace a2d2_ros_preparer { TimedPointCloudData FilterPointCloud(const TimedPointCloudData& point_cloud, uint64_t sensor_id); TimedPointCloudData FilterPointDuplicates(const TimedPointCloudData& point_cloud); - void WriteToXYZFileVerbose(const TimedPointCloudData& point_cloud, const std::filesystem::path& filepath); void WriteToXYZFile(const TimedPointCloudData& point_cloud, const std::filesystem::path& filepath); } diff --git a/a2d2_ros_preparer/src/vehicle/vehicle.cpp b/a2d2_ros_preparer/src/vehicle/vehicle.cpp index 4a634b6..d079945 100644 --- a/a2d2_ros_preparer/src/vehicle/vehicle.cpp +++ b/a2d2_ros_preparer/src/vehicle/vehicle.cpp @@ -122,6 +122,8 @@ namespace a2d2_ros_preparer { } void Vehicle::WriteLidarDataToXYZ(const std::filesystem::path &directory_path, std::optional