Skip to content

Commit fcebec6

Browse files
technolojintechnolojin
authored
refactor(multi_object_tracker): define motion model class (#6587)
* fix: define function to adjust object yaw angle for ekf update Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: update tracking offset to object position Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: enlarge yaw noise when the orientation is unknown Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: rearrange obj extension, noise model, observation vector length Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: grouping part of codes Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: separate object pre-process getUpdatingObject Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: bicycle tracker object preprocessing Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: separated motion model class, not tested Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: add motion model Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: replace tracker to motion model class Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: normal vehicle tracker replace to motion model Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: big vehicle tracker is replaced by motion model Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: bicycle tracker replace by motion model Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: remove non-used members Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: add CTRV motion model and implement it to pedestrian tracker Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring parameters Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: object initialization Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: grouping param setting Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: create cv motion model, not tested Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: implementing unknown tracker, process is dying Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: align number precisions to double Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: resolve overflow issue by including proper header files Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: missing twist cov Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: limit too large dt on unknown tracker, set size gain to 0.1 Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: set parameter before init Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: fix init to initialize chore: fix init to initialize Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: motion model base class Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: common methods to the base class Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: access to state vector Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: remove not-used tracker's last_update_time_ member Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Revert "fix: remove not-used tracker's last_update_time_ member" This reverts commit bd8aabb. Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: header cleanup Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: relocate motion models Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: timestamp const Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: exception when object couldn't be predicted Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: remove last_update_time_ member Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: set minimum size, to avoid iou error/overflow Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: rollback tracking size and offset algorithm Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: set minimum pedestrian size Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: adjust minimum sizes to 0.3m Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: cleanup unused members Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 69572f1 commit fcebec6

21 files changed

+2647
-1989
lines changed

‎perception/multi_object_tracker/CMakeLists.txt

+6-2
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,12 @@ include_directories(
2222
# Generate exe file
2323
set(MULTI_OBJECT_TRACKER_SRC
2424
src/multi_object_tracker_core.cpp
25+
src/data_association/data_association.cpp
26+
src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
27+
src/tracker/motion_model/motion_model_base.cpp
28+
src/tracker/motion_model/bicycle_motion_model.cpp
29+
src/tracker/motion_model/ctrv_motion_model.cpp
30+
src/tracker/motion_model/cv_motion_model.cpp
2531
src/tracker/model/tracker_base.cpp
2632
src/tracker/model/big_vehicle_tracker.cpp
2733
src/tracker/model/normal_vehicle_tracker.cpp
@@ -31,8 +37,6 @@ set(MULTI_OBJECT_TRACKER_SRC
3137
src/tracker/model/pedestrian_and_bicycle_tracker.cpp
3238
src/tracker/model/unknown_tracker.cpp
3339
src/tracker/model/pass_through_tracker.cpp
34-
src/data_association/data_association.cpp
35-
src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
3640
)
3741

3842
ament_auto_add_library(multi_object_tracker_node SHARED

‎perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp

+13-27
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
2121

2222
#include "multi_object_tracker/tracker/model/tracker_base.hpp"
23+
#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
2324

2425
#include <kalman_filter/kalman_filter.hpp>
2526

@@ -30,55 +31,40 @@ class BicycleTracker : public Tracker
3031
rclcpp::Logger logger_;
3132

3233
private:
33-
KalmanFilter ekf_;
34-
rclcpp::Time last_update_time_;
35-
enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
3634
struct EkfParams
3735
{
38-
char dim_x = 5;
39-
float q_stddev_acc_long;
40-
float q_stddev_acc_lat;
41-
float q_stddev_yaw_rate_min;
42-
float q_stddev_yaw_rate_max;
43-
float q_cov_slip_rate_min;
44-
float q_cov_slip_rate_max;
45-
float q_max_slip_angle;
46-
float p0_cov_vel;
47-
float p0_cov_slip;
48-
float r_cov_x;
49-
float r_cov_y;
50-
float r_cov_yaw;
51-
float p0_cov_x;
52-
float p0_cov_y;
53-
float p0_cov_yaw;
36+
double r_cov_x;
37+
double r_cov_y;
38+
double r_cov_yaw;
5439
} ekf_params_;
5540

56-
double max_vel_;
57-
double max_slip_;
58-
double lf_;
59-
double lr_;
60-
float z_;
41+
double z_;
6142

62-
private:
6343
struct BoundingBox
6444
{
6545
double length;
6646
double width;
6747
double height;
6848
};
6949
BoundingBox bounding_box_;
70-
BoundingBox last_input_bounding_box_;
50+
51+
private:
52+
BicycleMotionModel motion_model_;
53+
const char DIM = motion_model_.DIM;
54+
using IDX = BicycleMotionModel::IDX;
7155

7256
public:
7357
BicycleTracker(
7458
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
7559
const geometry_msgs::msg::Transform & self_transform);
7660

7761
bool predict(const rclcpp::Time & time) override;
78-
bool predict(const double dt, KalmanFilter & ekf) const;
7962
bool measure(
8063
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
8164
const geometry_msgs::msg::Transform & self_transform) override;
65+
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
66+
const autoware_auto_perception_msgs::msg::DetectedObject & object,
67+
const geometry_msgs::msg::Transform & self_transform);
8268
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
8369
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
8470
bool getTrackedObject(

‎perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp

+27-30
Original file line numberDiff line numberDiff line change
@@ -20,47 +20,28 @@
2020
#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
2121

2222
#include "multi_object_tracker/tracker/model/tracker_base.hpp"
23+
#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
2324

2425
#include <kalman_filter/kalman_filter.hpp>
26+
2527
class BigVehicleTracker : public Tracker
2628
{
2729
private:
2830
autoware_auto_perception_msgs::msg::DetectedObject object_;
2931
rclcpp::Logger logger_;
30-
int last_nearest_corner_index_;
3132

3233
private:
33-
KalmanFilter ekf_;
34-
rclcpp::Time last_update_time_;
35-
enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
3634
struct EkfParams
3735
{
38-
char dim_x = 5;
39-
float q_stddev_acc_long;
40-
float q_stddev_acc_lat;
41-
float q_stddev_yaw_rate_min;
42-
float q_stddev_yaw_rate_max;
43-
float q_cov_slip_rate_min;
44-
float q_cov_slip_rate_max;
45-
float q_max_slip_angle;
46-
float p0_cov_vel;
47-
float p0_cov_slip;
48-
float r_cov_x;
49-
float r_cov_y;
50-
float r_cov_yaw;
51-
float r_cov_vel;
52-
float p0_cov_x;
53-
float p0_cov_y;
54-
float p0_cov_yaw;
36+
double r_cov_x;
37+
double r_cov_y;
38+
double r_cov_yaw;
39+
double r_cov_vel;
5540
} ekf_params_;
56-
double max_vel_;
57-
double max_slip_;
58-
double lf_;
59-
double lr_;
60-
float z_;
6141
double velocity_deviation_threshold_;
6242

63-
private:
43+
double z_;
44+
6445
struct BoundingBox
6546
{
6647
double length;
@@ -70,25 +51,41 @@ class BigVehicleTracker : public Tracker
7051
BoundingBox bounding_box_;
7152
BoundingBox last_input_bounding_box_;
7253
Eigen::Vector2d tracking_offset_;
54+
int last_nearest_corner_index_;
55+
56+
private:
57+
BicycleMotionModel motion_model_;
58+
const char DIM = motion_model_.DIM;
59+
using IDX = BicycleMotionModel::IDX;
7360

7461
public:
7562
BigVehicleTracker(
7663
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
7764
const geometry_msgs::msg::Transform & self_transform);
7865

7966
bool predict(const rclcpp::Time & time) override;
80-
bool predict(const double dt, KalmanFilter & ekf) const;
8167
bool measure(
8268
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
8369
const geometry_msgs::msg::Transform & self_transform) override;
70+
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
71+
const autoware_auto_perception_msgs::msg::DetectedObject & object,
72+
const geometry_msgs::msg::Transform & self_transform);
8473
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
8574
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
8675
bool getTrackedObject(
8776
const rclcpp::Time & time,
8877
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
89-
double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object);
90-
void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform);
9178
virtual ~BigVehicleTracker() {}
79+
80+
private:
81+
void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
82+
{
83+
Eigen::MatrixXd X_t(DIM, 1);
84+
motion_model_.getStateVector(X_t);
85+
last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
86+
X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
87+
self_transform);
88+
}
9289
};
9390

9491
#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_

‎perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp

+27-33
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,8 @@
1919
#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
2020
#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
2121

22-
#include "tracker_base.hpp"
22+
#include "multi_object_tracker/tracker/model/tracker_base.hpp"
23+
#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
2324

2425
#include <kalman_filter/kalman_filter.hpp>
2526

@@ -28,42 +29,19 @@ class NormalVehicleTracker : public Tracker
2829
private:
2930
autoware_auto_perception_msgs::msg::DetectedObject object_;
3031
rclcpp::Logger logger_;
31-
int last_nearest_corner_index_;
3232

3333
private:
34-
KalmanFilter ekf_;
35-
rclcpp::Time last_update_time_;
36-
enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
37-
3834
struct EkfParams
3935
{
40-
char dim_x = 5;
41-
float q_stddev_acc_long;
42-
float q_stddev_acc_lat;
43-
float q_stddev_yaw_rate_min;
44-
float q_stddev_yaw_rate_max;
45-
float q_cov_slip_rate_min;
46-
float q_cov_slip_rate_max;
47-
float q_max_slip_angle;
48-
float p0_cov_vel;
49-
float p0_cov_slip;
50-
float r_cov_x;
51-
float r_cov_y;
52-
float r_cov_yaw;
53-
float r_cov_vel;
54-
float p0_cov_x;
55-
float p0_cov_y;
56-
float p0_cov_yaw;
36+
double r_cov_x;
37+
double r_cov_y;
38+
double r_cov_yaw;
39+
double r_cov_vel;
5740
} ekf_params_;
58-
59-
double max_vel_;
60-
double max_slip_;
61-
double lf_;
62-
double lr_;
63-
float z_;
6441
double velocity_deviation_threshold_;
6542

66-
private:
43+
double z_;
44+
6745
struct BoundingBox
6846
{
6947
double length;
@@ -73,25 +51,41 @@ class NormalVehicleTracker : public Tracker
7351
BoundingBox bounding_box_;
7452
BoundingBox last_input_bounding_box_;
7553
Eigen::Vector2d tracking_offset_;
54+
int last_nearest_corner_index_;
55+
56+
private:
57+
BicycleMotionModel motion_model_;
58+
const char DIM = motion_model_.DIM;
59+
using IDX = BicycleMotionModel::IDX;
7660

7761
public:
7862
NormalVehicleTracker(
7963
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
8064
const geometry_msgs::msg::Transform & self_transform);
8165

8266
bool predict(const rclcpp::Time & time) override;
83-
bool predict(const double dt, KalmanFilter & ekf) const;
8467
bool measure(
8568
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
8669
const geometry_msgs::msg::Transform & self_transform) override;
70+
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
71+
const autoware_auto_perception_msgs::msg::DetectedObject & object,
72+
const geometry_msgs::msg::Transform & self_transform);
8773
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
8874
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
8975
bool getTrackedObject(
9076
const rclcpp::Time & time,
9177
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
92-
void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform);
93-
double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object);
9478
virtual ~NormalVehicleTracker() {}
79+
80+
private:
81+
void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
82+
{
83+
Eigen::MatrixXd X_t(DIM, 1);
84+
motion_model_.getStateVector(X_t);
85+
last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
86+
X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
87+
self_transform);
88+
}
9589
};
9690

