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

feat(multi_object_tracker): object position update by size change #7170

Merged
merged 5 commits into from
Jun 12, 2024
Merged

feat(multi_object_tracker): object position update by size change #7170

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
17db3fc
fix: update object position offset caused by size change
technolojin Mar 13, 2024
4aae3a0
fix: update object size only by updating object size, do not memorize…
technolojin Mar 13, 2024
3d73987
fix: rename message package
technolojin Jun 5, 2024
715ff92
Merge branch 'main' into feat/mot_obj_position_update_by_size_change
technolojin Jun 12, 2024
aa7a2df
Merge branch 'main' into feat/mot_obj_position_update_by_size_change
technolojin Jun 12, 2024
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
12 changes: 0 additions & 12 deletions ...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,7 @@ class BigVehicleTracker : public Tracker
double height;
};
BoundingBox bounding_box_;
BoundingBox last_input_bounding_box_;
Eigen::Vector2d tracking_offset_;
int last_nearest_corner_index_;

private:
BicycleMotionModel motion_model_;
Expand All @@ -77,16 +75,6 @@ class BigVehicleTracker : public Tracker
const rclcpp::Time & time,
autoware_perception_msgs::msg::TrackedObject & object) const override;
virtual ~BigVehicleTracker() {}

private:
void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
{
Eigen::MatrixXd X_t(DIM, 1);
motion_model_.getStateVector(X_t);
last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
self_transform);
}
};

#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
12 changes: 0 additions & 12 deletions ...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,7 @@ class NormalVehicleTracker : public Tracker
double height;
};
BoundingBox bounding_box_;
BoundingBox last_input_bounding_box_;
Eigen::Vector2d tracking_offset_;
int last_nearest_corner_index_;

private:
BicycleMotionModel motion_model_;
Expand All @@ -77,16 +75,6 @@ class NormalVehicleTracker : public Tracker
const rclcpp::Time & time,
autoware_perception_msgs::msg::TrackedObject & object) const override;
virtual ~NormalVehicleTracker() {}

private:
void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
{
Eigen::MatrixXd X_t(DIM, 1);
motion_model_.getStateVector(X_t);
last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
self_transform);
}
};

#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
2 changes: 1 addition & 1 deletion perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -280,7 +280,7 @@ inline void calcAnchorPointOffset(

// update offset
const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx);
tracking_offset += offset;
tracking_offset = offset;

// offset input object
const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix();
Expand Down
47 changes: 18 additions & 29 deletions perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,143 +44,138 @@

BigVehicleTracker::BigVehicleTracker(
const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size,
const uint & channel_index)
: Tracker(time, object.classification, channel_size),
logger_(rclcpp::get_logger("BigVehicleTracker")),
z_(object.kinematics.pose_with_covariance.pose.position.z),
tracking_offset_(Eigen::Vector2d::Zero())
{
object_ = object;

// initialize existence probability
initializeExistenceProbabilities(channel_index, object.existence_probability);

// Initialize parameters
// measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
float r_stddev_x = 0.5; // in vehicle coordinate [m]
float r_stddev_y = 0.4; // in vehicle coordinate [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad]
float r_stddev_vel = 1.0; // in object coordinate [m/s]
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0);

// velocity deviation threshold
// if the predicted velocity is close to the observed velocity,
// the observed velocity is used as the measurement.
velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s]

// OBJECT SHAPE MODEL
if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
bounding_box_ = {
object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
last_input_bounding_box_ = bounding_box_;
} else {
autoware_perception_msgs::msg::DetectedObject bbox_object;
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
RCLCPP_WARN(
logger_,
"BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box.");
bounding_box_ = {6.0, 2.0, 2.0}; // default value
} else {
bounding_box_ = {
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
bbox_object.shape.dimensions.z};
}
last_input_bounding_box_ = bounding_box_;
}
// set maximum and minimum size
constexpr double max_size = 30.0;
constexpr double min_size = 1.0;
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);

// Set motion model parameters
{
constexpr double q_stddev_acc_long =
9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration
constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration
constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum
constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum
constexpr double q_stddev_slip_rate_min =
0.3; // [deg/s] uncertain slip angle change rate, minimum
constexpr double q_stddev_slip_rate_max =
10.0; // [deg/s] uncertain slip angle change rate, maximum
constexpr double q_max_slip_angle = 30; // [deg] max slip angle
constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position
constexpr double lf_min = 1.5; // [m] minimum front wheel position
constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position
constexpr double lr_min = 1.5; // [m] minimum rear wheel position
motion_model_.setMotionParams(
q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max,
q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio,
lr_min);
}

// Set motion limits
{
constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity
constexpr double max_slip = 30; // [deg] maximum slip angle
motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle
}

