feat(detection_by_tracker, shape_estimation): modify reference yaw to…

… add search range (autowarefoundation#428)

* modify ref yaw to add search range in shape estimation

* ci(pre-commit): autofix

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

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -26,6 +26,7 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
+using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 namespace
 {
 std::optional<geometry_msgs::msg::Transform> getTransform(
@@ -119,6 +120,16 @@ autoware_auto_perception_msgs::msg::DetectedObjects toDetectedObjects(
   return out_objects;
 }
 
+boost::optional<ReferenceYawInfo> getReferenceYawInfo(const uint8_t label, const float yaw)
+{
+  const bool is_vehicle =
+    Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label;
+  if (is_vehicle) {
+    return ReferenceYawInfo{yaw, tier4_autoware_utils::deg2rad(30)};
+  } else {
+    return boost::none;
+  }
+}
 }  // namespace
 
 void TrackerHandler::onTrackedObjects(
@@ -321,6 +332,8 @@ float DetectionByTracker::optimizeUnderSegmentedObject(
   constexpr float initial_voxel_size = initial_cluster_range / 2.0f;
   float voxel_size = initial_voxel_size;
 
+  const auto & label = target_object.classification.front().label;
+
   // initialize clustering parameters
   euclidean_cluster::VoxelGridBasedEuclideanCluster cluster(
     false, 4, 10000, initial_cluster_range, initial_voxel_size, 0);
@@ -346,8 +359,9 @@ float DetectionByTracker::optimizeUnderSegmentedObject(
     highest_iou_object_in_current_iter.object.classification = target_object.classification;
     for (const auto & divided_cluster : divided_clusters) {
       bool is_shape_estimated = shape_estimator_->estimateShapeAndPose(
-        target_object.classification.front().label, divided_cluster,
-        tf2::getYaw(target_object.kinematics.pose_with_covariance.pose.orientation),
+        label, divided_cluster,
+        getReferenceYawInfo(
+          label, tf2::getYaw(target_object.kinematics.pose_with_covariance.pose.orientation)),
         highest_iou_object_in_current_iter.object.shape,
         highest_iou_object_in_current_iter.object.kinematics.pose_with_covariance.pose);
       if (!is_shape_estimated) {
@@ -391,6 +405,7 @@ void DetectionByTracker::mergeOverSegmentedObjects(
   out_no_found_tracked_objects.header = tracked_objects.header;
 
   for (const auto & tracked_object : tracked_objects.objects) {
+    const auto & label = tracked_object.classification.front().label;
     // extend shape
     autoware_auto_perception_msgs::msg::DetectedObject extended_tracked_object = tracked_object;
     extended_tracked_object.shape = extendShape(tracked_object.shape, /*scale*/ 1.1);
@@ -426,8 +441,9 @@ void DetectionByTracker::mergeOverSegmentedObjects(
     feature_object.object.classification = tracked_object.classification;
 
     bool is_shape_estimated = shape_estimator_->estimateShapeAndPose(
-      tracked_object.classification.front().label, pcl_merged_cluster,
-      tf2::getYaw(tracked_object.kinematics.pose_with_covariance.pose.orientation),
+      label, pcl_merged_cluster,
+      getReferenceYawInfo(
+        label, tf2::getYaw(tracked_object.kinematics.pose_with_covariance.pose.orientation)),
       feature_object.object.shape, feature_object.object.kinematics.pose_with_covariance.pose);
     if (!is_shape_estimated) {
       out_no_found_tracked_objects.objects.push_back(tracked_object);

perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp

@@ -38,8 +38,7 @@ class BusCorrector : public ShapeEstimationCorrectorInterface
   ~BusCorrector() = default;
 
   bool correct(
-    autoware_auto_perception_msgs::msg::Shape & shape_output,
-    geometry_msgs::msg::Pose & pose_output) override;
+    autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) override;
 };
 
 #endif  // SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_

perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp

@@ -38,8 +38,7 @@ class CarCorrector : public ShapeEstimationCorrectorInterface
   ~CarCorrector() = default;
 
   bool correct(
-    autoware_auto_perception_msgs::msg::Shape & shape_output,
-    geometry_msgs::msg::Pose & pose_output) override;
+    autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) override;
 };
 
 #endif  // SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_

perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp

@@ -28,8 +28,7 @@ class ShapeEstimationCorrectorInterface
   virtual ~ShapeEstimationCorrectorInterface() {}
 
   virtual bool correct(
-    autoware_auto_perception_msgs::msg::Shape & shape_output,
-    geometry_msgs::msg::Pose & pose_output) = 0;
+    autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) = 0;
 };
 
 #endif  // SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_

perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp

@@ -26,8 +26,7 @@ class NoCorrector : public ShapeEstimationCorrectorInterface
   ~NoCorrector() {}
 
   bool correct(
-    autoware_auto_perception_msgs::msg::Shape & shape_output,
-    geometry_msgs::msg::Pose & pose_output) override;
+    autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) override;
 };
 
 #endif  // SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_

perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp

@@ -38,8 +38,7 @@ class TruckCorrector : public ShapeEstimationCorrectorInterface
   ~TruckCorrector() = default;
 
   bool correct(
-    autoware_auto_perception_msgs::msg::Shape & shape_output,
-    geometry_msgs::msg::Pose & pose_output) override;
+    autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) override;
 };
 
 #endif  // SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_

perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp

@@ -16,6 +16,7 @@
 #define SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_
 
 #include "shape_estimation/model/model_interface.hpp"
+#include "shape_estimation/shape_estimator.hpp"
 
 #include <vector>
 
@@ -30,8 +31,8 @@ class BoundingBoxShapeModel : public ShapeEstimationModelInterface
 
 public:
   BoundingBoxShapeModel();
-  explicit BoundingBoxShapeModel(const boost::optional<float> & reference_yaw);
-  boost::optional<float> reference_yaw_;
+  explicit BoundingBoxShapeModel(const boost::optional<ReferenceYawInfo> & ref_yaw_info);
+  boost::optional<ReferenceYawInfo> ref_yaw_info_;
 
   ~BoundingBoxShapeModel() {}
 

perception/shape_estimation/include/shape_estimation/shape_estimator.hpp

@@ -27,20 +27,26 @@
 
 #include <string>
 
+struct ReferenceYawInfo
+{
+  float yaw;
+  float search_angle_range;
+};
+
 class ShapeEstimator
 {
 private:
-  bool estimateShape(
+  bool estimateOriginalShapeAndPose(
     const uint8_t label, const pcl::PointCloud<pcl::PointXYZ> & cluster,
-    const boost::optional<float> & yaw, autoware_auto_perception_msgs::msg::Shape & shape_output,
+    const boost::optional<ReferenceYawInfo> & ref_yaw_info,
+    autoware_auto_perception_msgs::msg::Shape & shape_output,
     geometry_msgs::msg::Pose & pose_output);
   bool applyFilter(
-    const uint8_t label, const autoware_auto_perception_msgs::msg::Shape & shape_output,
-    const geometry_msgs::msg::Pose & pose_output);
+    const uint8_t label, const autoware_auto_perception_msgs::msg::Shape & shape,
+    const geometry_msgs::msg::Pose & pose);
   bool applyCorrector(
     const uint8_t label, const bool use_reference_yaw,
-    autoware_auto_perception_msgs::msg::Shape & shape_output,
-    geometry_msgs::msg::Pose & pose_output);
+    autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose);
 
   bool use_corrector_;
   bool use_filter_;
@@ -52,7 +58,8 @@ class ShapeEstimator
 
   virtual bool estimateShapeAndPose(
     const uint8_t label, const pcl::PointCloud<pcl::PointXYZ> & cluster,
-    const boost::optional<float> & yaw, autoware_auto_perception_msgs::msg::Shape & shape_output,
+    const boost::optional<ReferenceYawInfo> & ref_yaw_info,
+    autoware_auto_perception_msgs::msg::Shape & shape_output,
     geometry_msgs::msg::Pose & pose_output);
 };
 

