feat(shape_estimation): add boost optimizer (#1292)

* feat(shape_estimation): add boost optimizer

Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>

* ci(pre-commit): autofix

* change default param

Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>

* move boost into bbox class

Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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<float> & C_1, const std::vector<float> & C_2);
+  float optimize(
+    const pcl::PointCloud<pcl::PointXYZ> & cluster, const float min_angle, const float max_angle);
+  float boostOptimize(
+    const pcl::PointCloud<pcl::PointXYZ> & cluster, const float min_angle, const float max_angle);
 
 public:
   BoundingBoxShapeModel();
-  explicit BoundingBoxShapeModel(const boost::optional<ReferenceYawInfo> & ref_yaw_info);
+  explicit BoundingBoxShapeModel(
+    const boost::optional<ReferenceYawInfo> & ref_yaw_info, bool use_boost_bbox_optimizer = false);
   boost::optional<ReferenceYawInfo> ref_yaw_info_;
+  bool use_boost_bbox_optimizer_;
 
   ~BoundingBoxShapeModel() {}
 

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;
 

perception/shape_estimation/launch/shape_estimation.launch.xml

@@ -5,11 +5,13 @@
   <arg name="use_corrector" default="true"/>
   <arg name="node_name" default="shape_estimation"/>
   <arg name="use_vehicle_reference_yaw" default="false"/>
+  <arg name="use_boost_bbox_optimizer" default="false"/>
   <node pkg="shape_estimation" exec="shape_estimation" name="$(var node_name)" output="screen">
     <remap from="input" to="$(var input/objects)"/>
     <remap from="objects" to="$(var output/objects)"/>
     <param name="use_filter" value="$(var use_filter)"/>
     <param name="use_corrector" value="$(var use_corrector)"/>
     <param name="use_vehicle_reference_yaw" value="$(var use_vehicle_reference_yaw)"/>
+    <param name="use_boost_bbox_optimizer" value="$(var use_boost_bbox_optimizer)"/>
   </node>
 </launch>

perception/shape_estimation/lib/model/bounding_box.cpp

@@ -20,6 +20,8 @@
 
 #include <autoware_auto_perception_msgs/msg/shape.hpp>
 
+#include <boost/math/tools/minima.hpp>
+
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
@@ -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<ReferenceYawInfo> & ref_yaw_info)
-: ref_yaw_info_(ref_yaw_info)
+BoundingBoxShapeModel::BoundingBoxShapeModel(
+  const boost::optional<ReferenceYawInfo> & 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<std::pair<float /*theta*/, float /*q*/>> 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<float> C_1;                    // col.5, Algo.2
-    std::vector<float> 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<pcl::PointXYZ> & cluster, const float min_angle, const float max_angle)
+{
+  std::vector<std::pair<float /*theta*/, float /*q*/>> 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<float> C_1;                    // col.5, Algo.2
+    std::vector<float> 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<pcl::PointXYZ> & 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<float> C_1;                    // col.5, Algo.2
+    std::vector<float> 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<float, float> 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;
+}

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<ShapeEstimationModelInterface> 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());
   }

perception/shape_estimation/src/node.cpp

@@ -30,6 +30,7 @@
 #endif
 
 #include <memory>
+#include <string>
 
 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<ShapeEstimator>(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<ShapeEstimator>(use_corrector, use_filter, use_boost_bbox_optimizer);
 }
 
 void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg)