From 15d9be7d52fcaea4dafa3161c0d3c05ca782bb74 Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Mon, 7 Mar 2022 02:47:25 +0900 Subject: [PATCH 1/9] add iou gate Signed-off-by: Yukihiro Saito --- .../config/data_association_matrix.param.yaml | 10 +++ .../data_association/data_association.hpp | 3 +- .../multi_object_tracker/utils/utils.hpp | 5 ++ .../src/data_association/data_association.cpp | 80 ++++++++++++------- .../src/multi_object_tracker_core.cpp | 4 +- .../multi_object_tracker/src/utils/utils.cpp | 65 +++++++-------- 6 files changed, 102 insertions(+), 65 deletions(-) diff --git a/perception/multi_object_tracker/config/data_association_matrix.param.yaml b/perception/multi_object_tracker/config/data_association_matrix.param.yaml index 393bb949d048d..c2ab698dd2d2a 100644 --- a/perception/multi_object_tracker/config/data_association_matrix.param.yaml +++ b/perception/multi_object_tracker/config/data_association_matrix.param.yaml @@ -50,3 +50,13 @@ 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #PEDESTRIAN 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #ANIMAL + min_iou_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.1, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, #BUS + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #PEDESTRIAN + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #ANIMAL diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp index 23feca474fe4f..a29fcdaa4e1e5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp @@ -41,6 +41,7 @@ class DataAssociation Eigen::MatrixXd max_area_matrix_; Eigen::MatrixXd min_area_matrix_; Eigen::MatrixXd max_rad_matrix_; + Eigen::MatrixXd min_iou_matrix_; const double score_threshold_; std::unique_ptr gnn_solver_ptr_; @@ -49,7 +50,7 @@ class DataAssociation DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_area_vector, std::vector min_area_vector, - std::vector max_rad_vector); + std::vector max_rad_vector, std::vector min_iou_vector); void assign( const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 00ba413700901..0e63677af1958 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -36,6 +36,11 @@ double getPolygonArea(const geometry_msgs::msg::Polygon & footprint); double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions); double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions); double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); +double get2dIoU( + const std::tuple< + const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object1, + const std::tuple< + const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object2); double get2dIoU( const autoware_auto_perception_msgs::msg::TrackedObject & object1, const autoware_auto_perception_msgs::msg::TrackedObject & object2); diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 42923a59c7f0b..a36db0ede4cf4 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -79,7 +79,7 @@ double getFormedYawAngle( DataAssociation::DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_area_vector, std::vector min_area_vector, - std::vector max_rad_vector) + std::vector max_rad_vector, std::vector min_iou_vector) : score_threshold_(0.01) { { @@ -112,6 +112,12 @@ DataAssociation::DataAssociation( max_rad_vector.data(), max_rad_label_num, max_rad_label_num); max_rad_matrix_ = max_rad_matrix_tmp.transpose(); } + { + const int min_iou_label_num = static_cast(std::sqrt(min_iou_vector.size())); + Eigen::Map min_iou_matrix_tmp( + max_rad_vector.data(), min_iou_label_num, min_iou_label_num); + min_iou_matrix_ = min_iou_matrix_tmp.transpose(); + } gnn_solver_ptr_ = std::make_unique(); } @@ -167,44 +173,60 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( utils::getHighestProbLabel(measurement_object.classification); double score = 0.0; - const geometry_msgs::msg::PoseWithCovariance tracker_pose_covariance = - (*tracker_itr)->getPoseWithCovariance(measurements.header.stamp); if (can_assign_matrix_(tracker_label, measurement_label)) { + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); + const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double max_area = max_area_matrix_(tracker_label, measurement_label); - const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double max_rad = max_rad_matrix_(tracker_label, measurement_label); const double dist = getDistance( measurement_object.kinematics.pose_with_covariance.pose.position, - tracker_pose_covariance.pose.position); - const double area = utils::getArea(measurement_object.shape); - score = (max_dist - std::min(dist, max_dist)) / max_dist; + tracked_object.kinematics.pose_with_covariance.pose.position); + bool passed_gate = true; // dist gate - if (max_dist < dist) { - score = 0.0; - // area gate - } else if (area < min_area || max_area < area) { - score = 0.0; - // angle gate - } else if (std::fabs(max_rad) < M_PI) { + if (passed_gate) { + if (max_dist < dist) passed_gate = false; + } + // area gate + if (passed_gate) { + const double max_area = max_area_matrix_(tracker_label, measurement_label); + const double min_area = min_area_matrix_(tracker_label, measurement_label); + const double area = utils::getArea(measurement_object.shape); + if (area < min_area || max_area < area) passed_gate = false; + } + // angle gate + if (passed_gate) { + const double max_rad = max_rad_matrix_(tracker_label, measurement_label); const double angle = getFormedYawAngle( measurement_object.kinematics.pose_with_covariance.pose.orientation, - tracker_pose_covariance.pose.orientation, false); - if (std::fabs(max_rad) < std::fabs(angle)) { - score = 0.0; - } - // mahalanobis dist gate - } else if (score < score_threshold_) { - double mahalanobis_dist = getMahalanobisDistance( - measurements.objects.at(measurement_idx).kinematics.pose_with_covariance.pose.position, - tracker_pose_covariance.pose.position, getXYCovariance(tracker_pose_covariance)); - - if (2.448 /*95%*/ <= mahalanobis_dist) { - score = 0.0; - } + tracked_object.kinematics.pose_with_covariance.pose.orientation, false); + if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) + passed_gate = false; + } + // mahalanobis dist gate + if (passed_gate) { + const double mahalanobis_dist = getMahalanobisDistance( + measurement_object.kinematics.pose_with_covariance.pose.position, + tracked_object.kinematics.pose_with_covariance.pose.position, + getXYCovariance(tracked_object.kinematics.pose_with_covariance)); + if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = false; + } + // 2d iou gate + if (passed_gate) { + const double min_iou = min_iou_matrix_(tracker_label, measurement_label); + const double iou = utils::get2dIoU( + {measurement_object.kinematics.pose_with_covariance.pose, measurement_object.shape}, + {tracked_object.kinematics.pose_with_covariance.pose, tracked_object.shape}); + if (iou < min_iou) passed_gate = false; + } + + // all gate is passed + if (passed_gate) { + score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (score < score_threshold_) score = 0.0; } } + score_matrix(tracker_idx, measurement_idx) = score; } } diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 6ec99f44fa6ce..d5c2946843365 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -194,9 +194,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); + const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); data_association_ = std::make_unique( - can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix); + can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, + min_iou_matrix); } void MultiObjectTracker::onMeasurement( diff --git a/perception/multi_object_tracker/src/utils/utils.cpp b/perception/multi_object_tracker/src/utils/utils.cpp index 79dfeb92359ea..315cddb395377 100644 --- a/perception/multi_object_tracker/src/utils/utils.cpp +++ b/perception/multi_object_tracker/src/utils/utils.cpp @@ -11,10 +11,6 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -// -// -// Author: v1.0 Yukihiro Saito -// #include "multi_object_tracker/utils/utils.hpp" @@ -29,7 +25,7 @@ namespace utils { void toPolygon2d( - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, tier4_autoware_utils::Polygon2d & output); bool isClockWise(const tier4_autoware_utils::Polygon2d & polygon); tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_autoware_utils::Polygon2d & polygon); @@ -51,8 +47,8 @@ double getPolygonArea(const geometry_msgs::msg::Polygon & footprint) { double area = 0.0; - for (int i = 0; i < static_cast(footprint.points.size()); ++i) { - int j = (i + 1) % static_cast(footprint.points.size()); + for (size_t i = 0; i < footprint.points.size(); ++i) { + size_t j = (i + 1) % footprint.points.size(); area += 0.5 * (footprint.points.at(i).x * footprint.points.at(j).y - footprint.points.at(j).x * footprint.points.at(i).y); } @@ -71,12 +67,14 @@ double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions) } double get2dIoU( - const autoware_auto_perception_msgs::msg::TrackedObject & object1, - const autoware_auto_perception_msgs::msg::TrackedObject & object2) + const std::tuple< + const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object1, + const std::tuple< + const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object2) { tier4_autoware_utils::Polygon2d polygon1, polygon2; - toPolygon2d(object1, polygon1); - toPolygon2d(object2, polygon2); + toPolygon2d(std::get<0>(object1), std::get<1>(object1), polygon1); + toPolygon2d(std::get<0>(object2), std::get<1>(object2), polygon2); std::vector union_polygons; std::vector intersection_polygons; @@ -91,14 +89,19 @@ double get2dIoU( for (const auto & intersection_polygon : intersection_polygons) { intersection_area += boost::geometry::area(intersection_polygon); } - double iou; - if (union_area < 0.01) { - iou = 0.0f; - } - iou = std::min(1.0, intersection_area / union_area); + const double iou = union_area < 0.01 ? 0.0 : std::min(1.0, intersection_area / union_area); return iou; } +double get2dIoU( + const autoware_auto_perception_msgs::msg::TrackedObject & object1, + const autoware_auto_perception_msgs::msg::TrackedObject & object2) +{ + return get2dIoU( + {object1.kinematics.pose_with_covariance.pose, object1.shape}, + {object2.kinematics.pose_with_covariance.pose, object2.shape}); +} + tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_autoware_utils::Polygon2d & polygon) { tier4_autoware_utils::Polygon2d inverted_polygon; @@ -124,23 +127,22 @@ bool isClockWise(const tier4_autoware_utils::Polygon2d & polygon) } void toPolygon2d( - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, tier4_autoware_utils::Polygon2d & output) { - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - const auto & pose = object.kinematics.pose_with_covariance.pose; - double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); Eigen::Matrix2d rotation; rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); Eigen::Vector2d offset0, offset1, offset2, offset3; offset0 = rotation * - Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + Eigen::Vector2d(shape.dimensions.x * 0.5f, shape.dimensions.y * 0.5f); offset1 = rotation * - Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); + Eigen::Vector2d(shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); offset2 = rotation * - Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); + Eigen::Vector2d(-shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); offset3 = rotation * - Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + Eigen::Vector2d(-shape.dimensions.x * 0.5f, shape.dimensions.y * 0.5f); output.outer().push_back(boost::geometry::make( pose.position.x + offset0.x(), pose.position.y + offset0.y())); output.outer().push_back(boost::geometry::make( @@ -150,9 +152,9 @@ void toPolygon2d( output.outer().push_back(boost::geometry::make( pose.position.x + offset3.x(), pose.position.y + offset3.y())); output.outer().push_back(output.outer().front()); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - const auto & center = object.kinematics.pose_with_covariance.pose.position; - const auto & radius = object.shape.dimensions.x * 0.5; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + const auto & center = pose.position; + const auto & radius = shape.dimensions.x * 0.5; constexpr int n = 6; for (int i = 0; i < n; ++i) { Eigen::Vector2d point; @@ -170,13 +172,8 @@ void toPolygon2d( boost::geometry::make(point.x(), point.y())); } output.outer().push_back(output.outer().front()); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - const auto & pose = object.kinematics.pose_with_covariance.pose; - // don't use yaw - // double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); - // Eigen::Matrix2d rotation; - // rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); - for (const auto & point : object.shape.footprint.points) { + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + for (const auto & point : shape.footprint.points) { output.outer().push_back(boost::geometry::make( pose.position.x + point.x, pose.position.y + point.y)); } From 7af48dffd2c2d9642a4826921b9a244f3ae92d06 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sun, 6 Mar 2022 17:49:14 +0000 Subject: [PATCH 2/9] ci(pre-commit): autofix --- perception/multi_object_tracker/src/utils/utils.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/perception/multi_object_tracker/src/utils/utils.cpp b/perception/multi_object_tracker/src/utils/utils.cpp index 315cddb395377..556ffb8fcdc5f 100644 --- a/perception/multi_object_tracker/src/utils/utils.cpp +++ b/perception/multi_object_tracker/src/utils/utils.cpp @@ -135,14 +135,10 @@ void toPolygon2d( Eigen::Matrix2d rotation; rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); Eigen::Vector2d offset0, offset1, offset2, offset3; - offset0 = rotation * - Eigen::Vector2d(shape.dimensions.x * 0.5f, shape.dimensions.y * 0.5f); - offset1 = rotation * - Eigen::Vector2d(shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); - offset2 = rotation * - Eigen::Vector2d(-shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); - offset3 = rotation * - Eigen::Vector2d(-shape.dimensions.x * 0.5f, shape.dimensions.y * 0.5f); + offset0 = rotation * Eigen::Vector2d(shape.dimensions.x * 0.5f, shape.dimensions.y * 0.5f); + offset1 = rotation * Eigen::Vector2d(shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); + offset2 = rotation * Eigen::Vector2d(-shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); + offset3 = rotation * Eigen::Vector2d(-shape.dimensions.x * 0.5f, shape.dimensions.y * 0.5f); output.outer().push_back(boost::geometry::make( pose.position.x + offset0.x(), pose.position.y + offset0.y())); output.outer().push_back(boost::geometry::make( From 932fd56402ca83388702832086b5268d5f5b7671 Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Mon, 7 Mar 2022 23:20:12 +0900 Subject: [PATCH 3/9] cosmetic change Signed-off-by: Yukihiro Saito --- .../config/data_association_matrix.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/config/data_association_matrix.param.yaml b/perception/multi_object_tracker/config/data_association_matrix.param.yaml index c2ab698dd2d2a..385eb335c3879 100644 --- a/perception/multi_object_tracker/config/data_association_matrix.param.yaml +++ b/perception/multi_object_tracker/config/data_association_matrix.param.yaml @@ -50,7 +50,7 @@ 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #PEDESTRIAN 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #ANIMAL - min_iou_matrix: # If value is greater than pi, it will be ignored. + min_iou_matrix: #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN 0.1, 0.3, 0.2, 0.2, 0.1, 0.1, 0.1, 0.1, #CAR From 01610a3ca232eda6aadbeabd0e54a9225964e49f Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Mon, 7 Mar 2022 23:20:20 +0900 Subject: [PATCH 4/9] fix bug Signed-off-by: Yukihiro Saito --- .../src/data_association/data_association.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index a36db0ede4cf4..4d08a5f7b2dd9 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -115,7 +115,7 @@ DataAssociation::DataAssociation( { const int min_iou_label_num = static_cast(std::sqrt(min_iou_vector.size())); Eigen::Map min_iou_matrix_tmp( - max_rad_vector.data(), min_iou_label_num, min_iou_label_num); + min_iou_vector.data(), min_iou_label_num, min_iou_label_num); min_iou_matrix_ = min_iou_matrix_tmp.transpose(); } @@ -217,7 +217,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const double iou = utils::get2dIoU( {measurement_object.kinematics.pose_with_covariance.pose, measurement_object.shape}, {tracked_object.kinematics.pose_with_covariance.pose, tracked_object.shape}); - if (iou < min_iou) passed_gate = false; + // if (iou < min_iou) passed_gate = false; } // all gate is passed @@ -226,7 +226,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (score < score_threshold_) score = 0.0; } } - + std::cerr << __LINE__ <<":" << score << std::endl; score_matrix(tracker_idx, measurement_idx) = score; } } From cb1b3cf756fb7d01545a164dab7d5162fbf3e256 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 7 Mar 2022 14:21:28 +0000 Subject: [PATCH 5/9] ci(pre-commit): autofix --- .../src/data_association/data_association.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 4d08a5f7b2dd9..b1d1dfbcf8d22 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -226,7 +226,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (score < score_threshold_) score = 0.0; } } - std::cerr << __LINE__ <<":" << score << std::endl; + std::cerr << __LINE__ << ":" << score << std::endl; score_matrix(tracker_idx, measurement_idx) = score; } } From 5c707576649dc9f57d6726ba5c083de2f2f3e6f5 Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Tue, 8 Mar 2022 11:29:15 +0900 Subject: [PATCH 6/9] fix bug Signed-off-by: Yukihiro Saito --- .../src/data_association/data_association.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index b1d1dfbcf8d22..4d3d83e20f3c0 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -217,7 +217,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const double iou = utils::get2dIoU( {measurement_object.kinematics.pose_with_covariance.pose, measurement_object.shape}, {tracked_object.kinematics.pose_with_covariance.pose, tracked_object.shape}); - // if (iou < min_iou) passed_gate = false; + if (iou < min_iou) passed_gate = false; } // all gate is passed @@ -226,7 +226,6 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (score < score_threshold_) score = 0.0; } } - std::cerr << __LINE__ << ":" << score << std::endl; score_matrix(tracker_idx, measurement_idx) = score; } } From 7ae2fb22e56c2fcc7d722ada0de153b6d8171747 Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Thu, 10 Mar 2022 23:21:32 +0900 Subject: [PATCH 7/9] fix tier4_launch Signed-off-by: Yukihiro Saito --- .../data_association_matrix.param.yaml | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml index 393bb949d048d..385eb335c3879 100644 --- a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -50,3 +50,13 @@ 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #PEDESTRIAN 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #ANIMAL + min_iou_matrix: + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.1, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, #BUS + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #PEDESTRIAN + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #ANIMAL From b6269fd9b782fb7608653a615ef01b3646774858 Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Thu, 17 Mar 2022 12:48:43 +0900 Subject: [PATCH 8/9] fix include header by cpplint Signed-off-by: Yukihiro Saito --- .../include/multi_object_tracker/utils/utils.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 0e63677af1958..95e7af5573f55 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -29,6 +29,7 @@ #include #include +#include namespace utils { From 9b4930d627f17a803d89fda9cd358a9865477472 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 17 Mar 2022 03:49:51 +0000 Subject: [PATCH 9/9] ci(pre-commit): autofix --- .../include/multi_object_tracker/utils/utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 95e7af5573f55..58ff17c0c9d68 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -28,8 +28,8 @@ #include #include -#include #include +#include namespace utils {