Skip to content

Commit

Permalink
Merge pull request #904 from tier4/fix/radar_detection_bugs
Browse files Browse the repository at this point in the history 
fix(radar_detection): radar detection bugs
  • Loading branch information
@tkimura4
tkimura4 authored Oct 3, 2023
2 parents 3ab1243 + 2a9f160 commit f73de99
Show file tree
Hide file tree
Showing 7 changed files with 78 additions and 71 deletions.
4 changes: 2 additions & 2 deletions ...dar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp
View file
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
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
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
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
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
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
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

0 comments on commit f73de99

Please sign in to comment.