// Set initial state
{
const double x = object.kinematics.pose_with_covariance.pose.position.x;
const double y = object.kinematics.pose_with_covariance.pose.position.y;
const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
auto pose_cov = object.kinematics.pose_with_covariance.covariance;
double vel = 0.0;
double vel_cov;
const double & length = bounding_box_.length;

if (object.kinematics.has_twist) {
vel = object.kinematics.twist_with_covariance.twist.linear.x;
}

if (!object.kinematics.has_position_covariance) {
// initial state covariance
constexpr double p0_stddev_x = 1.5; // in object coordinate [m]
constexpr double p0_stddev_y = 0.5; // in object coordinate [m]
constexpr double p0_stddev_yaw =
tier4_autoware_utils::deg2rad(25); // in map coordinate [rad]
constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x;
constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y;
constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw;

const double cos_yaw = std::cos(yaw);
const double sin_yaw = std::sin(yaw);
const double sin_2yaw = std::sin(2.0 * yaw);
pose_cov[utils::MSG_COV_IDX::X_X] =
p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw;
pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw;
pose_cov[utils::MSG_COV_IDX::Y_Y] =
p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw;
pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y];
pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw;
}

if (!object.kinematics.has_twist_covariance) {
constexpr double p0_stddev_vel =
tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s]
vel_cov = std::pow(p0_stddev_vel, 2.0);
} else {
vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
}

const double slip = 0.0;
const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s]
const double slip_cov = std::pow(p0_stddev_slip, 2.0);

// initialize motion model
motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length);
}

Check notice on line 178 in perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Large Method 

BigVehicleTracker::BigVehicleTracker decreases from 112 to 109 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

/* calc nearest corner index*/
setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step
}

bool BigVehicleTracker::predict(const rclcpp::Time & time)
Expand Down Expand Up @@ -214,11 +209,11 @@
}

// get offset measurement
int nearest_corner_index = utils::getNearestCornerOrSurface(
const int nearest_corner_index = utils::getNearestCornerOrSurface(
tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform);
utils::calcAnchorPointOffset(
last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index,
bbox_object, tracked_yaw, updating_object, tracking_offset_);
bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw,
updating_object, tracking_offset_);

// UNCERTAINTY MODEL
if (!object.kinematics.has_position_covariance) {
Expand Down Expand Up @@ -332,15 +327,13 @@
return false;
}

constexpr double gain = 0.1;
constexpr double gain = 0.5;
constexpr double gain_inv = 1.0 - gain;

// update object size
bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x;
bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y;
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
last_input_bounding_box_ = {
object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};

// set maximum and minimum size
constexpr double max_size = 30.0;
Expand All @@ -352,11 +345,19 @@
// update motion model
motion_model_.updateExtendedState(bounding_box_.length);

// // update offset into object position
// motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
// // update offset
// tracking_offset_.x() = gain_inv * tracking_offset_.x();
// tracking_offset_.y() = gain_inv * tracking_offset_.y();
// update offset into object position
{
// rotate back the offset vector from object coordinate to global coordinate
const double yaw = motion_model_.getStateElement(IDX::YAW);
const double offset_x_global =
tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw);
const double offset_y_global =
tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw);
motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global);
// update offset (object coordinate)
tracking_offset_.x() = gain_inv * tracking_offset_.x();
tracking_offset_.y() = gain_inv * tracking_offset_.y();
}

return true;
}
Expand Down Expand Up @@ -390,9 +391,6 @@
measureWithPose(updating_object);
measureWithShape(updating_object);

/* calc nearest corner index*/
setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step

return true;
}

Expand All @@ -414,15 +412,6 @@
return false;
}

// recover bounding box from tracking point
const double dl = bounding_box_.length - last_input_bounding_box_.length;
const double dw = bounding_box_.width - last_input_bounding_box_.width;
const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint(
pose_with_cov.pose.position.x, pose_with_cov.pose.position.y,
motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_);
pose_with_cov.pose.position.x = recovered_pose.x();
pose_with_cov.pose.position.y = recovered_pose.y();

// position
pose_with_cov.pose.position.z = z_;

Expand Down
47 changes: 18 additions & 29 deletions perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,144 +44,139 @@

NormalVehicleTracker::NormalVehicleTracker(
const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size,
const uint & channel_index)
: Tracker(time, object.classification, channel_size),
logger_(rclcpp::get_logger("NormalVehicleTracker")),
z_(object.kinematics.pose_with_covariance.pose.position.z),
tracking_offset_(Eigen::Vector2d::Zero())
{
object_ = object;

// initialize existence probability
initializeExistenceProbabilities(channel_index, object.existence_probability);

// Initialize parameters
// measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
float r_stddev_x = 0.5; // in vehicle coordinate [m]
float r_stddev_y = 0.4; // in vehicle coordinate [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad]
float r_stddev_vel = 1.0; // in object coordinate [m/s]
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0);

// velocity deviation threshold
// if the predicted velocity is close to the observed velocity,
// the observed velocity is used as the measurement.
velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s]

// OBJECT SHAPE MODEL
if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
bounding_box_ = {
object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
last_input_bounding_box_ = bounding_box_;
} else {
autoware_perception_msgs::msg::DetectedObject bbox_object;
if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
RCLCPP_WARN(
logger_,
"NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding "
"box.");
bounding_box_ = {3.0, 2.0, 1.8}; // default value
} else {
bounding_box_ = {
bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
bbox_object.shape.dimensions.z};
}
last_input_bounding_box_ = bounding_box_;
}
// set maximum and minimum size
constexpr double max_size = 20.0;
constexpr double min_size = 1.0;
bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);

