Skip to content

Commit c497588

Browse files
technolojintechnolojin
committed
fix: rollback tracking size and offset algorithm
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 0ce0cbc commit c497588

File tree

4 files changed

+66
-12
lines changed

4 files changed

+66
-12
lines changed

‎perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp

+11
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ class BigVehicleTracker : public Tracker
5252
BoundingBox bounding_box_;
5353
BoundingBox last_input_bounding_box_;
5454
Eigen::Vector2d tracking_offset_;
55+
int last_nearest_corner_index_;
5556

5657
private:
5758
BicycleMotionModel motion_model_;
@@ -76,6 +77,16 @@ class BigVehicleTracker : public Tracker
7677
const rclcpp::Time & time,
7778
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
7879
virtual ~BigVehicleTracker() {}
80+
81+
private:
82+
void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
83+
{
84+
Eigen::MatrixXd X_t(DIM, 1);
85+
motion_model_.getStateVector(X_t);
86+
last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
87+
X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
88+
self_transform);
89+
}
7990
};
8091

8192
#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_

‎perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp

+11
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,7 @@ class NormalVehicleTracker : public Tracker
5353
BoundingBox bounding_box_;
5454
BoundingBox last_input_bounding_box_;
5555
Eigen::Vector2d tracking_offset_;
56+
int last_nearest_corner_index_;
5657

5758
private:
5859
BicycleMotionModel motion_model_;
@@ -77,6 +78,16 @@ class NormalVehicleTracker : public Tracker
7778
const rclcpp::Time & time,
7879
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
7980
virtual ~NormalVehicleTracker() {}
81+
82+
private:
83+
void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
84+
{
85+
Eigen::MatrixXd X_t(DIM, 1);
86+
motion_model_.getStateVector(X_t);
87+
last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
88+
X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
89+
self_transform);
90+
}
8091
};
8192

8293
#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_

‎perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

+22-6
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
4444

4545
BigVehicleTracker::BigVehicleTracker(
4646
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
47-
const geometry_msgs::msg::Transform & /*self_transform*/)
47+
const geometry_msgs::msg::Transform & self_transform)
4848
: Tracker(time, object.classification),
4949
logger_(rclcpp::get_logger("BigVehicleTracker")),
5050
// last_update_time_(time),
@@ -167,6 +167,9 @@ BigVehicleTracker::BigVehicleTracker(
167167
// initialize motion model
168168
motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length);
169169
}
170+
171+
/* calc nearest corner index*/
172+
setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step
170173
}
171174

172175
bool BigVehicleTracker::predict(const rclcpp::Time & time)
@@ -316,11 +319,12 @@ bool BigVehicleTracker::measureWithShape(
316319

317320
// update motion model
318321
motion_model_.updateExtendedState(bounding_box_.length);
319-
// update offset into object position
320-
motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
321-
// update offset
322-
tracking_offset_.x() = gain_inv * tracking_offset_.x();
323-
tracking_offset_.y() = gain_inv * tracking_offset_.y();
322+
323+
// // update offset into object position
324+
// motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
325+
// // update offset
326+
// tracking_offset_.x() = gain_inv * tracking_offset_.x();
327+
// tracking_offset_.y() = gain_inv * tracking_offset_.y();
324328

325329
return true;
326330
}
@@ -354,6 +358,9 @@ bool BigVehicleTracker::measure(
354358
measureWithPose(updating_object);
355359
measureWithShape(updating_object);
356360

361+
/* calc nearest corner index*/
362+
setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step
363+
357364
return true;
358365
}
359366

@@ -375,6 +382,15 @@ bool BigVehicleTracker::getTrackedObject(
375382
return false;
376383
}
377384

385+
// recover bounding box from tracking point
386+
const double dl = bounding_box_.length - last_input_bounding_box_.length;
387+
const double dw = bounding_box_.width - last_input_bounding_box_.width;
388+
const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint(
389+
pose_with_cov.pose.position.x, pose_with_cov.pose.position.y,
390+
motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_);
391+
pose_with_cov.pose.position.x = recovered_pose.x();
392+
pose_with_cov.pose.position.y = recovered_pose.y();
393+
378394
// position
379395
pose_with_cov.pose.position.z = z_;
380396

‎perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

+22-6
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
4444

4545
NormalVehicleTracker::NormalVehicleTracker(
4646
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
47-
const geometry_msgs::msg::Transform & /*self_transform*/)
47+
const geometry_msgs::msg::Transform & self_transform)
4848
: Tracker(time, object.classification),
4949
logger_(rclcpp::get_logger("NormalVehicleTracker")),
5050
// last_update_time_(time),
@@ -167,6 +167,9 @@ NormalVehicleTracker::NormalVehicleTracker(
167167
// initialize motion model
168168
motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length);
169169
}
170+
171+
/* calc nearest corner index*/
172+
setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step
170173
}
171174

172175
bool NormalVehicleTracker::predict(const rclcpp::Time & time)
@@ -316,11 +319,12 @@ bool NormalVehicleTracker::measureWithShape(
316319

317320
// update motion model
318321
motion_model_.updateExtendedState(bounding_box_.length);
319-
// update offset into object position
320-
motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
321-
// update offset
322-
tracking_offset_.x() = gain_inv * tracking_offset_.x();
323-
tracking_offset_.y() = gain_inv * tracking_offset_.y();
322+
323+
// // update offset into object position
324+
// motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
325+
// // update offset
326+
// tracking_offset_.x() = gain_inv * tracking_offset_.x();
327+
// tracking_offset_.y() = gain_inv * tracking_offset_.y();
324328

325329
return true;
326330
}
@@ -354,6 +358,9 @@ bool NormalVehicleTracker::measure(
354358
measureWithPose(updating_object);
355359
measureWithShape(updating_object);
356360

361+
/* calc nearest corner index*/
362+
setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step
363+
357364
return true;
358365
}
359366

@@ -375,6 +382,15 @@ bool NormalVehicleTracker::getTrackedObject(
375382
return false;
376383
}
377384

385+
// recover bounding box from tracking point
386+
const double dl = bounding_box_.length - last_input_bounding_box_.length;
387+
const double dw = bounding_box_.width - last_input_bounding_box_.width;
388+
const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint(
389+
pose_with_cov.pose.position.x, pose_with_cov.pose.position.y,
390+
motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_);
391+
pose_with_cov.pose.position.x = recovered_pose.x();
392+
pose_with_cov.pose.position.y = recovered_pose.y();
393+
378394
// position
379395
pose_with_cov.pose.position.z = z_;
380396

0 commit comments

Comments
 (0)