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.

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): tracker refactoring #7271

Merged
Merged
Changes from 1 commit
Commits
Show all changes
31 commits
Select commit Hold shift + click to select a range
d6e50a9
feat: separate filters
technolojin Jun 6, 2024
24f66cd
fix: object validator to modular
technolojin Jun 7, 2024
c5eeaa5
fix: replace xyzrpy covariance index
technolojin May 30, 2024
68d9964
chore: refactor tracker class members
technolojin Jun 4, 2024
6a752a8
chore: refactor
technolojin Jun 4, 2024
a75daa8
chore: refactoring destructor
technolojin Jun 4, 2024
0f9089b
chore: refactoring getUpdatingObject
technolojin Jun 4, 2024
4b0e6cf
chore: refactoring object size checker
technolojin Jun 4, 2024
b963c09
fix: debugger delay calculation order is fixed
technolojin Jun 4, 2024
34057df
fix: bicycle size update bug fix
technolojin Jun 5, 2024
daa4e16
fix: renamed message package
technolojin Jun 5, 2024
8cf586a
chore: refactoring
technolojin Jun 10, 2024
74f08da
feat: object parameter class
technolojin Jun 10, 2024
80806ce
style(pre-commit): autofix
pre-commit-ci[bot] Jun 10, 2024
b316282
chore: refactor includes
technolojin Jun 13, 2024
9fb235c
feat: initial impl. of object model
technolojin Jun 13, 2024
cbb1a58
feat: impl. object model to bicycle, big, normal
technolojin Jun 13, 2024
cb895a9
feat: impl. object model to pedestrian
technolojin Jun 13, 2024
716d1b8
feat: impl. cont
technolojin Jun 13, 2024
2e86d09
style(pre-commit): autofix
pre-commit-ci[bot] Jun 13, 2024
9bdd911
chore: fix missing refactoring
technolojin Jun 13, 2024
ad97751
fix: align unit of angles, yaw rates
technolojin Jun 13, 2024
724dc8e
fix: rename tier4_autoware_utils to autoware_universe_utils
technolojin Jun 18, 2024
b63596d
fix: missing unit conversion
technolojin Jun 18, 2024
8cbb2f3
style(pre-commit): autofix
pre-commit-ci[bot] Jun 18, 2024
0f62e75
fix: sources
technolojin Jun 18, 2024
b139ffd
chore: Update include and import statements
technolojin Jun 19, 2024
0e84b4d
fix: unit convert bug
technolojin Jun 20, 2024
10fdb99
fix: update to autoware::universe_utils
technolojin Jun 20, 2024
3f20c98
fix: mis-implementation of process noise
technolojin Jun 20, 2024
893fdc5
Merge branch 'main' into feat/mot_tracker_refactoring
technolojin Jun 21, 2024
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
Prev Previous commit
Next Next commit
chore: refactoring getUpdatingObject
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
technolojin committed Jun 20, 2024
commit 0f9089b6085eae274ba821a233e14320b8874a25
Original file line number Diff line number Diff line change
@@ -60,14 +60,16 @@ class BicycleTracker : public Tracker
bool measure(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform) override;
autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform);
bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool getTrackedObject(
const rclcpp::Time & time,
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;

private:
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform) const;
};

#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
Original file line number Diff line number Diff line change
@@ -63,14 +63,16 @@ class BigVehicleTracker : public Tracker
bool measure(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform) override;
autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform);
bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool getTrackedObject(
const rclcpp::Time & time,
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;

private:
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform);
};

#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
Original file line number Diff line number Diff line change
@@ -63,14 +63,16 @@ class NormalVehicleTracker : public Tracker
bool measure(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform) override;
autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform);
bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool getTrackedObject(
const rclcpp::Time & time,
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;

private:
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform);
};

#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
Original file line number Diff line number Diff line change
@@ -68,14 +68,16 @@ class PedestrianTracker : public Tracker
bool measure(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform) override;
autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform);
bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool getTrackedObject(
const rclcpp::Time & time,
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;

private:
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform) const;
};

