Merge pull request #192 from utiasASRL/save_raw_pts

Added saving raw pt clouds as config option
utiasASRL · Aug 11, 2023 · 3bef4ed · 3bef4ed
2 parents 47fac2a + 645bd06
commit 3bef4ed
Show file tree

Hide file tree

Showing 8 changed files with 20 additions and 11 deletions.
diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake
@@ -35,5 +35,4 @@ endif()
 ## Enable certain pipelines
 add_definitions(-DVTR_ENABLE_LIDAR)
 add_definitions(-DVTR_ENABLE_RADAR)
-#add_definitions(-DSAVE_FULL_LIDAR)
 #add_definitions(-DSAVE_ALL_REPEATS)
diff --git a/main/src/vtr_lidar/include/vtr_lidar/pipeline.hpp b/main/src/vtr_lidar/include/vtr_lidar/pipeline.hpp
@@ -44,6 +44,7 @@ class LidarPipeline : public tactic::BasePipeline {
     double submap_translation_threshold = 0.0;  // in meters
     double submap_rotation_threshold = 0.0;     // in degrees
 
+    bool save_raw_point_cloud = false;
     bool save_nn_point_cloud = false;
 
     static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node,

diff --git a/main/src/vtr_lidar/src/pipeline.cpp b/main/src/vtr_lidar/src/pipeline.cpp
@@ -39,6 +39,7 @@ auto LidarPipeline::Config::fromROS(const rclcpp::Node::SharedPtr &node,
   config->submap_translation_threshold = node->declare_parameter<double>(param_prefix + ".submap_translation_threshold", config->submap_translation_threshold);
   config->submap_rotation_threshold = node->declare_parameter<double>(param_prefix + ".submap_rotation_threshold", config->submap_rotation_threshold);
 
+  config->save_raw_point_cloud = node->declare_parameter<bool>(param_prefix + ".save_raw_point_cloud", config->save_raw_point_cloud);
   config->save_nn_point_cloud = node->declare_parameter<bool>(param_prefix + ".save_nn_point_cloud", config->save_nn_point_cloud);
   // clang-format on
   return config;
@@ -159,7 +160,7 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0,
 
   }
   // raw point cloud
-#if defined(VTR_ENABLE_LIDAR) && defined(SAVE_FULL_LIDAR)
+  if (config_->save_raw_point_cloud)
   {
     auto raw_scan_odo = std::make_shared<PointScan<PointWithInfo>>();
     raw_scan_odo->point_cloud() = *qdata->raw_point_cloud;
@@ -171,9 +172,8 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0,
         std::make_shared<PointScanLM>(raw_scan_odo, *qdata->stamp);
     vertex->insert<PointScan<PointWithInfo>>(
         "raw_point_cloud", "vtr_lidar_msgs/msg/PointScan", raw_scan_odo_msg);
+    CLOG(DEBUG, "lidar.pipeline") << "Saved raw pointcloud to vertex" << vertex;
   }
-  CLOG(DEBUG, "lidar.pipeline") << "Saved raw pointcloud to vertex" << vertex;
-#endif
 
   // raw point cloud
   if (config_->save_nn_point_cloud) {

diff --git a/main/src/vtr_radar/include/vtr_radar/pipeline.hpp b/main/src/vtr_radar/include/vtr_radar/pipeline.hpp
@@ -44,6 +44,8 @@ class RadarPipeline : public tactic::BasePipeline {
     double submap_translation_threshold = 0.0;  // in meters
     double submap_rotation_threshold = 0.0;     // in degrees
 
+    bool save_raw_point_cloud = false;
+
     static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node,
                             const std::string &param_prefix);
   };

diff --git a/main/src/vtr_radar/src/modules/preprocessing/preprocessing_module.cpp b/main/src/vtr_radar/src/modules/preprocessing/preprocessing_module.cpp
@@ -80,7 +80,7 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &,
   /// Grid subsampling
 
   if (config_->voxel_downsample) {
-    // Get subsampling of the frame in carthesian coordinates
+    // Get subsampling of the frame in cartesian coordinates
     voxelDownsample(*filtered_point_cloud, config_->frame_voxel_size);
 
     CLOG(DEBUG, "radar.preprocessing")

diff --git a/main/src/vtr_radar/src/pipeline.cpp b/main/src/vtr_radar/src/pipeline.cpp
@@ -38,6 +38,8 @@ auto RadarPipeline::Config::fromROS(const rclcpp::Node::SharedPtr &node,
   // submap creation thresholds
   config->submap_translation_threshold = node->declare_parameter<double>(param_prefix + ".submap_translation_threshold", config->submap_translation_threshold);
   config->submap_rotation_threshold = node->declare_parameter<double>(param_prefix + ".submap_rotation_threshold", config->submap_rotation_threshold);
+
+  config->save_raw_point_cloud = node->declare_parameter<bool>(param_prefix + ".save_raw_point_cloud", config->save_raw_point_cloud);
   // clang-format on
   return config;
 }
@@ -153,10 +155,11 @@ void RadarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0,
         "filtered_point_cloud", "vtr_radar_msgs/msg/PointScan", scan_odo_msg);
   }
   // raw point cloud