9791
#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_

‎perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp

+13-28
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
2121

2222
#include "multi_object_tracker/tracker/model/tracker_base.hpp"
23+
#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp"
2324

2425
#include <kalman_filter/kalman_filter.hpp>
2526

@@ -30,38 +31,15 @@ class PedestrianTracker : public Tracker
3031
rclcpp::Logger logger_;
3132

3233
private:
33-
KalmanFilter ekf_;
34-
rclcpp::Time last_update_time_;
35-
enum IDX {
36-
X = 0,
37-
Y = 1,
38-
YAW = 2,
39-
VX = 3,
40-
WZ = 4,
41-
};
4234
struct EkfParams
4335
{
44-
char dim_x = 5;
45-
float q_cov_x;
46-
float q_cov_y;
47-
float q_cov_yaw;
48-
float q_cov_wz;
49-
float q_cov_vx;
50-
float p0_cov_vx;
51-
float p0_cov_wz;
52-
float r_cov_x;
53-
float r_cov_y;
54-
float r_cov_yaw;
55-
float p0_cov_x;
56-
float p0_cov_y;
57-
float p0_cov_yaw;
36+
double r_cov_x;
37+
double r_cov_y;
38+
double r_cov_yaw;
5839
} ekf_params_;
5940

60-
double max_vx_;
61-
double max_wz_;
62-
float z_;
41+
double z_;
6342

64-
private:
6543
struct BoundingBox
6644
{
6745
double length;
@@ -76,16 +54,23 @@ class PedestrianTracker : public Tracker
7654
BoundingBox bounding_box_;
7755
Cylinder cylinder_;
7856

57+
private:
58+
CTRVMotionModel motion_model_;
59+
const char DIM = motion_model_.DIM;
60+
using IDX = CTRVMotionModel::IDX;
61+
7962
public:
8063
PedestrianTracker(
8164
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
8265
const geometry_msgs::msg::Transform & self_transform);
8366

8467
bool predict(const rclcpp::Time & time) override;
85-
bool predict(const double dt, KalmanFilter & ekf) const;
8668
bool measure(
8769
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
8870
const geometry_msgs::msg::Transform & self_transform) override;
71+
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
72+
const autoware_auto_perception_msgs::msg::DetectedObject & object,
73+
const geometry_msgs::msg::Transform & self_transform);
8974
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
9075
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
9176
bool getTrackedObject(

0 commit comments

Comments
 (0)