Skip to content

Commit

Permalink
refactor: remove MSG_COV_IDX enum in utils.hpp
Browse files Browse the repository at this point in the history 
Signed-off-by: Taekjin LEE <[email protected]>
  • Loading branch information
@technolojin
technolojin committed Jun 24, 2024
1 parent ee204c0 commit 7f78781
Show file tree
Hide file tree
Showing 3 changed files with 71 additions and 107 deletions.
39 changes: 0 additions & 39 deletions perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,45 +35,6 @@
#include <vector>
namespace autoware::radar_object_tracker::utils
{
enum MSG_COV_IDX {
X_X = 0,
X_Y = 1,
X_Z = 2,
X_ROLL = 3,
X_PITCH = 4,
X_YAW = 5,
Y_X = 6,
Y_Y = 7,
Y_Z = 8,
Y_ROLL = 9,
Y_PITCH = 10,
Y_YAW = 11,
Z_X = 12,
Z_Y = 13,
Z_Z = 14,
Z_ROLL = 15,
Z_PITCH = 16,
Z_YAW = 17,
ROLL_X = 18,
ROLL_Y = 19,
ROLL_Z = 20,
ROLL_ROLL = 21,
ROLL_PITCH = 22,
ROLL_YAW = 23,
PITCH_X = 24,
PITCH_Y = 25,
PITCH_Z = 26,
PITCH_ROLL = 27,
PITCH_PITCH = 28,
PITCH_YAW = 29,
YAW_X = 30,
YAW_Y = 31,
YAW_Z = 32,
YAW_ROLL = 33,
YAW_PITCH = 34,
YAW_YAW = 35
};

// matrix concatenate
Eigen::MatrixXd stackMatricesVertically(const std::vector<Eigen::MatrixXd> & matrices);
Eigen::MatrixXd stackMatricesDiagonally(const std::vector<Eigen::MatrixXd> & matrices);
Expand Down
56 changes: 29 additions & 27 deletions perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include "autoware/radar_object_tracker/utils/utils.hpp"
#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
#include "autoware/universe_utils/math/unit_conversion.hpp"
#include "autoware/universe_utils/ros/msg_covariance.hpp"

#include <bits/stdc++.h>
#include <tf2/LinearMath/Matrix3x3.h>
Expand All @@ -45,6 +46,7 @@
namespace autoware::radar_object_tracker
{
using Label = autoware_perception_msgs::msg::ObjectClassification;
using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;

// init static member variables
bool ConstantTurnRateMotionTracker::is_initialized_ = false;
Expand Down Expand Up @@ -116,10 +118,10 @@ ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker(
// Rotate the covariance matrix according to the vehicle yaw
// because p0_cov_x and y are in the vehicle coordinate system.
if (object.kinematics.has_position_covariance) {
P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
P_xy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X],
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y],
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X],
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];
P_xy = R * P_xy_local * R.transpose();
} else {
// rotate
Expand All @@ -130,9 +132,9 @@ ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker(

// covariance often written in object frame
if (object.kinematics.has_twist_covariance) {
const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
const auto vx_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X];

Check warning on line 135 in perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp#L135 

Added line #L135 was not covered by tests
// const auto vy_cov =
// object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
// object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];
P(IDX::VX, IDX::VX) = vx_cov;
} else {
P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
Expand Down Expand Up @@ -373,11 +375,11 @@ bool ConstantTurnRateMotionTracker::measureWithPose(
Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate
Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose();
} else {
Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X],
Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X],

Check warning on line 378 in perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp#L378 

Added line #L378 was not covered by tests
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y],
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X],
object.kinematics.pose_with_covariance
.covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate
.covariance[XYZRPY_COV_IDX::Y_Y]; // xy in vehicle coordinate

Check warning on line 382 in perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp#L382 

Added line #L382 was not covered by tests
Rxy = RotationYaw * Rxy_local * RotationYaw.transpose();
}
R_block_list.push_back(Rxy);
Expand Down Expand Up @@ -429,7 +431,7 @@ bool ConstantTurnRateMotionTracker::measureWithPose(
if (!object.kinematics.has_twist_covariance) {
R_vx << ekf_params_.r_cov_vx;
} else {
R_vx << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
R_vx << object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X];
}
R_block_list.push_back(R_vx);
}
Expand Down Expand Up @@ -584,27 +586,27 @@ bool ConstantTurnRateMotionTracker::getTrackedObject(
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] = 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;
pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov;
pose_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_xy(IDX::X, IDX::X);
pose_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y);
pose_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X);
pose_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y);
pose_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = z_cov;
pose_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
pose_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
pose_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov;

Check warning on line 596 in perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp#L589-L596 

Added lines #L589 - L596 were not covered by tests

