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

fix(radar_detection): radar detection bugs #904

Merged
merged 4 commits into from
Oct 3, 2023
+78 −71
Merged

fix(radar_detection): radar detection bugs #904

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
08009ed
fix(radar_fusion_to_detected_object): fix confidence (#4992)
scepter914 Sep 19, 2023
8c6ec88
fix(radar_tracks_msg_converter): fix twist coordinate conversion (#4993)
scepter914 Sep 19, 2023
131f26a
chore: fix spell-check errors in apollo-related packages (#4987)
kminoda Sep 19, 2023
2a9f160
chore(radar_object_tracker): fix spell-check error (#5024)
kminoda Sep 20, 2023
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
4 changes: 2 additions & 2 deletions ...dar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,12 +79,12 @@ struct Obstacle
float height;
float heading;
MetaType meta_type;
std::vector<float> meta_type_probs;
std::vector<float> meta_type_probabilities;

Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(META_UNKNOWN)
{
cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>);
meta_type_probs.assign(MAX_META_TYPE, 0.0);
meta_type_probabilities.assign(MAX_META_TYPE, 0.0);
}
};

Expand Down
6 changes: 3 additions & 3 deletions perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -235,13 +235,13 @@ void Cluster2D::classify(const float * inferred_data)
for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) {
int grid = obs->grids[grid_id];
for (int k = 0; k < num_classes; k++) {
obs->meta_type_probs[k] += classify_pt_data[k * size_ + grid];
obs->meta_type_probabilities[k] += classify_pt_data[k * size_ + grid];
}
}
int meta_type_id = 0;
for (int k = 0; k < num_classes; k++) {
obs->meta_type_probs[k] /= obs->grids.size();
if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) {
obs->meta_type_probabilities[k] /= obs->grids.size();
if (obs->meta_type_probabilities[k] > obs->meta_type_probabilities[meta_type_id]) {
meta_type_id = k;
}
}
Expand Down
4 changes: 2 additions & 2 deletions perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,12 +57,12 @@ struct Obstacle
float height;
float heading;
MetaType meta_type;
std::vector<float> meta_type_probs;
std::vector<float> meta_type_probabilities;

Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(MetaType::META_UNKNOWN)
{
cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>);
meta_type_probs.assign(static_cast<int>(MetaType::MAX_META_TYPE), 0.0);
meta_type_probabilities.assign(static_cast<int>(MetaType::MAX_META_TYPE), 0.0);
}
};

