From 5c24ed0243a46fd8939008c139e5229a9928369c Mon Sep 17 00:00:00 2001 From: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> Date: Fri, 29 Jul 2022 12:54:51 +0900 Subject: [PATCH] feat(shape_estimation): add boost optimizer (#1292) * feat(shape_estimation): add boost optimizer Signed-off-by: Xinyu Wang * ci(pre-commit): autofix * change default param Signed-off-by: Xinyu Wang * move boost into bbox class Signed-off-by: Xinyu Wang * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../shape_estimation/model/bounding_box.hpp | 8 +- .../shape_estimation/shape_estimator.hpp | 3 +- .../launch/shape_estimation.launch.xml | 2 + .../lib/model/bounding_box.cpp | 98 ++++++++++++++----- .../shape_estimation/lib/shape_estimator.cpp | 14 ++- perception/shape_estimation/src/node.cpp | 6 +- 6 files changed, 94 insertions(+), 37 deletions(-) diff --git a/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp b/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp index 255cf64176b1..c6fef49b0965 100644 --- a/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp +++ b/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp @@ -28,11 +28,17 @@ class BoundingBoxShapeModel : public ShapeEstimationModelInterface autoware_auto_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output); float calcClosenessCriterion(const std::vector & C_1, const std::vector & C_2); + float optimize( + const pcl::PointCloud & cluster, const float min_angle, const float max_angle); + float boostOptimize( + const pcl::PointCloud & cluster, const float min_angle, const float max_angle); public: BoundingBoxShapeModel(); - explicit BoundingBoxShapeModel(const boost::optional & ref_yaw_info); + explicit BoundingBoxShapeModel( + const boost::optional & ref_yaw_info, bool use_boost_bbox_optimizer = false); boost::optional ref_yaw_info_; + bool use_boost_bbox_optimizer_; ~BoundingBoxShapeModel() {} diff --git a/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp b/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp index af43c02f7bed..bb0878f22510 100644 --- a/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp +++ b/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp @@ -50,9 +50,10 @@ class ShapeEstimator bool use_corrector_; bool use_filter_; + bool use_boost_bbox_optimizer_; public: - ShapeEstimator(bool use_corrector, bool use_filter); + ShapeEstimator(bool use_corrector, bool use_filter, bool use_boost_bbox_optimizer = false); virtual ~ShapeEstimator() = default; diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index 4107e09a1059..439b292d2e69 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -5,11 +5,13 @@ + + diff --git a/perception/shape_estimation/lib/model/bounding_box.cpp b/perception/shape_estimation/lib/model/bounding_box.cpp index 5e7b36494cd7..50ee476c13ed 100644 --- a/perception/shape_estimation/lib/model/bounding_box.cpp +++ b/perception/shape_estimation/lib/model/bounding_box.cpp @@ -20,6 +20,8 @@ #include +#include + #include #include #include @@ -42,10 +44,14 @@ constexpr float epsilon = 0.001; -BoundingBoxShapeModel::BoundingBoxShapeModel() : ref_yaw_info_(boost::none) {} +BoundingBoxShapeModel::BoundingBoxShapeModel() +: ref_yaw_info_(boost::none), use_boost_bbox_optimizer_(false) +{ +} -BoundingBoxShapeModel::BoundingBoxShapeModel(const boost::optional & ref_yaw_info) -: ref_yaw_info_(ref_yaw_info) +BoundingBoxShapeModel::BoundingBoxShapeModel( + const boost::optional & ref_yaw_info, bool use_boost_bbox_optimizer) +: ref_yaw_info_(ref_yaw_info), use_boost_bbox_optimizer_(use_boost_bbox_optimizer) { } @@ -82,31 +88,13 @@ bool BoundingBoxShapeModel::fitLShape( */ // Paper : Algo.2 Search-Based Rectangle Fitting - std::vector> Q; - constexpr float angle_resolution = M_PI / 180.0; - for (float theta = min_angle; theta <= max_angle + epsilon; theta += angle_resolution) { - Eigen::Vector2f e_1; - e_1 << std::cos(theta), std::sin(theta); // col.3, Algo.2 - Eigen::Vector2f e_2; - e_2 << -std::sin(theta), std::cos(theta); // col.4, Algo.2 - std::vector C_1; // col.5, Algo.2 - std::vector C_2; // col.6, Algo.2 - for (const auto & point : cluster) { - C_1.push_back(point.x * e_1.x() + point.y * e_1.y()); - C_2.push_back(point.x * e_2.x() + point.y * e_2.y()); - } - float q = calcClosenessCriterion(C_1, C_2); // col.7, Algo.2 - Q.push_back(std::make_pair(theta, q)); // col.8, Algo.2 + double theta_star; + if (use_boost_bbox_optimizer_) { + theta_star = boostOptimize(cluster, min_angle, max_angle); + } else { + theta_star = optimize(cluster, min_angle, max_angle); } - float theta_star{0.0}; // col.10, Algo.2 - float max_q = 0.0; - for (size_t i = 0; i < Q.size(); ++i) { - if (max_q < Q.at(i).second || i == 0) { - max_q = Q.at(i).second; - theta_star = Q.at(i).first; - } - } const float sin_theta_star = std::sin(theta_star); const float cos_theta_star = std::cos(theta_star); @@ -206,3 +194,61 @@ float BoundingBoxShapeModel::calcClosenessCriterion( } return beta; } + +float BoundingBoxShapeModel::optimize( + const pcl::PointCloud & cluster, const float min_angle, const float max_angle) +{ + std::vector> Q; + constexpr float angle_resolution = M_PI / 180.0; + for (float theta = min_angle; theta <= max_angle + epsilon; theta += angle_resolution) { + Eigen::Vector2f e_1; + e_1 << std::cos(theta), std::sin(theta); // col.3, Algo.2 + Eigen::Vector2f e_2; + e_2 << -std::sin(theta), std::cos(theta); // col.4, Algo.2 + std::vector C_1; // col.5, Algo.2 + std::vector C_2; // col.6, Algo.2 + for (const auto & point : cluster) { + C_1.push_back(point.x * e_1.x() + point.y * e_1.y()); + C_2.push_back(point.x * e_2.x() + point.y * e_2.y()); + } + float q = calcClosenessCriterion(C_1, C_2); // col.7, Algo.2 + Q.push_back(std::make_pair(theta, q)); // col.8, Algo.2 + } + + float theta_star{0.0}; // col.10, Algo.2 + float max_q = 0.0; + for (size_t i = 0; i < Q.size(); ++i) { + if (max_q < Q.at(i).second || i == 0) { + max_q = Q.at(i).second; + theta_star = Q.at(i).first; + } + } + + return theta_star; +} + +float BoundingBoxShapeModel::boostOptimize( + const pcl::PointCloud & cluster, const float min_angle, const float max_angle) +{ + auto closeness_func = [&](float theta) { + Eigen::Vector2f e_1; + e_1 << std::cos(theta), std::sin(theta); // col.3, Algo.2 + Eigen::Vector2f e_2; + e_2 << -std::sin(theta), std::cos(theta); // col.4, Algo.2 + std::vector C_1; // col.5, Algo.2 + std::vector C_2; // col.6, Algo.2 + for (const auto & point : cluster) { + C_1.push_back(point.x * e_1.x() + point.y * e_1.y()); + C_2.push_back(point.x * e_2.x() + point.y * e_2.y()); + } + float q = calcClosenessCriterion(C_1, C_2); + return -q; + }; + + int bits = 6; + boost::uintmax_t max_iter = 20; + std::pair min = boost::math::tools::brent_find_minima( + closeness_func, min_angle, max_angle + epsilon, bits, max_iter); + float theta_star = min.first; + return theta_star; +} diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index 5e337a3790ca..2f3f6c42a84d 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -23,8 +23,10 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; -ShapeEstimator::ShapeEstimator(bool use_corrector, bool use_filter) -: use_corrector_(use_corrector), use_filter_(use_filter) +ShapeEstimator::ShapeEstimator(bool use_corrector, bool use_filter, bool use_boost_bbox_optimizer) +: use_corrector_(use_corrector), + use_filter_(use_filter), + use_boost_bbox_optimizer_(use_boost_bbox_optimizer) { } @@ -69,14 +71,10 @@ bool ShapeEstimator::estimateOriginalShapeAndPose( std::unique_ptr model_ptr; if ( label == Label::CAR || label == Label::TRUCK || label == Label::BUS || - label == Label::TRAILER) { - model_ptr.reset(new BoundingBoxShapeModel(ref_yaw_info)); + label == Label::TRAILER || label == Label::MOTORCYCLE || label == Label::BICYCLE) { + model_ptr.reset(new BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer_)); } else if (label == Label::PEDESTRIAN) { model_ptr.reset(new CylinderShapeModel()); - } else if (label == Label::MOTORCYCLE) { - model_ptr.reset(new BoundingBoxShapeModel(ref_yaw_info)); - } else if (label == Label::BICYCLE) { - model_ptr.reset(new BoundingBoxShapeModel(ref_yaw_info)); } else { model_ptr.reset(new ConvexHullShapeModel()); } diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 7752681d6ff4..efdf7ddb23bf 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -30,6 +30,7 @@ #endif #include +#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -44,7 +45,10 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option bool use_corrector = declare_parameter("use_corrector", true); bool use_filter = declare_parameter("use_filter", true); use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw", true); - estimator_ = std::make_unique(use_corrector, use_filter); + bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer", false); + RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); + estimator_ = + std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); } void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg)