added XYZ point cloud writing for individual LiDAR sensors

tum-gis · Oct 20, 2023 · 5505535 · 5505535
1 parent 94a9c9f
commit 5505535
Show file tree

Hide file tree

Showing 5 changed files with 30 additions and 30 deletions.
diff --git 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
@@ -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<std::string, std::set<DataSequenceId>>& ids);
         TimedPointCloudData GetTimedPointCloudData(DataSequenceId sequence_id, const std::set<std::string>& view_ids = std::set<std::string>());

diff --git 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<double>::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<double>::digits10 + 2) <<
-                 range.position.x() << " " <<
-                 range.position.y() << " " <<
-                 range.position.z() << " " <<
-                 range.intensity << " " <<
-                 range.time << " " <<
-                 static_cast<double>(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
@@ -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
@@ -122,6 +122,8 @@ namespace a2d2_ros_preparer {
     }
 
     void Vehicle::WriteLidarDataToXYZ(const std::filesystem::path &directory_path, std::optional<Time> filter_start_timestamp, std::optional<Time> filter_stop_timestamp, Duration time_delta) {
+        CHECK_GT(time_delta, 0) << "Time delta duration must greater than zero.";
+        LOG(INFO) << "Writing lidar data with a time_delta=" << time_delta << " to XYZ files to " << directory_path;
         auto valid_start_timestamp = GetValidStartTimestamp(filter_start_timestamp);
         auto valid_stop_timestamp = GetValidStopTimestamp(filter_stop_timestamp);
 
@@ -139,9 +141,8 @@ namespace a2d2_ros_preparer {
             LOG(INFO) << "Writing lidar step " << count << "/" << complete << " (" << std::setprecision(2) << current_percent << "%)";
 
             auto filename = std::string(complete_digits - std::to_string(count).length(), '0') + std::to_string(count);
-            auto filepath = directory_path / (filename + ".xyz");
 
-            lidar_scan_stream_.WriteAllDataToXYZFile(filepath, current_timestamp, current_timestamp_stop);
+            lidar_scan_stream_.WriteAllDataToXYZFile(directory_path, filename, current_timestamp, current_timestamp_stop);
             current_timestamp += time_delta;
             count++;
         }