Expand Down
6 changes: 3 additions & 3 deletions perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -218,13 +218,13 @@ void Cluster2D::classify(const float * inferred_data)
for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) {
int32_t grid = obs->grids[grid_id];
for (int k = 0; k < num_classes; k++) {
obs->meta_type_probs[k] += classify_pt_data[k * siz_ + grid];
obs->meta_type_probabilities[k] += classify_pt_data[k * siz_ + grid];
}
}
int meta_type_id = 0;
for (int k = 0; k < num_classes; k++) {
obs->meta_type_probs[k] /= static_cast<float>(obs->grids.size());
if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) {
obs->meta_type_probabilities[k] /= static_cast<float>(obs->grids.size());
if (obs->meta_type_probabilities[k] > obs->meta_type_probabilities[meta_type_id]) {
meta_type_id = k;
}
}
Expand Down
2 changes: 1 addition & 1 deletion perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -293,7 +293,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist(
bool RadarFusionToDetectedObject::isQualified(
const DetectedObject & object, std::shared_ptr<std::vector<RadarInput>> & radars)
{
if (object.classification[0].probability > param_.threshold_probability) {
if (object.existence_probability > param_.threshold_probability) {
return true;
} else {
if (!radars || !(*radars).empty()) {
Expand Down
112 changes: 56 additions & 56 deletions perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,15 +91,15 @@ LinearMotionTracker::LinearMotionTracker(
R << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;

// covariance matrix in the target vehicle coordinate system
Eigen::Matrix2d P_xy_local, P_vxy_local, P_axy_local;
Eigen::Matrix2d P_xy_local, P_v_xy_local, P_a_xy_local;
P_xy_local << ekf_params_.p0_cov_x, 0.0, 0.0, ekf_params_.p0_cov_y;
P_vxy_local << ekf_params_.p0_cov_vx, 0.0, 0.0, ekf_params_.p0_cov_vy;
P_axy_local << ekf_params_.p0_cov_ax, 0.0, 0.0, ekf_params_.p0_cov_ay;
P_v_xy_local << ekf_params_.p0_cov_vx, 0.0, 0.0, ekf_params_.p0_cov_vy;
P_a_xy_local << ekf_params_.p0_cov_ax, 0.0, 0.0, ekf_params_.p0_cov_ay;

// Rotated covariance matrix
// covariance is rotated by 2D rotation matrix R
// P′=R P RT
Eigen::Matrix2d P_xy, P_vxy, P_axy;
Eigen::Matrix2d P_xy, P_v_xy, P_a_xy;

// Rotate the covariance matrix according to the vehicle yaw
// because p0_cov_x and y are in the vehicle coordinate system.
Expand All @@ -117,12 +117,12 @@ LinearMotionTracker::LinearMotionTracker(
if (object.kinematics.has_twist_covariance) {
const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
const auto vy_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
P_vxy_local << vx_cov, 0.0, 0.0, vy_cov;
P_vxy = R * P_vxy_local * R.transpose();
P_v_xy_local << vx_cov, 0.0, 0.0, vy_cov;
P_v_xy = R * P_v_xy_local * R.transpose();
} else {
P_vxy = R * P_vxy_local * R.transpose();
P_v_xy = R * P_v_xy_local * R.transpose();
}
// acceleration covariance often writen in object frame
// acceleration covariance often written in object frame
const bool has_acceleration_covariance =
false; // currently message does not have acceleration covariance
if (has_acceleration_covariance) {
Expand All @@ -132,18 +132,18 @@ LinearMotionTracker::LinearMotionTracker(
// const auto ay_cov =
// object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This
// is future update
// Eigen::Matrix2d P_axy_local;
// P_axy_local << ax_cov, 0.0, 0.0, ay_cov;
P_axy = R * P_axy_local * R.transpose();
// Eigen::Matrix2d P_a_xy_local;
// P_a_xy_local << ax_cov, 0.0, 0.0, ay_cov;
P_a_xy = R * P_a_xy_local * R.transpose();
} else {
P_axy = R * P_axy_local * R.transpose();
P_a_xy = R * P_a_xy_local * R.transpose();
}

// put value in P matrix
// use block description. This assume x,y,vx,vy,ax,ay in this order
P.block<2, 2>(IDX::X, IDX::X) = P_xy;
P.block<2, 2>(IDX::VX, IDX::VX) = P_vxy;
P.block<2, 2>(IDX::AX, IDX::AX) = P_axy;
P.block<2, 2>(IDX::VX, IDX::VX) = P_v_xy;
P.block<2, 2>(IDX::AX, IDX::AX) = P_a_xy;

// init shape
if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
Expand Down Expand Up @@ -292,8 +292,8 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const
// system noise in local coordinate
// we assume acceleration random walk model
//
// Q_local = [dt^3/6 0 dt^2/2 0 dt 0] ^ T qcov_ax [dt^3/6 0 dt^2/2 0 dt 0]
// + [0 dt^3/6 0 dt^2/2 0 dt] ^ T qcov_ay [0 dt^3/6 0 dt^2/2 0 dt]
// Q_local = [dt^3/6 0 dt^2/2 0 dt 0] ^ T q_cov_ax [dt^3/6 0 dt^2/2 0 dt 0]
// + [0 dt^3/6 0 dt^2/2 0 dt] ^ T q_cov_ay [0 dt^3/6 0 dt^2/2 0 dt]
// Eigen::MatrixXd qx = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);
// Eigen::MatrixXd qy = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);
// qx << dt * dt * dt / 6, 0, dt * dt / 2, 0, dt, 0;
Expand Down Expand Up @@ -386,10 +386,10 @@ bool LinearMotionTracker::measureWithPose(
// 2. add linear velocity measurement
const bool enable_velocity_measurement = object.kinematics.has_twist;
if (enable_velocity_measurement) {
Eigen::MatrixXd Cvxvy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x);
Cvxvy(0, IDX::VX) = 1;
Cvxvy(1, IDX::VY) = 1;
C_list.push_back(Cvxvy);
Eigen::MatrixXd C_vx_vy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x);
C_vx_vy(0, IDX::VX) = 1;
C_vx_vy(1, IDX::VY) = 1;
C_list.push_back(C_vx_vy);

// velocity is in the target vehicle coordinate system
Eigen::MatrixXd Vxy_local = Eigen::MatrixXd::Zero(2, 1);
Expand All @@ -399,19 +399,19 @@ bool LinearMotionTracker::measureWithPose(
Vxy = RotationYaw * Vxy_local;
Y_list.push_back(Vxy);

Eigen::Matrix2d Rvxy_local = Eigen::MatrixXd::Zero(2, 2);
Eigen::Matrix2d R_v_xy_local = Eigen::MatrixXd::Zero(2, 2);
if (!object.kinematics.has_twist_covariance) {
Rvxy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy;
R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy;
} else {
Rvxy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], 0,
0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
R_v_xy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
0, 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
}
Eigen::MatrixXd Rvxy = Eigen::MatrixXd::Zero(2, 2);
Rvxy = RotationYaw * Rvxy_local * RotationYaw.transpose();
R_block_list.push_back(Rvxy);
Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2);
R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose();
R_block_list.push_back(R_v_xy);
}

// 3. sumup matrices
// 3. sum up matrices
const auto row_number = std::accumulate(
C_list.begin(), C_list.end(), 0,
[](const auto & sum, const auto & C) { return sum + C.rows(); });
Expand Down Expand Up @@ -522,9 +522,9 @@ bool LinearMotionTracker::getTrackedObject(
auto & twist_with_cov = object.kinematics.twist_with_covariance;
auto & acceleration_with_cov = object.kinematics.acceleration_with_covariance;
// rotation matrix with yaw_
Eigen::Matrix2d Ryaw = Eigen::Matrix2d::Zero();
Ryaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_);
const Eigen::Matrix2d Ryaw_inv = Ryaw.transpose();
Eigen::Matrix2d R_yaw = Eigen::Matrix2d::Zero();
R_yaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_);
const Eigen::Matrix2d R_yaw_inv = R_yaw.transpose();

// position
pose_with_cov.pose.position.x = X_t(IDX::X);
Expand All @@ -550,41 +550,41 @@ bool LinearMotionTracker::getTrackedObject(
const auto vx = X_t(IDX::VX);
const auto vy = X_t(IDX::VY);
// rotate this vector with -yaw
Eigen::Vector2d vxy = Ryaw_inv * Eigen::Vector2d(vx, vy);
twist_with_cov.twist.linear.x = vxy(0);
twist_with_cov.twist.linear.y = vxy(1);
Eigen::Vector2d v_local = R_yaw_inv * Eigen::Vector2d(vx, vy);
twist_with_cov.twist.linear.x = v_local(0);
twist_with_cov.twist.linear.y = v_local(1);

// acceleration
// acceleration need to converted to local coordinate
const auto ax = X_t(IDX::AX);
const auto ay = X_t(IDX::AY);
// rotate this vector with -yaw
Eigen::Vector2d axy = Ryaw_inv * Eigen::Vector2d(ax, ay);
acceleration_with_cov.accel.linear.x = axy(0);
acceleration_with_cov.accel.linear.y = axy(1);
Eigen::Vector2d a_local = R_yaw_inv * Eigen::Vector2d(ax, ay);
acceleration_with_cov.accel.linear.x = a_local(0);
acceleration_with_cov.accel.linear.y = a_local(1);

// ===== covariance transformation =====
// since covariance in EKF is in map coordinate and output should be in object coordinate,
// we need to transform covariance
Eigen::Matrix2d Pxy_map, Pvxy_map, Paxy_map;
Pxy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y);
Pvxy_map << P(IDX::VX, IDX::VX), P(IDX::VX, IDX::VY), P(IDX::VY, IDX::VX), P(IDX::VY, IDX::VY);
Paxy_map << P(IDX::AX, IDX::AX), P(IDX::AX, IDX::AY), P(IDX::AY, IDX::AX), P(IDX::AY, IDX::AY);
Eigen::Matrix2d P_xy_map, P_v_xy_map, P_a_xy_map;
P_xy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y);
P_v_xy_map << P(IDX::VX, IDX::VX), P(IDX::VX, IDX::VY), P(IDX::VY, IDX::VX), P(IDX::VY, IDX::VY);
P_a_xy_map << P(IDX::AX, IDX::AX), P(IDX::AX, IDX::AY), P(IDX::AY, IDX::AX), P(IDX::AY, IDX::AY);

// rotate covariance with -yaw
Eigen::Matrix2d Pxy = Ryaw_inv * Pxy_map * Ryaw_inv.transpose();
Eigen::Matrix2d Pvxy = Ryaw_inv * Pvxy_map * Ryaw_inv.transpose();
Eigen::Matrix2d Paxy = Ryaw_inv * Paxy_map * Ryaw_inv.transpose();
Eigen::Matrix2d P_xy = R_yaw_inv * P_xy_map * R_yaw_inv.transpose();
Eigen::Matrix2d P_v_xy = R_yaw_inv * P_v_xy_map * R_yaw_inv.transpose();
Eigen::Matrix2d P_a_xy = R_yaw_inv * P_a_xy_map * R_yaw_inv.transpose();

// position covariance
constexpr double no_info_cov = 1e9; // no information
constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative
constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative

pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Pxy(IDX::X, IDX::X);
pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Pxy(IDX::X, IDX::Y);
pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Pxy(IDX::Y, IDX::X);
pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Pxy(IDX::Y, IDX::Y);
pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X);
pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y);
pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X);
pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y);
pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov;
pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov;
pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov;
Expand All @@ -594,20 +594,20 @@ bool LinearMotionTracker::getTrackedObject(
constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative
constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative

twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Pvxy(IDX::X, IDX::X);
twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Pvxy(IDX::X, IDX::Y);
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Pvxy(IDX::Y, IDX::X);
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Pvxy(IDX::Y, IDX::Y);
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X);
twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y);
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X);
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y);
twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov;