perception/shape_estimation/lib/corrector/bus_corrector.cpp

@@ -15,10 +15,10 @@
 #include "shape_estimation/corrector/bus_corrector.hpp"
 
 bool BusCorrector::correct(
-  autoware_auto_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
+  autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose)
 {
   if (use_reference_yaw_) {
-    return correctVehicleBoundingBoxWithReferenceYaw(params_, shape_output, pose_output);
+    return correctVehicleBoundingBoxWithReferenceYaw(params_, shape, pose);
   }
-  return correctVehicleBoundingBox(params_, shape_output, pose_output);
+  return correctVehicleBoundingBox(params_, shape, pose);
 }

perception/shape_estimation/lib/corrector/car_corrector.cpp

@@ -15,10 +15,10 @@
 #include "shape_estimation/corrector/car_corrector.hpp"
 
 bool CarCorrector::correct(
-  autoware_auto_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
+  autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose)
 {
   if (use_reference_yaw_) {
-    return correctVehicleBoundingBoxWithReferenceYaw(params_, shape_output, pose_output);
+    return correctVehicleBoundingBoxWithReferenceYaw(params_, shape, pose);
   }
-  return correctVehicleBoundingBox(params_, shape_output, pose_output);
+  return correctVehicleBoundingBox(params_, shape, pose);
 }

