Ros2 v0.8.0 shape estimation (autowarefoundation#257)

* fix typos in perception (autowarefoundation#862) * Feature/camera lidar perception (autowarefoundation#937) * add object splitter Signed-off-by: Yukihiro Saito <[email protected]> * add object merger Signed-off-by: Yukihiro Saito <[email protected]> * change pkg name Signed-off-by: Yukihiro Saito <[email protected]> * cosmetic change Signed-off-by: Yukihiro Saito <[email protected]> * add comment Signed-off-by: Yukihiro Saito <[email protected]> * remove litter Signed-off-by: Yukihiro Saito <[email protected]> * bug fix : debug code Signed-off-by: Yukihiro Saito <[email protected]> * enable vehicle to unknown track Signed-off-by: Yukihiro Saito <[email protected]> * bug fix * add object position in clustering * 🚮 * change param * fix name * bug fix * add install Co-authored-by: Kazuki Miyahara <[email protected]> Co-authored-by: Yukihiro Saito <[email protected]>
kosuke55 · Jan 30, 2021 · c1367e6 · c1367e6
1 parent e079e34
commit c1367e6
Show file tree

Hide file tree

Showing 5 changed files with 20 additions and 19 deletions.
diff --git a/...n/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/bus_corrector.cpp b/...n/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/bus_corrector.cpp
@@ -20,10 +20,11 @@ namespace yaw_fixed
 bool BusCorrector::correct(
   autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
 {
-  double min_width = 2.0;
-  double max_width = 2.9;
-  double min_length = 5.0;
-  double max_length = 17.0;
+  const double min_width = 2.0;
+  const double max_width = 2.9;
+  const double min_length = 5.0;
+  const double max_length = 17.0;
+  const double avg_length = 7.0;
 
   /*
     c1 is nearest point and other points are arranged like below
@@ -78,7 +79,7 @@ bool BusCorrector::correct(
   if (min_length < shape_output.dimensions.x && shape_output.dimensions.x < max_length) {
     length = shape_output.dimensions.x;
   } else {
-    length = (max_length + min_length) * 0.5;
+    length = avg_length;
   }
   double width = 0;
   if (min_width < shape_output.dimensions.y && shape_output.dimensions.y < max_width) {

diff --git a/...n/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/car_corrector.cpp b/...n/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/car_corrector.cpp
@@ -20,10 +20,10 @@ namespace yaw_fixed
 bool CarCorrector::correct(
   autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
 {
-  double min_width = 1.2;
-  double max_width = 2.2;
-  double min_length = 3.0;
-  double max_length = 5.0;
+  const double min_width = 1.2;
+  const double max_width = 2.2;
+  const double min_length = 3.0;
+  const double max_length = 5.0;
 
   /*
     c1 is nearest point and other points are arranged like below

diff --git a/...object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/truck_corrector.cpp b/...object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/truck_corrector.cpp
@@ -20,10 +20,10 @@ namespace yaw_fixed
 bool TruckCorrector::correct(
   autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
 {
-  double min_width = 1.5;
-  double max_width = 2.9;
-  double min_length = 4.0;
-  double max_length = 7.9;
+  const double min_width = 1.5;
+  const double max_width = 2.9;
+  const double min_length = 4.0;
+  const double max_length = 7.9;
 
   /*
     c1 is nearest point and other points are arranged like below

diff --git a/perception/object_recognition/detection/shape_estimation/src/model/normal/bounding_box.cpp b/perception/object_recognition/detection/shape_estimation/src/model/normal/bounding_box.cpp
@@ -68,9 +68,9 @@ bool BoundingBoxModel::estimate(
   // Paper : Algo.2 Search-Based Rectangle Fitting
   std::vector<std::pair<double /*theta*/, double /*q*/>> Q;
   const double max_angle = M_PI / 2.0;
-  const double eplison = 0.001;
+  const double epsilon = 0.001;
   const double angle_reso = M_PI / 180.0;
-  for (double theta = 0; theta <= max_angle + eplison; theta += angle_reso) {
+  for (double theta = 0; theta <= max_angle + epsilon; theta += angle_reso) {
     Eigen::Vector2d e_1;
     e_1 << std::cos(theta), std::sin(theta);  // col.3, Algo.2
     Eigen::Vector2d e_2;
@@ -130,7 +130,7 @@ bool BoundingBoxModel::estimate(
   double intersection_x_2 = (b_3 * c_4 - b_4 * c_3) / (a_4 * b_3 - a_3 * b_4);
   double intersection_y_2 = (a_3 * c_4 - a_4 * c_3) / (a_3 * b_4 - a_4 * b_3);
 
-  // calc dimention of bounding box
+  // calc dimension of bounding box
   Eigen::Vector2d e_x;
   Eigen::Vector2d e_y;
   e_x << a_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1)), b_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1));

diff --git a/...eption/object_recognition/detection/shape_estimation/src/model/yaw_fixed/bounding_box.cpp b/...eption/object_recognition/detection/shape_estimation/src/model/yaw_fixed/bounding_box.cpp
@@ -94,9 +94,9 @@ bool BoundingBoxModel::estimate(
 
   // Paper : Algo.2 Search-Based Rectangle Fitting
   std::vector<std::pair<double /*theta*/, double /*q*/>> Q;
-  const double eplison = 0.001;
+  const double epsilon = 0.001;
   const double angle_reso = M_PI / 180.0;
-  for (double theta = min_angle; theta <= max_angle + eplison; theta += angle_reso) {
+  for (double theta = min_angle; theta <= max_angle + epsilon; theta += angle_reso) {
     Eigen::Vector2d e_1;
     e_1 << std::cos(theta), std::sin(theta);  // col.3, Algo.2
     Eigen::Vector2d e_2;
@@ -156,7 +156,7 @@ bool BoundingBoxModel::estimate(
   double intersection_x_2 = (b_3 * c_4 - b_4 * c_3) / (a_4 * b_3 - a_3 * b_4);
   double intersection_y_2 = (a_3 * c_4 - a_4 * c_3) / (a_3 * b_4 - a_4 * b_3);
 
-  // calc dimention of bounding box
+  // calc dimension of bounding box
   Eigen::Vector2d e_x;
   Eigen::Vector2d e_y;
   e_x << a_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1)), b_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1));