// Set motion model parameters
{
constexpr double q_stddev_acc_long =
9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration
constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration
constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum
constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum
constexpr double q_stddev_slip_rate_min =
0.3; // [deg/s] uncertain slip angle change rate, minimum
constexpr double q_stddev_slip_rate_max =
10.0; // [deg/s] uncertain slip angle change rate, maximum
constexpr double q_max_slip_angle = 30; // [deg] max slip angle
constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position
constexpr double lf_min = 1.0; // [m] minimum front wheel position
constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position
constexpr double lr_min = 1.0; // [m] minimum rear wheel position
motion_model_.setMotionParams(
q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max,
q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio,
lr_min);
}

// Set motion limits
{
constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity
constexpr double max_slip = 30; // [deg] maximum slip angle
motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle
}

// Set initial state
{
const double x = object.kinematics.pose_with_covariance.pose.position.x;
const double y = object.kinematics.pose_with_covariance.pose.position.y;
const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
auto pose_cov = object.kinematics.pose_with_covariance.covariance;
double vel = 0.0;
double vel_cov;
const double & length = bounding_box_.length;

if (object.kinematics.has_twist) {
vel = object.kinematics.twist_with_covariance.twist.linear.x;
}

if (!object.kinematics.has_position_covariance) {
// initial state covariance
constexpr double p0_stddev_x = 1.0; // in object coordinate [m]
constexpr double p0_stddev_y = 0.3; // in object coordinate [m]
constexpr double p0_stddev_yaw =
tier4_autoware_utils::deg2rad(25); // in map coordinate [rad]
constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x;
constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y;
constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw;

const double cos_yaw = std::cos(yaw);
const double sin_yaw = std::sin(yaw);
const double sin_2yaw = std::sin(2.0 * yaw);
pose_cov[utils::MSG_COV_IDX::X_X] =
p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw;
pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw;
pose_cov[utils::MSG_COV_IDX::Y_Y] =
p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw;
pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y];
pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw;
}

if (!object.kinematics.has_twist_covariance) {
constexpr double p0_stddev_vel =
tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s]
vel_cov = std::pow(p0_stddev_vel, 2.0);
} else {
vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
}

const double slip = 0.0;
const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s]
const double slip_cov = std::pow(p0_stddev_slip, 2.0);

// initialize motion model
motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length);
}

Check notice on line 179 in perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Large Method 

NormalVehicleTracker::NormalVehicleTracker decreases from 113 to 110 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

/* calc nearest corner index*/
setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step
}

bool NormalVehicleTracker::predict(const rclcpp::Time & time)
Expand Down Expand Up @@ -216,11 +211,11 @@
}

// get offset measurement
int nearest_corner_index = utils::getNearestCornerOrSurface(
const int nearest_corner_index = utils::getNearestCornerOrSurface(
tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform);
utils::calcAnchorPointOffset(
last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index,
bbox_object, tracked_yaw, updating_object, tracking_offset_);
bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw,
updating_object, tracking_offset_);

// UNCERTAINTY MODEL
if (!object.kinematics.has_position_covariance) {
Expand Down Expand Up @@ -334,15 +329,13 @@
return false;
}

constexpr double gain = 0.1;
constexpr double gain = 0.5;
constexpr double gain_inv = 1.0 - gain;

// update object size
bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x;
bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y;
bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
last_input_bounding_box_ = {
object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};

// set maximum and minimum size
constexpr double max_size = 20.0;
Expand All @@ -354,11 +347,19 @@
// update motion model
motion_model_.updateExtendedState(bounding_box_.length);

// // update offset into object position
// motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
// // update offset
// tracking_offset_.x() = gain_inv * tracking_offset_.x();
// tracking_offset_.y() = gain_inv * tracking_offset_.y();
// update offset into object position
{
// rotate back the offset vector from object coordinate to global coordinate
const double yaw = motion_model_.getStateElement(IDX::YAW);
const double offset_x_global =
tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw);
const double offset_y_global =
tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw);
motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global);
// update offset (object coordinate)
tracking_offset_.x() = gain_inv * tracking_offset_.x();
tracking_offset_.y() = gain_inv * tracking_offset_.y();
}

return true;
}
Expand Down Expand Up @@ -392,9 +393,6 @@
measureWithPose(updating_object);
measureWithShape(updating_object);

/* calc nearest corner index*/
setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step

return true;
}

Expand All @@ -416,15 +414,6 @@
return false;
}

// recover bounding box from tracking point
const double dl = bounding_box_.length - last_input_bounding_box_.length;
const double dw = bounding_box_.width - last_input_bounding_box_.width;
const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint(
pose_with_cov.pose.position.x, pose_with_cov.pose.position.y,
motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_);
pose_with_cov.pose.position.x = recovered_pose.x();
pose_with_cov.pose.position.y = recovered_pose.y();

// position
pose_with_cov.pose.position.z = z_;

Expand Down
Loading