From c1367e662a180b66f824be5f7ab4b560af62e224 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Sun, 17 Jan 2021 14:06:24 +0900 Subject: [PATCH] Ros2 v0.8.0 shape estimation (#257) * fix typos in perception (#862) * Feature/camera lidar perception (#937) * add object splitter Signed-off-by: Yukihiro Saito * add object merger Signed-off-by: Yukihiro Saito * change pkg name Signed-off-by: Yukihiro Saito * cosmetic change Signed-off-by: Yukihiro Saito * add comment Signed-off-by: Yukihiro Saito * remove litter Signed-off-by: Yukihiro Saito * bug fix : debug code Signed-off-by: Yukihiro Saito * enable vehicle to unknown track Signed-off-by: Yukihiro Saito * bug fix * add object position in clustering * :put_litter_in_its_place: * change param * fix name * bug fix * add install Co-authored-by: Kazuki Miyahara Co-authored-by: Yukihiro Saito --- .../src/corrector/yaw_fixed/bus_corrector.cpp | 11 ++++++----- .../src/corrector/yaw_fixed/car_corrector.cpp | 8 ++++---- .../src/corrector/yaw_fixed/truck_corrector.cpp | 8 ++++---- .../src/model/normal/bounding_box.cpp | 6 +++--- .../src/model/yaw_fixed/bounding_box.cpp | 6 +++--- 5 files changed, 20 insertions(+), 19 deletions(-) diff --git a/perception/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/bus_corrector.cpp b/perception/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/bus_corrector.cpp index f5ef77e191eaa..cc03e9e87d93d 100644 --- a/perception/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/bus_corrector.cpp +++ b/perception/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/perception/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/car_corrector.cpp b/perception/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/car_corrector.cpp index 107c2635ece6b..f5093359a958a 100644 --- a/perception/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/car_corrector.cpp +++ b/perception/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/perception/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/truck_corrector.cpp b/perception/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/truck_corrector.cpp index 556b518560424..f5d1e67ef8278 100644 --- a/perception/object_recognition/detection/shape_estimation/src/corrector/yaw_fixed/truck_corrector.cpp +++ b/perception/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 index 5302cf5655e71..f930babc1d782 100644 --- 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> 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/perception/object_recognition/detection/shape_estimation/src/model/yaw_fixed/bounding_box.cpp b/perception/object_recognition/detection/shape_estimation/src/model/yaw_fixed/bounding_box.cpp index 454187e083621..a8f48119ad68e 100644 --- a/perception/object_recognition/detection/shape_estimation/src/model/yaw_fixed/bounding_box.cpp +++ b/perception/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> 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));