Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Added saving raw pt clouds as config option #192

Merged
merged 1 commit into from
Aug 11, 2023
Merged

Added saving raw pt clouds as config option #192

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion main/src/vtr_common/vtr_include.cmake
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,5 +34,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)
1 change: 1 addition & 0 deletions main/src/vtr_lidar/include/vtr_lidar/pipeline.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
6 changes: 3 additions & 3 deletions main/src/vtr_lidar/src/pipeline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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) {
Expand Down
2 changes: 2 additions & 0 deletions main/src/vtr_radar/include/vtr_radar/pipeline.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
};
Expand Down
2 changes: 1 addition & 1 deletion main/src/vtr_radar/src/modules/preprocessing/preprocessing_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down
9 changes: 6 additions & 3 deletions main/src/vtr_radar/src/pipeline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
//
Expand All @@ -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 = [&] {
Expand Down
2 changes: 2 additions & 0 deletions main/src/vtr_radar_lidar/include/vtr_radar_lidar/pipeline.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
};
Expand Down
8 changes: 5 additions & 3 deletions main/src/vtr_radar_lidar/src/pipeline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
//
Expand All @@ -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 = [&] {
Expand Down