From 08009ed10de676ea3e56d72cde4fe7ffb1f87ca8 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 19 Sep 2023 11:58:56 +0900 Subject: [PATCH 1/4] fix(radar_fusion_to_detected_object): fix confidence (#4992) Signed-off-by: scepter914 --- .../src/radar_fusion_to_detected_object.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 4f0933e993de8..523124078d7f2 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -293,7 +293,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( bool RadarFusionToDetectedObject::isQualified( const DetectedObject & object, std::shared_ptr> & radars) { - if (object.classification[0].probability > param_.threshold_probability) { + if (object.existence_probability > param_.threshold_probability) { return true; } else { if (!radars || !(*radars).empty()) { From 8c6ec88a65445697a0bcd478785349418dabb128 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 19 Sep 2023 14:16:06 +0900 Subject: [PATCH 2/4] fix(radar_tracks_msg_converter): fix twist coordinate conversion (#4993) Signed-off-by: scepter914 --- .../radar_tracks_msgs_converter_node.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 5d3dd3b997f24..ddd1d47c66664 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -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; @@ -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; From 131f26a61bf215f8935610a14d9a8de60071c69b Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 19 Sep 2023 17:39:39 +0900 Subject: [PATCH 3/4] chore: fix spell-check errors in apollo-related packages (#4987) Signed-off-by: kminoda --- .../lidar_apollo_instance_segmentation/cluster2d.hpp | 4 ++-- .../lidar_apollo_instance_segmentation/src/cluster2d.cpp | 6 +++--- .../include/lidar_apollo_segmentation_tvm/cluster2d.hpp | 4 ++-- perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp | 6 +++--- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp index 53cab7409578e..1bb6da0374fe9 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp @@ -79,12 +79,12 @@ struct Obstacle float height; float heading; MetaType meta_type; - std::vector meta_type_probs; + std::vector meta_type_probabilities; Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(META_UNKNOWN) { cloud_ptr.reset(new pcl::PointCloud); - meta_type_probs.assign(MAX_META_TYPE, 0.0); + meta_type_probabilities.assign(MAX_META_TYPE, 0.0); } }; diff --git a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp index a7b1bee69aebb..8d758531758eb 100644 --- a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp @@ -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; } } diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp index 35655cd16cb7c..4587184ecfb1f 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp @@ -57,12 +57,12 @@ struct Obstacle float height; float heading; MetaType meta_type; - std::vector meta_type_probs; + std::vector meta_type_probabilities; Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(MetaType::META_UNKNOWN) { cloud_ptr.reset(new pcl::PointCloud); - meta_type_probs.assign(static_cast(MetaType::MAX_META_TYPE), 0.0); + meta_type_probabilities.assign(static_cast(MetaType::MAX_META_TYPE), 0.0); } }; diff --git a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp index 39c0da04a9c39..f96a32a5bb465 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp @@ -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(obs->grids.size()); - if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) { + obs->meta_type_probabilities[k] /= static_cast(obs->grids.size()); + if (obs->meta_type_probabilities[k] > obs->meta_type_probabilities[meta_type_id]) { meta_type_id = k; } } From 2a9f160fa239a7e9ed7705b398c2fb871481a9b5 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 20 Sep 2023 09:56:29 +0900 Subject: [PATCH 4/4] chore(radar_object_tracker): fix spell-check error (#5024) * chore(radar_object_tracker): fix spell-check errors Signed-off-by: kminoda * Finally perception directory has been removed from cspell ignorance Signed-off-by: kminoda * fix Signed-off-by: kminoda * style(pre-commit): autofix * revert fix of spell-check dict Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../tracker/model/linear_motion_tracker.cpp | 112 +++++++++--------- 1 file changed, 56 insertions(+), 56 deletions(-) diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index bfd84159706fb..62880d9471047 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -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. @@ -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) { @@ -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) { @@ -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; @@ -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); @@ -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(); }); @@ -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); @@ -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; @@ -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;