-#if false
+  // for radar we are saving the preprocessed, but not undistrorted, point cloud
+  if (config_->save_raw_point_cloud)
   {
     auto raw_scan_odo = std::make_shared<PointScan<PointWithInfo>>();
-    raw_scan_odo->point_cloud() = *qdata->undistorted_raw_point_cloud;
+    raw_scan_odo->point_cloud() = *qdata->raw_point_cloud;
     raw_scan_odo->T_vertex_this() = qdata->T_s_r->inverse();
     raw_scan_odo->vertex_id() = *qdata->vid_odo;
     //
@@ -165,8 +168,8 @@ void RadarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0,
         std::make_shared<PointScanLM>(raw_scan_odo, *qdata->stamp);
     vertex->insert<PointScan<PointWithInfo>>(
         "raw_point_cloud", "vtr_radar_msgs/msg/PointScan", raw_scan_odo_msg);
+    CLOG(DEBUG, "radar.pipeline") << "Saved raw pointcloud to vertex" << vertex;
   }
-#endif
 
   /// save the sliding map as vertex submap if we have traveled far enough
   const bool create_submap = [&] {

diff --git a/main/src/vtr_radar_lidar/include/vtr_radar_lidar/pipeline.hpp b/main/src/vtr_radar_lidar/include/vtr_radar_lidar/pipeline.hpp
@@ -46,6 +46,8 @@ class RadarLidarPipeline : public tactic::BasePipeline {
     double submap_translation_threshold = 0.0;  // in meters
     double submap_rotation_threshold = 0.0;     // in degrees
 
+    bool save_raw_point_cloud = false;
+
     static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node,
                             const std::string &param_prefix);
   };

diff --git a/main/src/vtr_radar_lidar/src/pipeline.cpp b/main/src/vtr_radar_lidar/src/pipeline.cpp
@@ -38,6 +38,8 @@ auto RadarLidarPipeline::Config::fromROS(const rclcpp::Node::SharedPtr &node,
   // submap creation thresholds
   config->submap_translation_threshold = node->declare_parameter<double>(param_prefix + ".submap_translation_threshold", config->submap_translation_threshold);
   config->submap_rotation_threshold = node->declare_parameter<double>(param_prefix + ".submap_rotation_threshold", config->submap_rotation_threshold);
+
+  config->save_raw_point_cloud = node->declare_parameter<bool>(param_prefix + ".save_raw_point_cloud", config->save_raw_point_cloud);
   // clang-format on
   return config;
 }
@@ -155,11 +157,11 @@ void RadarLidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0,
         scan_odo_msg);
   }
   // raw point cloud
-#if false
+  if (config_->save_raw_point_cloud)
   {
     auto raw_scan_odo =
         std::make_shared<radar::PointScan<radar::PointWithInfo>>();
-    raw_scan_odo->point_cloud() = *qdata->undistorted_raw_point_cloud;
+    raw_scan_odo->point_cloud() = *qdata->raw_point_cloud;
     raw_scan_odo->T_vertex_this() = qdata->T_s_r->inverse();
     raw_scan_odo->vertex_id() = *qdata->vid_odo;
     //
@@ -170,8 +172,8 @@ void RadarLidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0,
     vertex->insert<radar::PointScan<radar::PointWithInfo>>(
         "radar_raw_point_cloud", "vtr_radar_msgs/msg/PointScan",
         raw_scan_odo_msg);
+    CLOG(DEBUG, "radar.pipeline") << "Saved raw pointcloud to vertex" << vertex;
   }
-#endif
 
   /// save the sliding map as vertex submap if we have traveled far enough
   const bool create_submap = [&] {