// twist covariance
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] = P(IDX::VX, IDX::VX);
twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = 0.0;
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = 0.0;
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = no_info_cov;
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;
twist_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
twist_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = 0.0;
twist_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = 0.0;
twist_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = no_info_cov;
twist_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = vz_cov;
twist_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
twist_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
twist_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = wz_cov;

Check warning on line 609 in perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp#L602-L609 

Added lines #L602 - L609 were not covered by tests

// set shape
if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
Expand Down
83 changes: 42 additions & 41 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 @@ -21,9 +21,9 @@
#include "autoware/radar_object_tracker/tracker/model/linear_motion_tracker.hpp"

#include "autoware/radar_object_tracker/utils/utils.hpp"

#include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
#include <autoware/universe_utils/math/unit_conversion.hpp>
#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
#include "autoware/universe_utils/math/unit_conversion.hpp"
#include "autoware/universe_utils/ros/msg_covariance.hpp"

#include <bits/stdc++.h>
#include <tf2/LinearMath/Matrix3x3.h>
Expand All @@ -46,6 +46,7 @@
namespace autoware::radar_object_tracker
{
using Label = autoware_perception_msgs::msg::ObjectClassification;
using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;

// initialize static parameter
bool LinearMotionTracker::is_initialized_ = false;
Expand Down Expand Up @@ -122,19 +123,19 @@ LinearMotionTracker::LinearMotionTracker(
// Rotate the covariance matrix according to the vehicle yaw
// because p0_cov_x and y are in the vehicle coordinate system.
if (object.kinematics.has_position_covariance) {
P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
P_xy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X],
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y],
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X],
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];
P_xy = R * P_xy_local * R.transpose();
} else {
// rotate
P_xy = R * P_xy_local * R.transpose();
}
// covariance often written in object frame
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];
const auto vx_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X];
const auto vy_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];

Check warning on line 138 in perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp#L137-L138 

Added lines #L137 - L138 were not covered by tests
P_v_xy_local << vx_cov, 0.0, 0.0, vy_cov;
P_v_xy = R * P_v_xy_local * R.transpose();
} else {
Expand All @@ -145,10 +146,10 @@ LinearMotionTracker::LinearMotionTracker(
false; // currently message does not have acceleration covariance
if (has_acceleration_covariance) {
// const auto ax_cov =
// object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; // This
// object.kinematics.acceleration_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; // This
// is future update
// const auto ay_cov =
// object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This
// object.kinematics.acceleration_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; // This
// is future update
// Eigen::Matrix2d P_a_xy_local;
// P_a_xy_local << ax_cov, 0.0, 0.0, ay_cov;
Expand Down Expand Up @@ -418,11 +419,11 @@ bool LinearMotionTracker::measureWithPose(
Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate
Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose();
} else {
Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y],
object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X],
Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X],

Check warning on line 422 in perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp#L422 

Added line #L422 was not covered by tests
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y],
object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X],
object.kinematics.pose_with_covariance
.covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate
.covariance[XYZRPY_COV_IDX::Y_Y]; // xy in vehicle coordinate

Check warning on line 426 in perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp#L426 

Added line #L426 was not covered by tests
Rxy = RotationYaw * Rxy_local * RotationYaw.transpose();
}
R_block_list.push_back(Rxy);
Expand Down Expand Up @@ -450,8 +451,8 @@ bool LinearMotionTracker::measureWithPose(
R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy;
R_v_xy = RotationBaseLink * R_v_xy_local * RotationBaseLink.transpose();
} else {
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];
R_v_xy_local << object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X], 0, 0,
object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y];
R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose();
}
R_block_list.push_back(R_v_xy);
Expand Down Expand Up @@ -640,37 +641,37 @@ bool LinearMotionTracker::getTrackedObject(
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] = 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;
pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov;
pose_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_xy(IDX::X, IDX::X);
pose_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y);
pose_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X);
pose_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y);
pose_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = z_cov;
pose_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
pose_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
pose_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov;

Check warning on line 651 in perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp#L644-L651 

Added lines #L644 - L651 were not covered by tests

// twist covariance
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] = 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;
twist_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X);
twist_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y);
twist_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X);
twist_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y);
twist_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = vz_cov;
twist_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
twist_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
twist_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = wz_cov;

Check warning on line 664 in perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp#L657-L664 

Added lines #L657 - L664 were not covered by tests

// acceleration covariance
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;
acceleration_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = no_info_cov;
acceleration_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X);
acceleration_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y);
acceleration_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X);
acceleration_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y);
acceleration_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = no_info_cov;
acceleration_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov;
acceleration_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov;
acceleration_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = no_info_cov;

Check warning on line 674 in perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp#L667-L674 

Added lines #L667 - L674 were not covered by tests

// set shape
if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
Expand Down

0 comments on commit 7f78781

Please sign in to comment.