From 25166e63806cee51365dd954be3e1a3eb01385e5 Mon Sep 17 00:00:00 2001 From: Hirokazu Ishida Date: Tue, 10 May 2022 23:18:43 +0900 Subject: [PATCH 1/2] fix: single -> multiple layers pointcloud Signed-off-by: Hirokazu Ishida --- .../src/pointcloud_creator.cpp | 38 ++++++++++++++++--- 1 file changed, 32 insertions(+), 6 deletions(-) diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp index 6c964625a3e69..111d360d24304 100644 --- a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -206,7 +206,24 @@ std::vector::Ptr> EgoCentricPointCloudCreator::cr pointclouds.at(i) = (pcl::PointCloud::Ptr(new pcl::PointCloud)); } + std::vector min_zs(obj_infos.size()); + std::vector max_zs(obj_infos.size()); + + for (size_t idx = 0; idx < obj_infos.size(); ++idx) { + const auto & obj_info = obj_infos.at(idx); + const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object; + const double min_z = -1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z(); + const double max_z = 1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z(); + min_zs.at(idx) = min_z; + max_zs.at(idx) = max_z; + } + + const double epsilon = 0.001; + const double vertical_theta_step = (1.0 / 180.0) * M_PI; + const double vertical_min_theta = (-15.0 / 180.0) * M_PI; + const double vertical_max_theta = (15.0 / 180.0) * M_PI; const double horizontal_theta_step = 0.25 * M_PI / 180.0; + double angle = 0.0; const auto n_scan = static_cast(std::floor(2 * M_PI / horizontal_theta_step)); for (size_t i = 0; i < n_scan; ++i) { @@ -217,13 +234,22 @@ std::vector::Ptr> EgoCentricPointCloudCreator::cr const auto x_hit = dist * cos(angle); const auto y_hit = dist * sin(angle); const auto idx_hit = composite_sdf.nearest_sdf_index(x_hit, y_hit); + const auto obj_info_here = obj_infos.at(idx_hit); + const auto min_z_here = min_zs.at(idx_hit); + const auto max_z_here = max_zs.at(idx_hit); + std::normal_distribution<> x_random(0.0, obj_info_here.std_dev_x); + std::normal_distribution<> y_random(0.0, obj_info_here.std_dev_y); + std::normal_distribution<> z_random(0.0, obj_info_here.std_dev_z); - std::normal_distribution<> x_random(0.0, obj_infos.at(idx_hit).std_dev_x); - std::normal_distribution<> y_random(0.0, obj_infos.at(idx_hit).std_dev_y); - std::normal_distribution<> z_random(0.0, obj_infos.at(idx_hit).std_dev_z); - pointclouds.at(idx_hit)->push_back(pcl::PointXYZ( - x_hit + x_random(random_generator), y_hit + y_random(random_generator), - z_random(random_generator))); + for (double vertical_theta = vertical_min_theta; + vertical_theta <= vertical_max_theta + epsilon; vertical_theta += vertical_theta_step) { + const double z = dist * std::tan(vertical_theta); + if (min_z_here <= z && z <= max_z_here + epsilon) { + pointclouds.at(idx_hit)->push_back(pcl::PointXYZ( + x_hit + x_random(random_generator), y_hit + y_random(random_generator), + z + z_random(random_generator))); + } + } } } From d49b56adcb5a2f9cc1d84aa516118acf66308892 Mon Sep 17 00:00:00 2001 From: Hirokazu Ishida Date: Tue, 10 May 2022 23:19:48 +0900 Subject: [PATCH 2/2] refactor: share common among different pcloud creators Signed-off-by: Hirokazu Ishida --- .../src/pointcloud_creator.cpp | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp index 111d360d24304..d55c42a0c28bd 100644 --- a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -25,6 +25,18 @@ #include #include +namespace +{ + +static constexpr double epsilon = 0.001; +static constexpr double step = 0.05; +static constexpr double vertical_theta_step = (1.0 / 180.0) * M_PI; +static constexpr double vertical_min_theta = (-15.0 / 180.0) * M_PI; +static constexpr double vertical_max_theta = (15.0 / 180.0) * M_PI; +static constexpr double horizontal_theta_step = (0.1 / 180.0) * M_PI; +static constexpr double horizontal_min_theta = (-180.0 / 180.0) * M_PI; +static constexpr double horizontal_max_theta = (180.0 / 180.0) * M_PI; + pcl::PointXYZ getPointWrtBaseLink( const tf2::Transform & tf_base_link2moved_object, double x, double y, double z) { @@ -32,6 +44,8 @@ pcl::PointXYZ getPointWrtBaseLink( return pcl::PointXYZ(p_wrt_base.x(), p_wrt_base.y(), p_wrt_base.z()); } +} // namespace + void ObjectCentricPointCloudCreator::create_object_pointcloud( const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const @@ -39,14 +53,6 @@ void ObjectCentricPointCloudCreator::create_object_pointcloud( std::normal_distribution<> x_random(0.0, obj_info.std_dev_x); std::normal_distribution<> y_random(0.0, obj_info.std_dev_y); std::normal_distribution<> z_random(0.0, obj_info.std_dev_z); - const double epsilon = 0.001; - const double step = 0.05; - const double vertical_theta_step = (1.0 / 180.0) * M_PI; - const double vertical_min_theta = (-15.0 / 180.0) * M_PI; - const double vertical_max_theta = (15.0 / 180.0) * M_PI; - const double horizontal_theta_step = (0.1 / 180.0) * M_PI; - const double horizontal_min_theta = (-180.0 / 180.0) * M_PI; - const double horizontal_max_theta = (180.0 / 180.0) * M_PI; const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object; @@ -218,12 +224,6 @@ std::vector::Ptr> EgoCentricPointCloudCreator::cr max_zs.at(idx) = max_z; } - const double epsilon = 0.001; - const double vertical_theta_step = (1.0 / 180.0) * M_PI; - const double vertical_min_theta = (-15.0 / 180.0) * M_PI; - const double vertical_max_theta = (15.0 / 180.0) * M_PI; - const double horizontal_theta_step = 0.25 * M_PI / 180.0; - double angle = 0.0; const auto n_scan = static_cast(std::floor(2 * M_PI / horizontal_theta_step)); for (size_t i = 0; i < n_scan; ++i) {