#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
Original file line number Diff line number Diff line change
@@ -93,8 +93,8 @@ inline int getNearestCornerOrSurface(
// (left) | | (right)
// --
// x- (rear)
int xgrid;
int ygrid;
int xgrid = 0;
int ygrid = 0;
const int labels[3][3] = {
{BBOX_IDX::FRONT_L_CORNER, BBOX_IDX::FRONT_SURFACE, BBOX_IDX::FRONT_R_CORNER},
{BBOX_IDX::LEFT_SURFACE, BBOX_IDX::INSIDE, BBOX_IDX::RIGHT_SURFACE},
Original file line number Diff line number Diff line change
@@ -166,9 +166,9 @@ bool BicycleTracker::predict(const rclcpp::Time & time)
return motion_model_.predictState(time);
}

autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject(
const autoware_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & /*self_transform*/)
autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & /*self_transform*/) const
{
autoware_perception_msgs::msg::DetectedObject updating_object;

Original file line number Diff line number Diff line change
@@ -219,22 +219,15 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje
// UNCERTAINTY MODEL
if (!object.kinematics.has_position_covariance) {
// measurement noise covariance
float r_cov_x;
float r_cov_y;
using Label = autoware_perception_msgs::msg::ObjectClassification;
float r_cov_x = static_cast<float>(ekf_params_.r_cov_x);
float r_cov_y = static_cast<float>(ekf_params_.r_cov_y);
const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification);
if (utils::isLargeVehicleLabel(label)) {
r_cov_x = ekf_params_.r_cov_x;
r_cov_y = ekf_params_.r_cov_y;
} else if (label == Label::CAR) {
if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR) {
// if label is changed, enlarge the measurement noise covariance
constexpr float r_stddev_x = 2.0; // [m]
constexpr float r_stddev_y = 2.0; // [m]
r_cov_x = std::pow(r_stddev_x, 2.0);
r_cov_y = std::pow(r_stddev_y, 2.0);
} else {
r_cov_x = ekf_params_.r_cov_x;
r_cov_y = ekf_params_.r_cov_y;
r_cov_x = r_stddev_x * r_stddev_x;
r_cov_y = r_stddev_y * r_stddev_y;
}

// yaw angle fix
@@ -315,17 +308,18 @@ bool BigVehicleTracker::measureWithPose(
bool BigVehicleTracker::measureWithShape(
const autoware_perception_msgs::msg::DetectedObject & object)
{
if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
// do not update shape if the input is not a bounding box
return false;
}

// check object size abnormality
constexpr double size_max = 40.0; // [m]
constexpr double size_min = 1.0; // [m]
if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) {
return false;
} else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) {
bool isSizeValid =
(object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max &&
object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min);
if (!isSizeValid) {
return false;
}

Original file line number Diff line number Diff line change
@@ -221,22 +221,15 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO
// UNCERTAINTY MODEL
if (!object.kinematics.has_position_covariance) {
// measurement noise covariance
float r_cov_x;
float r_cov_y;
using Label = autoware_perception_msgs::msg::ObjectClassification;
float r_cov_x = static_cast<float>(ekf_params_.r_cov_x);
float r_cov_y = static_cast<float>(ekf_params_.r_cov_y);
const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification);
if (label == Label::CAR) {
r_cov_x = ekf_params_.r_cov_x;
r_cov_y = ekf_params_.r_cov_y;
} else if (utils::isLargeVehicleLabel(label)) {
if (utils::isLargeVehicleLabel(label)) {
// if label is changed, enlarge the measurement noise covariance
constexpr float r_stddev_x = 2.0; // [m]
constexpr float r_stddev_y = 2.0; // [m]
r_cov_x = std::pow(r_stddev_x, 2.0);
r_cov_y = std::pow(r_stddev_y, 2.0);
} else {
r_cov_x = ekf_params_.r_cov_x;
r_cov_y = ekf_params_.r_cov_y;
r_cov_x = r_stddev_x * r_stddev_x;
r_cov_y = r_stddev_y * r_stddev_y;
}

// yaw angle fix
@@ -317,17 +310,18 @@ bool NormalVehicleTracker::measureWithPose(
bool NormalVehicleTracker::measureWithShape(
const autoware_perception_msgs::msg::DetectedObject & object)
{
if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
// do not update shape if the input is not a bounding box
return false;
}

// check object size abnormality
constexpr double size_max = 30.0; // [m]
constexpr double size_min = 1.0; // [m]
if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) {
return false;
} else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) {
bool isSizeValid =
(object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max &&
object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min);
if (!isSizeValid) {
return false;
}

Original file line number Diff line number Diff line change
@@ -160,9 +160,9 @@ bool PedestrianTracker::predict(const rclcpp::Time & time)
return motion_model_.predictState(time);
}

autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject(
const autoware_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & /*self_transform*/)
autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & /*self_transform*/) const
{
autoware_perception_msgs::msg::DetectedObject updating_object = object;