From 0886e01853aaa19905aab596723be5eeee304f24 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 23 Oct 2024 17:57:43 +0900 Subject: [PATCH] fix: transform to world first, add odometry covariance later --- .../lib/uncertainty/uncertainty_processor.cpp | 8 ++-- .../src/multi_object_tracker_node.cpp | 37 ++++++++++++------- 2 files changed, 27 insertions(+), 18 deletions(-) diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index 9d0a1a630f34a..6cab76d0b25ce 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -158,7 +158,7 @@ void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detecte // ego position uncertainty, position covariance + motion covariance Eigen::MatrixXd m_cov_ego_pose = Eigen::MatrixXd(2, 2); m_cov_ego_pose << odom_pose_cov[0], odom_pose_cov[1], odom_pose_cov[6], odom_pose_cov[7]; - m_cov_ego_pose = m_cov_ego_pose + m_rot_ego.transpose() * m_cov_motion * m_rot_ego; + m_cov_ego_pose = m_cov_ego_pose + m_rot_ego * m_cov_motion * m_rot_ego.transpose(); // ego yaw uncertainty, position covariance + yaw motion covariance const double & cov_ego_yaw = odom_pose_cov[35]; @@ -178,7 +178,7 @@ void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detecte object_pose_cov[XYZRPY_COV_IDX::Y_X], object_pose_cov[XYZRPY_COV_IDX::Y_Y]; const double dx = object_pose.position.x - odom_pose.position.x; const double dy = object_pose.position.y - odom_pose.position.y; - const double r = std::sqrt(dx * dx + dy * dy); + const double r2 = dx * dx + dy * dy; const double theta = std::atan2(dy, dx); // 1-a. add odometry position uncertainty to the object position covariance @@ -190,7 +190,7 @@ void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detecte // uncertainty is proportional to the distance between the object and the odometry // and the uncertainty orientation is vertical to the vector of the odometry position to the // object - const double cov_by_yaw = cov_ego_yaw * r * r; + const double cov_by_yaw = cov_ego_yaw * r2; // rotate the covariance matrix, add the yaw uncertainty, and rotate back Eigen::MatrixXd m_rot_theta = Eigen::Rotation2D(theta).toRotationMatrix(); Eigen::MatrixXd m_cov_rot = m_rot_theta.transpose() * m_pose_cov * m_rot_theta; @@ -219,7 +219,7 @@ void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detecte // 2-b. add odometry yaw rate uncertainty to the object linear twist covariance { - const double cov_by_yaw_rate = cov_yaw_rate * r * r; + const double cov_by_yaw_rate = cov_yaw_rate * r2; Eigen::MatrixXd m_rot_theta = Eigen::Rotation2D(theta).toRotationMatrix(); Eigen::MatrixXd m_twist_cov_rot = m_rot_theta.transpose() * m_twist_cov * m_rot_theta; m_twist_cov_rot(1, 1) += cov_by_yaw_rate; // yaw rate uncertainty is added to y-y element diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index dba00b6f589ee..4b46a2e2e244b 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -275,39 +275,48 @@ void MultiObjectTracker::runProcess( return; } + // Transform the objects to the world frame + DetectedObjects transformed_objects; + if (!autoware::object_recognition_utils::transformObjects( + input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { + return; + } + // the object uncertainty - DetectedObjects input_objects_with_uncertainty = input_objects; if (enable_odometry_uncertainty_) { // Create a modeled odometry message nav_msgs::msg::Odometry odometry; odometry.header.stamp = measurement_time + rclcpp::Duration::from_seconds(0.001); - auto & odom_pose_cov = odometry.pose.covariance; - odom_pose_cov[0] = 0.1; // x-x - odom_pose_cov[7] = 0.1; // y-y - odom_pose_cov[35] = 0.001; // yaw-yaw + // set odometry pose from self_transform + auto & odom_pose = odometry.pose.pose; + odom_pose.position.x = self_transform->translation.x; + odom_pose.position.y = self_transform->translation.y; + odom_pose.position.z = self_transform->translation.z; + odom_pose.orientation = self_transform->rotation; + + // set odometry twist auto & odom_twist = odometry.twist.twist; odom_twist.linear.x = 10.0; // m/s odom_twist.linear.y = 0.1; // m/s odom_twist.angular.z = 0.1; // rad/s + // model the uncertainty + auto & odom_pose_cov = odometry.pose.covariance; + odom_pose_cov[0] = 0.1; // x-x + odom_pose_cov[7] = 0.1; // y-y + odom_pose_cov[35] = 0.0001; // yaw-yaw + auto & odom_twist_cov = odometry.twist.covariance; odom_twist_cov[0] = 2.0; // x-x [m^2/s^2] odom_twist_cov[7] = 0.2; // y-y [m^2/s^2] odom_twist_cov[35] = 0.001; // yaw-yaw [rad^2/s^2] // Add the odometry uncertainty to the object uncertainty - uncertainty::addOdometryUncertainty(odometry, input_objects_with_uncertainty); + uncertainty::addOdometryUncertainty(odometry, transformed_objects); } // Normalize the object uncertainty - uncertainty::normalizeUncertainty(input_objects_with_uncertainty); - - // Transform the objects to the world frame - DetectedObjects transformed_objects; - if (!autoware::object_recognition_utils::transformObjects( - input_objects_with_uncertainty, world_frame_id_, tf_buffer_, transformed_objects)) { - return; - } + uncertainty::normalizeUncertainty(transformed_objects); /* prediction */ processor_->predict(measurement_time);