// acceleration covariance
acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Paxy(IDX::X, IDX::X);
acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Paxy(IDX::X, IDX::Y);
acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Paxy(IDX::Y, IDX::X);
acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Paxy(IDX::Y, IDX::Y);
acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X);
acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y);
acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X);
acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y);
acceleration_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = no_info_cov;
acceleration_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov;
acceleration_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov;
Expand Down
15 changes: 11 additions & 4 deletions ..._msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -212,8 +212,15 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()
kinematics.orientation_availability = TrackedObjectKinematics::AVAILABLE;
kinematics.is_stationary = false;

// Twist conversion
geometry_msgs::msg::Vector3 compensated_velocity = radar_track.velocity;
geometry_msgs::msg::Vector3 compensated_velocity{};
{
double rotate_yaw = tf2::getYaw(transform_->transform.rotation);
const geometry_msgs::msg::Vector3 & vel = radar_track.velocity;
compensated_velocity.x = vel.x * std::cos(rotate_yaw) - vel.y * std::sin(rotate_yaw);
compensated_velocity.y = vel.x * std::sin(rotate_yaw) + vel.y * std::cos(rotate_yaw);
compensated_velocity.z = radar_track.velocity.z;
}

if (node_param_.use_twist_compensation) {
if (odometry_data_) {
compensated_velocity.x += odometry_data_->twist.twist.linear.x;
Expand All @@ -233,12 +240,12 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()

double yaw = tier4_autoware_utils::normalizeRadian(
std::atan2(compensated_velocity.y, compensated_velocity.x));
radar_pose_stamped.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw);

geometry_msgs::msg::PoseStamped transformed_pose_stamped{};
tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_);
kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose;

kinematics.pose_with_covariance.pose.orientation =
tier4_autoware_utils::createQuaternionFromYaw(yaw);
{
auto & pose_cov = kinematics.pose_with_covariance.covariance;
auto & radar_position_cov = radar_track.position_covariance;
Expand Down