perception/shape_estimation/lib/corrector/truck_corrector.cpp

@@ -15,10 +15,10 @@
 #include "shape_estimation/corrector/truck_corrector.hpp"
 
 bool TruckCorrector::correct(
-  autoware_auto_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
+  autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose)
 {
   if (use_reference_yaw_) {
-    return correctVehicleBoundingBoxWithReferenceYaw(params_, shape_output, pose_output);
+    return correctVehicleBoundingBoxWithReferenceYaw(params_, shape, pose);
   }
-  return correctVehicleBoundingBox(params_, shape_output, pose_output);
+  return correctVehicleBoundingBox(params_, shape, pose);
 }

perception/shape_estimation/lib/corrector/utils.cpp

@@ -30,14 +30,14 @@
 namespace utils
 {
 bool correctVehicleBoundingBox(
-  const CorrectionParameters & param, autoware_auto_perception_msgs::msg::Shape & shape_output,
-  geometry_msgs::msg::Pose & pose_output)
+  const CorrectionParameters & param, autoware_auto_perception_msgs::msg::Shape & shape,
+  geometry_msgs::msg::Pose & pose)
 {
   // TODO(Yukihiro Saito): refactor following code
 
   Eigen::Translation<double, 2> trans =
-    Eigen::Translation<double, 2>(pose_output.position.x, pose_output.position.y);
-  Eigen::Rotation2Dd rotate(tf2::getYaw(pose_output.orientation));
+    Eigen::Translation<double, 2>(pose.position.x, pose.position.y);
+  Eigen::Rotation2Dd rotate(tf2::getYaw(pose.orientation));
   Eigen::Affine2d affine_mat;
   affine_mat = trans * rotate.toRotationMatrix();
 
@@ -52,10 +52,10 @@ bool correctVehicleBoundingBox(
    *         |
    */
   std::vector<Eigen::Vector2d> v_point;
-  v_point.push_back(Eigen::Vector2d(shape_output.dimensions.x / 2.0, 0.0));
-  v_point.push_back(Eigen::Vector2d(0.0, shape_output.dimensions.y / 2.0));
-  v_point.push_back(Eigen::Vector2d(-shape_output.dimensions.x / 2.0, 0.0));
-  v_point.push_back(Eigen::Vector2d(0.0, -shape_output.dimensions.y / 2.0));
+  v_point.push_back(Eigen::Vector2d(shape.dimensions.x / 2.0, 0.0));
+  v_point.push_back(Eigen::Vector2d(0.0, shape.dimensions.y / 2.0));
+  v_point.push_back(Eigen::Vector2d(-shape.dimensions.x / 2.0, 0.0));
+  v_point.push_back(Eigen::Vector2d(0.0, -shape.dimensions.y / 2.0));
 
   // most distant index from base link
   size_t first_most_distant_index = 0;
@@ -223,27 +223,27 @@ bool correctVehicleBoundingBox(
     return false;
   }
 
-  shape_output.dimensions.x += std::abs(correction_vector.x()) * 2.0;
-  shape_output.dimensions.y += std::abs(correction_vector.y()) * 2.0;
-  pose_output.position.x += (rotate.toRotationMatrix() * correction_vector).x();
-  pose_output.position.y += (rotate.toRotationMatrix() * correction_vector).y();
+  shape.dimensions.x += std::abs(correction_vector.x()) * 2.0;
+  shape.dimensions.y += std::abs(correction_vector.y()) * 2.0;
+  pose.position.x += (rotate.toRotationMatrix() * correction_vector).x();
+  pose.position.y += (rotate.toRotationMatrix() * correction_vector).y();
 
   // correct to set long length is x, short length is y
-  if (shape_output.dimensions.x < shape_output.dimensions.y) {
-    geometry_msgs::msg::Vector3 rpy = tier4_autoware_utils::getRPY(pose_output.orientation);
+  if (shape.dimensions.x < shape.dimensions.y) {
+    geometry_msgs::msg::Vector3 rpy = tier4_autoware_utils::getRPY(pose.orientation);
     rpy.z = rpy.z + M_PI_2;
-    pose_output.orientation = tier4_autoware_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z);
-    double temp = shape_output.dimensions.x;
-    shape_output.dimensions.x = shape_output.dimensions.y;
-    shape_output.dimensions.y = temp;
+    pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z);
+    double temp = shape.dimensions.x;
+    shape.dimensions.x = shape.dimensions.y;
+    shape.dimensions.y = temp;
   }
 
   return true;
 }
 
 bool correctVehicleBoundingBoxWithReferenceYaw(
-  const CorrectionParameters & param, autoware_auto_perception_msgs::msg::Shape & shape_output,
-  geometry_msgs::msg::Pose & pose_output)
+  const CorrectionParameters & param, autoware_auto_perception_msgs::msg::Shape & shape,
+  geometry_msgs::msg::Pose & pose)
 {
   /*
     c1 is nearest point and other points are arranged like below
@@ -259,21 +259,18 @@ bool correctVehicleBoundingBoxWithReferenceYaw(
   Eigen::Vector3d c1, c2, c3, c4;
 
   Eigen::Affine3d base2obj_transform;
-  tf2::fromMsg(pose_output, base2obj_transform);
+  tf2::fromMsg(pose, base2obj_transform);
 
   std::vector<Eigen::Vector3d> v_point;
   v_point.push_back(
-    base2obj_transform *
-    Eigen::Vector3d(shape_output.dimensions.x * 0.5, shape_output.dimensions.y * 0.5, 0.0));
+    base2obj_transform * Eigen::Vector3d(shape.dimensions.x * 0.5, shape.dimensions.y * 0.5, 0.0));
   v_point.push_back(
-    base2obj_transform *
-    Eigen::Vector3d(-shape_output.dimensions.x * 0.5, shape_output.dimensions.y * 0.5, 0.0));
+    base2obj_transform * Eigen::Vector3d(-shape.dimensions.x * 0.5, shape.dimensions.y * 0.5, 0.0));
   v_point.push_back(
-    base2obj_transform *
-    Eigen::Vector3d(shape_output.dimensions.x * 0.5, -shape_output.dimensions.y * 0.5, 0.0));
+    base2obj_transform * Eigen::Vector3d(shape.dimensions.x * 0.5, -shape.dimensions.y * 0.5, 0.0));
   v_point.push_back(
     base2obj_transform *
-    Eigen::Vector3d(-shape_output.dimensions.x * 0.5, -shape_output.dimensions.y * 0.5, 0.0));
+    Eigen::Vector3d(-shape.dimensions.x * 0.5, -shape.dimensions.y * 0.5, 0.0));
 
   double distance = std::pow(24, 24);
   size_t nearest_idx = 0;
@@ -295,15 +292,14 @@ bool correctVehicleBoundingBoxWithReferenceYaw(
   Eigen::Vector3d e1 = (Eigen::Vector3d(0, -ey, 0) - local_c1).normalized();
   Eigen::Vector3d e2 = (Eigen::Vector3d(-ex, 0, 0) - local_c1).normalized();
   double length = 0;
-  if (
-    param.min_length < shape_output.dimensions.x && shape_output.dimensions.x < param.max_length) {
-    length = shape_output.dimensions.x;
+  if (param.min_length < shape.dimensions.x && shape.dimensions.x < param.max_length) {
+    length = shape.dimensions.x;
   } else {
     length = param.avg_length;
   }
   double width = 0;
-  if (param.min_width < shape_output.dimensions.y && shape_output.dimensions.y < param.max_width) {
-    width = shape_output.dimensions.y;
+  if (param.min_width < shape.dimensions.y && shape.dimensions.y < param.max_width) {
+    width = shape.dimensions.y;
   } else {
     width = param.avg_width;
   }
@@ -312,12 +308,12 @@ bool correctVehicleBoundingBoxWithReferenceYaw(
   c3 = c1 + base2obj_transform.rotation() * (e2 * width);
   c4 = c1 + (c2 - c1) + (c3 - c1);
 
-  shape_output.dimensions.x = (c2 - c1).norm();
-  shape_output.dimensions.y = (c3 - c1).norm();
+  shape.dimensions.x = (c2 - c1).norm();
+  shape.dimensions.y = (c3 - c1).norm();
   Eigen::Vector3d new_centroid = c1 + ((c4 - c1) * 0.5);
-  pose_output.position.x = new_centroid.x();
-  pose_output.position.y = new_centroid.y();
-  pose_output.position.z = new_centroid.z();
+  pose.position.x = new_centroid.x();
+  pose.position.y = new_centroid.y();
+  pose.position.z = new_centroid.z();
 
   return true;
 }

perception/shape_estimation/lib/model/bounding_box.cpp

@@ -37,10 +37,10 @@
 
 constexpr float epsilon = 0.001;
 
-BoundingBoxShapeModel::BoundingBoxShapeModel() : reference_yaw_(boost::none) {}
+BoundingBoxShapeModel::BoundingBoxShapeModel() : ref_yaw_info_(boost::none) {}
 
-BoundingBoxShapeModel::BoundingBoxShapeModel(const boost::optional<float> & reference_yaw)
-: reference_yaw_(reference_yaw)
+BoundingBoxShapeModel::BoundingBoxShapeModel(const boost::optional<ReferenceYawInfo> & ref_yaw_info)
+: ref_yaw_info_(ref_yaw_info)
 {
 }
 
@@ -49,12 +49,12 @@ bool BoundingBoxShapeModel::estimate(
   autoware_auto_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
 {
   float min_angle, max_angle;
-  if (reference_yaw_) {
-    min_angle = reference_yaw_.get() - tier4_autoware_utils::deg2rad(3);
-    max_angle = reference_yaw_.get() + tier4_autoware_utils::deg2rad(3);
+  if (ref_yaw_info_) {
+    min_angle = ref_yaw_info_.get().yaw - ref_yaw_info_.get().search_angle_range;
+    max_angle = ref_yaw_info_.get().yaw + ref_yaw_info_.get().search_angle_range;
   } else {
     min_angle = 0.0;
-    max_angle = M_PI / 2.0;
+    max_angle = M_PI * 0.5;
   }
   return fitLShape(cluster, min_angle, max_angle, shape_output, pose_output);
 }