Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(multi_object_tracker): add iou gate #483

Merged
merged 10 commits into from
Mar 17, 2022
Merged

feat(multi_object_tracker): add iou gate #483

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
15d9be7
add iou gate
yukkysaito Mar 6, 2022
7af48df
ci(pre-commit): autofix
pre-commit-ci[bot] Mar 6, 2022
932fd56
cosmetic change
yukkysaito Mar 7, 2022
01610a3
fix bug
yukkysaito Mar 7, 2022
cb1b3cf
ci(pre-commit): autofix
pre-commit-ci[bot] Mar 7, 2022
5c70757
fix bug
yukkysaito Mar 8, 2022
7ae2fb2
fix tier4_launch
yukkysaito Mar 10, 2022
bd7bde2
Merge branch 'main' into feature/add_iou_gate
yukkysaito Mar 17, 2022
b6269fd
fix include header by cpplint
yukkysaito Mar 17, 2022
9b4930d
ci(pre-commit): autofix
pre-commit-ci[bot] Mar 17, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions ...onfig/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
10 changes: 10 additions & 0 deletions perception/multi_object_tracker/config/data_association_matrix.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
3 changes: 2 additions & 1 deletion ...n/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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::GnnSolverInterface> gnn_solver_ptr_;

Expand All @@ -49,7 +50,7 @@ class DataAssociation
DataAssociation(
std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
std::vector<double> max_area_vector, std::vector<double> min_area_vector,
std::vector<double> max_rad_vector);
std::vector<double> max_rad_vector, std::vector<double> min_iou_vector);
void assign(
const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
std::unordered_map<int, int> & reverse_assignment);
Expand Down
6 changes: 6 additions & 0 deletions perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include <geometry_msgs/msg/vector3.hpp>

#include <cmath>
#include <tuple>
#include <vector>

namespace utils
Expand All @@ -36,6 +37,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);
Expand Down
79 changes: 50 additions & 29 deletions perception/multi_object_tracker/src/data_association/data_association.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ double getFormedYawAngle(
DataAssociation::DataAssociation(
std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
std::vector<double> max_area_vector, std::vector<double> min_area_vector,
std::vector<double> max_rad_vector)
std::vector<double> max_rad_vector, std::vector<double> min_iou_vector)
: score_threshold_(0.01)
{
{
Expand Down Expand Up @@ -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<int>(std::sqrt(min_iou_vector.size()));
Eigen::Map<Eigen::MatrixXd> min_iou_matrix_tmp(
min_iou_vector.data(), min_iou_label_num, min_iou_label_num);
min_iou_matrix_ = min_iou_matrix_tmp.transpose();
}

gnn_solver_ptr_ = std::make_unique<gnn_solver::MuSSP>();
}
Expand Down Expand Up @@ -167,42 +173,57 @@ 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;
Expand Down
4 changes: 3 additions & 1 deletion perception/multi_object_tracker/src/multi_object_tracker_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -194,9 +194,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
const auto max_area_matrix = this->declare_parameter<std::vector<double>>("max_area_matrix");
const auto min_area_matrix = this->declare_parameter<std::vector<double>>("min_area_matrix");
const auto max_rad_matrix = this->declare_parameter<std::vector<double>>("max_rad_matrix");
const auto min_iou_matrix = this->declare_parameter<std::vector<double>>("min_iou_matrix");

data_association_ = std::make_unique<DataAssociation>(
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(
Expand Down
69 changes: 31 additions & 38 deletions perception/multi_object_tracker/src/utils/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand All @@ -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);
Expand All @@ -51,8 +47,8 @@ double getPolygonArea(const geometry_msgs::msg::Polygon & footprint)
{
double area = 0.0;

for (int i = 0; i < static_cast<int>(footprint.points.size()); ++i) {
int j = (i + 1) % static_cast<int>(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);
}
Expand All @@ -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<tier4_autoware_utils::Polygon2d> union_polygons;
std::vector<tier4_autoware_utils::Polygon2d> intersection_polygons;
Expand All @@ -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;
Expand All @@ -124,23 +127,18 @@ 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);
offset1 = rotation *
Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
offset2 = rotation *
Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
offset3 = rotation *
Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.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<tier4_autoware_utils::Point2d>(
pose.position.x + offset0.x(), pose.position.y + offset0.y()));
output.outer().push_back(boost::geometry::make<tier4_autoware_utils::Point2d>(
Expand All @@ -150,9 +148,9 @@ void toPolygon2d(
output.outer().push_back(boost::geometry::make<tier4_autoware_utils::Point2d>(
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;
Expand All @@ -170,13 +168,8 @@ void toPolygon2d(
boost::geometry::make<tier4_autoware_utils::Point2d>(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<tier4_autoware_utils::Point2d>(
pose.position.x + point.x, pose.position.y + point.y));
}
Expand Down