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 velocity observation #696

Merged
merged 5 commits into from
Apr 15, 2022
Merged

feat(multi_object_tracker): add velocity observation #696

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
e3027bc
add velocity observation to the normal vehicle tracker
purewater0901 Apr 14, 2022
e3a2c4d
fix format
purewater0901 Apr 14, 2022
17b8a93
update
purewater0901 Apr 14, 2022
46d74ad
add big vehicle tracker
purewater0901 Apr 14, 2022
ab7d56c
fix format
purewater0901 Apr 14, 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
1 change: 1 addition & 0 deletions ...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ class BigVehicleTracker : public Tracker
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
float r_cov_vx;
float p0_cov_x;
float p0_cov_y;
float p0_cov_yaw;
Expand Down
1 change: 1 addition & 0 deletions ...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ class NormalVehicleTracker : public Tracker
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
float r_cov_vx;
float p0_cov_x;
float p0_cov_y;
float p0_cov_yaw;
Expand Down
29 changes: 23 additions & 6 deletions perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ BigVehicleTracker::BigVehicleTracker(
float r_stddev_x = 1.5; // [m]
float r_stddev_y = 0.5; // [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad]
float r_stddev_vx = 1.0; // [m/s]
float p0_stddev_x = 1.5; // [m]
float p0_stddev_y = 0.5; // [m]
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad]
Expand All @@ -62,6 +63,7 @@ BigVehicleTracker::BigVehicleTracker(
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0);
ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
Expand Down Expand Up @@ -230,7 +232,8 @@ bool BigVehicleTracker::measureWithPose(
r_cov_y = ekf_params_.r_cov_y;
}

constexpr int dim_y = 3; // pos x, pos y, yaw, depending on Pose output
const int dim_y =
object.kinematics.has_twist ? 4 : 3; // pos x, pos y, yaw, vx depending on Pose output
double measurement_yaw = tier4_autoware_utils::normalizeRadian(
tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation));
{
Expand All @@ -247,17 +250,17 @@ bool BigVehicleTracker::measureWithPose(

/* Set measurement matrix */
Eigen::MatrixXd Y(dim_y, 1);
Y << object.kinematics.pose_with_covariance.pose.position.x,
object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw;

/* Set measurement matrix */
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);

Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x;
Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y;
Y(IDX::YAW, 0) = measurement_yaw;
C(0, IDX::X) = 1.0; // for pos x
C(1, IDX::Y) = 1.0; // for pos y
C(2, IDX::YAW) = 1.0; // for yaw

/* Set measurement noise covariance */
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
if (
!ekf_params_.use_measurement_covariance ||
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 ||
Expand All @@ -282,6 +285,20 @@ bool BigVehicleTracker::measureWithPose(
R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y];
R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
}

if (object.kinematics.has_twist) {
Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x;
C(3, IDX::VX) = 1.0; // for vx

if (
!ekf_params_.use_measurement_covariance ||
object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0) {
R(3, 3) = ekf_params_.r_cov_vx; // vx -vx
} else {
R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
}
}

if (!ekf_.update(Y, C, R)) {
RCLCPP_WARN(logger_, "Cannot update");
}
Expand Down
32 changes: 25 additions & 7 deletions perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ NormalVehicleTracker::NormalVehicleTracker(
float r_stddev_x = 1.0; // object coordinate [m]
float r_stddev_y = 0.3; // object coordinate [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad]
float r_stddev_vx = 1.0; // object coordinate [m/s]
float p0_stddev_x = 1.0; // object coordinate [m/s]
float p0_stddev_y = 0.3; // object coordinate [m/s]
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad]
Expand All @@ -62,6 +63,7 @@ NormalVehicleTracker::NormalVehicleTracker(
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0);
ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
Expand Down Expand Up @@ -230,7 +232,8 @@ bool NormalVehicleTracker::measureWithPose(
r_cov_y = ekf_params_.r_cov_y;
}

constexpr int dim_y = 3; // pos x, pos y, yaw, depending on Pose output
const int dim_y =
object.kinematics.has_twist ? 4 : 3; // pos x, pos y, yaw, vx depending on Pose output
double measurement_yaw = tier4_autoware_utils::normalizeRadian(
tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation));
{
Expand All @@ -245,19 +248,19 @@ bool NormalVehicleTracker::measureWithPose(
}
}

/* Set measurement matrix */
/* Set measurement matrix and noise covariance*/
Eigen::MatrixXd Y(dim_y, 1);
Y << object.kinematics.pose_with_covariance.pose.position.x,
object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw;

/* Set measurement matrix */
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);

Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x;
Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y;
Y(IDX::YAW, 0) = measurement_yaw;
C(0, IDX::X) = 1.0; // for pos x
C(1, IDX::Y) = 1.0; // for pos y
C(2, IDX::YAW) = 1.0; // for yaw

/* Set measurement noise covariance */
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
if (
!ekf_params_.use_measurement_covariance ||
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 ||
Expand All @@ -282,6 +285,21 @@ bool NormalVehicleTracker::measureWithPose(
R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y];
R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
}

if (object.kinematics.has_twist) {
Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x;
C(3, IDX::VX) = 1.0; // for vx

if (
!ekf_params_.use_measurement_covariance ||
object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0) {
R(3, 3) = ekf_params_.r_cov_vx; // vx -vx
} else {
R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
}
}

// update
if (!ekf_.update(Y, C, R)) {
RCLCPP_WARN(logger_, "Cannot update");
}
Expand Down