Skip to content

Commit

Permalink
feat: impl. cont
Browse files Browse the repository at this point in the history
  • Loading branch information
@technolojin
technolojin committed Jun 13, 2024
1 parent 7e617cc commit 7537210
Show file tree
Hide file tree
Showing 6 changed files with 40 additions and 50 deletions.
14 changes: 7 additions & 7 deletions ...n/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ struct MotionProcessNoise
double yaw_rate_max{0.0}; // [rad/s] uncertain yaw rate, maximum
double acc_long{0.0}; // [m/s^2] uncertain longitudinal acceleration
double acc_lat{0.0}; // [m/s^2] uncertain lateral acceleration
double rotate_rate{0.0}; // [rad/s^2] uncertain rotation change rate
double acc_turn{0.0}; // [rad/s^2] uncertain rotation acceleration
};
struct MotionProcessLimit
{
Expand Down Expand Up @@ -268,17 +268,17 @@ class ObjectModel
process_noise.vel_lat = 0.5;
process_noise.yaw_rate = deg2rad(20.0);
process_noise.acc_long = const_g * 0.3;
process_noise.rotate_rate = deg2rad(30.0);
process_noise.acc_turn = deg2rad(30.0);

process_limit.vel_long_max = kmph2mps(100.0);
process_limit.yaw_rate_max = deg2rad(30.0);

// initial covariance
initial_covariance.pos_x = sq(1.0);
initial_covariance.pos_y = sq(0.3);
initial_covariance.yaw = sq(deg2rad(25.0));
initial_covariance.vel_long = sq(kmph2mps(1000.0));
initial_covariance.vel_lat = sq(0.2);
initial_covariance.pos_x = sq(2.0);
initial_covariance.pos_y = sq(2.0);
initial_covariance.yaw = sq(deg2rad(1000.0));
initial_covariance.vel_long = sq(kmph2mps(120.0));
initial_covariance.yaw_rate = sq(deg2rad(360.0));

// measurement noise model
measurement_covariance.pos_x = sq(0.4);
Expand Down
4 changes: 2 additions & 2 deletions perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,10 +176,10 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject(
auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
const double cos_yaw = std::cos(pose_yaw);
const double sin_yaw = std::sin(pose_yaw);
const double sin_2yaw = std::sin(2.0f * pose_yaw);
const double sin_2yaw = std::sin(2.0 * pose_yaw);
pose_cov[XYZRPY_COV_IDX::X_X] =
r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x
pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y
pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y
pose_cov[XYZRPY_COV_IDX::Y_Y] =
r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y
pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x
Expand Down
4 changes: 2 additions & 2 deletions perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -216,10 +216,10 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje
auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
const double cos_yaw = std::cos(pose_yaw);
const double sin_yaw = std::sin(pose_yaw);
const double sin_2yaw = std::sin(2.0f * pose_yaw);
const double sin_2yaw = std::sin(2.0 * pose_yaw);
pose_cov[XYZRPY_COV_IDX::X_X] =
r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x
pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y
pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y
pose_cov[XYZRPY_COV_IDX::Y_Y] =
r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y
pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x
Expand Down
4 changes: 2 additions & 2 deletions perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,10 +217,10 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO
auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
const double cos_yaw = std::cos(pose_yaw);
const double sin_yaw = std::sin(pose_yaw);
const double sin_2yaw = std::sin(2.0f * pose_yaw);
const double sin_2yaw = std::sin(2.0 * pose_yaw);
pose_cov[XYZRPY_COV_IDX::X_X] =
r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x
pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y
pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y
pose_cov[XYZRPY_COV_IDX::Y_Y] =
r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y
pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x
Expand Down
54 changes: 22 additions & 32 deletions perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,42 +86,30 @@ PedestrianTracker::PedestrianTracker(
const double q_stddev_y = object_model_.process_noise.vel_lat;
const double q_stddev_yaw = object_model_.process_noise.yaw_rate;
const double q_stddev_vx = object_model_.process_noise.acc_long;
const double q_stddev_wz = object_model_.process_noise.rotate_rate;
const double q_stddev_wz = object_model_.process_noise.acc_turn;
motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz);
}

// Set motion limits
motion_model_.setMotionLimits(
tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */
30.0 /* [deg/s] maximum turn rate */
);
{
const double max_vel = object_model_.process_limit.vel_long_max;
const double max_turn_rate = object_model_.process_limit.yaw_rate_max;
motion_model_.setMotionLimits(max_vel, max_turn_rate); // maximum velocity and slip angle
}

// Set initial state
{
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
const double x = object.kinematics.pose_with_covariance.pose.position.x;
const double y = object.kinematics.pose_with_covariance.pose.position.y;
const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
auto pose_cov = object.kinematics.pose_with_covariance.covariance;
double vel = 0.0;
double wz = 0.0;
double vel_cov = 10.0;
double wz_cov = 10.0;

if (object.kinematics.has_twist) {
vel = object.kinematics.twist_with_covariance.twist.linear.x;
wz = object.kinematics.twist_with_covariance.twist.angular.z;
}

auto pose_cov = object.kinematics.pose_with_covariance.covariance;
if (!object.kinematics.has_position_covariance) {
// initial state covariance
constexpr double p0_stddev_x = 2.0; // in object coordinate [m]
constexpr double p0_stddev_y = 2.0; // in object coordinate [m]
constexpr double p0_stddev_yaw =
tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad]
constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x;
constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y;
constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw;
const auto & p0_cov_x = object_model_.initial_covariance.pos_x;
const auto & p0_cov_y = object_model_.initial_covariance.pos_y;
const auto & p0_cov_yaw = object_model_.initial_covariance.yaw;

const double cos_yaw = std::cos(yaw);
const double sin_yaw = std::sin(yaw);
Expand All @@ -133,14 +121,16 @@ PedestrianTracker::PedestrianTracker(
pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw;
}

if (!object.kinematics.has_twist_covariance) {
constexpr double p0_stddev_vel =
tier4_autoware_utils::kmph2mps(120); // in object coordinate [m/s]
constexpr double p0_stddev_wz =
tier4_autoware_utils::deg2rad(360); // in object coordinate [rad/s]
vel_cov = std::pow(p0_stddev_vel, 2.0);
wz_cov = std::pow(p0_stddev_wz, 2.0);
} else {
double vel = 0.0;
double wz = 0.0;
if (object.kinematics.has_twist) {
vel = object.kinematics.twist_with_covariance.twist.linear.x;
wz = object.kinematics.twist_with_covariance.twist.angular.z;
}

double vel_cov = object_model_.initial_covariance.vel_long;
double wz_cov = object_model_.initial_covariance.yaw_rate;
if (object.kinematics.has_twist_covariance) {
vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X];
wz_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW];

Check notice on line 135 in perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Large Method 

PedestrianTracker::PedestrianTracker decreases from 90 to 78 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
}
Expand Down Expand Up @@ -178,10 +168,10 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje
auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
const double cos_yaw = std::cos(pose_yaw);
const double sin_yaw = std::sin(pose_yaw);
const double sin_2yaw = std::sin(2.0f * pose_yaw);
const double sin_2yaw = std::sin(2.0 * pose_yaw);
pose_cov[XYZRPY_COV_IDX::X_X] =
r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x
pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y
pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y
pose_cov[XYZRPY_COV_IDX::Y_Y] =
r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y
pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x
Expand Down
10 changes: 5 additions & 5 deletions perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -296,11 +296,11 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons
A(IDX::YAW, IDX::WZ) = dt;

// Process noise covariance Q
const double q_cov_x = std::pow(0.5 * motion_params_.q_cov_x * dt, 2);
const double q_cov_y = std::pow(0.5 * motion_params_.q_cov_y * dt, 2);
const double q_cov_yaw = std::pow(motion_params_.q_cov_yaw * dt, 2);
const double q_cov_vel = std::pow(motion_params_.q_cov_vel * dt, 2);
const double q_cov_wz = std::pow(motion_params_.q_cov_wz * dt, 2);
const double q_cov_x = motion_params_.q_cov_x * dt * dt;
const double q_cov_y = motion_params_.q_cov_y * dt * dt;
const double q_cov_yaw = motion_params_.q_cov_yaw * dt * dt;
const double q_cov_vel = motion_params_.q_cov_vel * dt * dt;
const double q_cov_wz = motion_params_.q_cov_wz * dt * dt;
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM);
// Rotate the covariance matrix according to the vehicle yaw
// because q_cov_x and y are in the vehicle coordinate system.
Expand Down

0 comments on commit 7537210

Please sign in to comment.