Skip to content

Commit ec8c982

Browse files
technolojintechnolojin
committed
refactor(multi_object_tracker): modify getNearestCornerOrSurface function signature and update related logic
Signed-off-by: Taekjin LEE <[email protected]> refactor(multi_object_tracker): remove self_transform parameter from measure and update methods refactor(multi_object_tracker): update calcAnchorPointOffset function signature and streamline object handling refactor(multi_object_tracker): set shape type to BOUNDING_BOX for object trackers
1 parent ab23f91 commit ec8c982

22 files changed

+69
-157
lines changed

‎perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp

+3-4
Original file line numberDiff line numberDiff line change
@@ -37,12 +37,11 @@ bool convertConvexHullToBoundingBox(
3737
bool getMeasurementYaw(
3838
const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw);
3939

40-
geometry_msgs::msg::Point getNearestCornerOrSurface(
41-
const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform);
40+
void getNearestCornerOrSurface(
41+
const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object);
4242

4343
void calcAnchorPointOffset(
44-
const types::DynamicObject & this_object, const types::DynamicObject & input_object,
45-
const geometry_msgs::msg::Point anchor_vector, Eigen::Vector2d & tracking_offset,
44+
const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset,
4645
types::DynamicObject & offset_object);
4746
} // namespace shapes
4847
} // namespace autoware::multi_object_tracker

‎perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp

+1-8
Original file line numberDiff line numberDiff line change
@@ -43,17 +43,10 @@ class BicycleTracker : public Tracker
4343
BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
4444

4545
bool predict(const rclcpp::Time & time) override;
46-
bool measure(
47-
const types::DynamicObject & object, const rclcpp::Time & time,
48-
const geometry_msgs::msg::Transform & self_transform) override;
46+
bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
4947
bool measureWithPose(const types::DynamicObject & object);
5048
bool measureWithShape(const types::DynamicObject & object);
5149
bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
52-
53-
private:
54-
types::DynamicObject getUpdatingObject(
55-
const types::DynamicObject & object,
56-
const geometry_msgs::msg::Transform & self_transform) const;
5750
};
5851

5952
} // namespace autoware::multi_object_tracker

‎perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp

+1-3
Original file line numberDiff line numberDiff line change
@@ -39,9 +39,7 @@ class MultipleVehicleTracker : public Tracker
3939
MultipleVehicleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
4040

4141
bool predict(const rclcpp::Time & time) override;
42-
bool measure(
43-
const types::DynamicObject & object, const rclcpp::Time & time,
44-
const geometry_msgs::msg::Transform & self_transform) override;
42+
bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
4543
bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
4644
virtual ~MultipleVehicleTracker() {}
4745
};

‎perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp

+1-3
Original file line numberDiff line numberDiff line change
@@ -37,9 +37,7 @@ class PassThroughTracker : public Tracker
3737
public:
3838
PassThroughTracker(const rclcpp::Time & time, const types::DynamicObject & object);
3939
bool predict(const rclcpp::Time & time) override;
40-
bool measure(
41-
const types::DynamicObject & object, const rclcpp::Time & time,
42-
const geometry_msgs::msg::Transform & self_transform) override;
40+
bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
4341
bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
4442
};
4543

‎perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp

+1-3
Original file line numberDiff line numberDiff line change
@@ -39,9 +39,7 @@ class PedestrianAndBicycleTracker : public Tracker
3939
PedestrianAndBicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
4040

4141
bool predict(const rclcpp::Time & time) override;
42-
bool measure(
43-
const types::DynamicObject & object, const rclcpp::Time & time,
44-
const geometry_msgs::msg::Transform & self_transform) override;
42+
bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
4543
bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
4644
virtual ~PedestrianAndBicycleTracker() {}
4745
};

‎perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp

+1-8
Original file line numberDiff line numberDiff line change
@@ -44,17 +44,10 @@ class PedestrianTracker : public Tracker
4444
PedestrianTracker(const rclcpp::Time & time, const types::DynamicObject & object);
4545

4646
bool predict(const rclcpp::Time & time) override;
47-
bool measure(
48-
const types::DynamicObject & object, const rclcpp::Time & time,
49-
const geometry_msgs::msg::Transform & self_transform) override;
47+
bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
5048
bool measureWithPose(const types::DynamicObject & object);
5149
bool measureWithShape(const types::DynamicObject & object);
5250
bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
53-
54-
private:
55-
types::DynamicObject getUpdatingObject(
56-
const types::DynamicObject & object,
57-
const geometry_msgs::msg::Transform & self_transform) const;
5851
};
5952

6053
} // namespace autoware::multi_object_tracker

‎perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ class Tracker
6161
}
6262
bool updateWithMeasurement(
6363
const types::DynamicObject & object, const rclcpp::Time & measurement_time,
64-
const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info);
64+
const types::InputChannel & channel_info);
6565
bool updateWithoutMeasurement(const rclcpp::Time & now);
6666

6767
std::uint8_t getHighestProbLabel() const
@@ -87,9 +87,7 @@ class Tracker
8787
void limitObjectExtension(const object_model::ObjectModel object_model);
8888

8989
// virtual functions
90-
virtual bool measure(
91-
const types::DynamicObject & object, const rclcpp::Time & time,
92-
const geometry_msgs::msg::Transform & self_transform) = 0;
90+
virtual bool measure(const types::DynamicObject & object, const rclcpp::Time & time) = 0;
9391

9492
public:
9593
virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0;

‎perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp

+1-5
Original file line numberDiff line numberDiff line change
@@ -43,11 +43,7 @@ class UnknownTracker : public Tracker
4343
UnknownTracker(const rclcpp::Time & time, const types::DynamicObject & object);
4444

4545
bool predict(const rclcpp::Time & time) override;
46-
bool measure(
47-
const types::DynamicObject & object, const rclcpp::Time & time,
48-
const geometry_msgs::msg::Transform & self_transform) override;
49-
types::DynamicObject getUpdatingObject(
50-
const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform);
46+
bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
5147
bool measureWithPose(const types::DynamicObject & object);
5248
bool measureWithShape(const types::DynamicObject & object);
5349
bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;

‎perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp

+1-7
Original file line numberDiff line numberDiff line change
@@ -48,16 +48,10 @@ class VehicleTracker : public Tracker
4848
const types::DynamicObject & object);
4949

5050
bool predict(const rclcpp::Time & time) override;
51-
bool measure(
52-
const types::DynamicObject & object, const rclcpp::Time & time,
53-
const geometry_msgs::msg::Transform & self_transform) override;
51+
bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
5452
bool measureWithPose(const types::DynamicObject & object);
5553
bool measureWithShape(const types::DynamicObject & object);
5654
bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
57-
58-
private:
59-
types::DynamicObject getUpdatingObject(
60-
const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform);
6155
};
6256

6357
} // namespace autoware::multi_object_tracker

‎perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp

+11-13
Original file line numberDiff line numberDiff line change
@@ -171,8 +171,8 @@ enum BBOX_IDX {
171171
* @param self_transform: Ego vehicle position in map frame
172172
* @return int index
173173
*/
174-
geometry_msgs::msg::Point getNearestCornerOrSurface(
175-
const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform)
174+
void getNearestCornerOrSurface(
175+
const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object)
176176
{
177177
const double x = object.pose.position.x;
178178
const double y = object.pose.position.y;
@@ -212,24 +212,22 @@ geometry_msgs::msg::Point getNearestCornerOrSurface(
212212
} else {
213213
anchor_y = -width / 2.0;
214214
}
215-
geometry_msgs::msg::Point anchor_point;
216-
anchor_point.x = anchor_x;
217-
anchor_point.y = anchor_y;
218-
return anchor_point;
215+
216+
object.anchor_point.x = anchor_x;
217+
object.anchor_point.y = anchor_y;
219218
}
220219

221220
void calcAnchorPointOffset(
222-
const types::DynamicObject & this_object, const types::DynamicObject & input_object,
223-
const geometry_msgs::msg::Point anchor_vector, Eigen::Vector2d & tracking_offset,
224-
types::DynamicObject & offset_object)
221+
const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset,
222+
types::DynamicObject & updating_object)
225223
{
226224
// copy value
227-
offset_object = input_object;
225+
const geometry_msgs::msg::Point anchor_vector = updating_object.anchor_point;
228226
// invalid anchor
229227
if (anchor_vector.x <= 1e-6 && anchor_vector.y <= 1e-6) {
230228
return;
231229
}
232-
double input_yaw = tf2::getYaw(input_object.pose.orientation);
230+
double input_yaw = tf2::getYaw(updating_object.pose.orientation);
233231

234232
// current object width and height
235233
const double length = this_object.shape.dimensions.x;
@@ -251,8 +249,8 @@ void calcAnchorPointOffset(
251249
// offset input object
252250
const Eigen::Matrix2d R = Eigen::Rotation2Dd(input_yaw).toRotationMatrix();
253251
const Eigen::Vector2d rotated_offset = R * tracking_offset;
254-
offset_object.pose.position.x += rotated_offset.x();
255-
offset_object.pose.position.y += rotated_offset.y();
252+
updating_object.pose.position.x += rotated_offset.x();
253+
updating_object.pose.position.y += rotated_offset.y();
256254
}
257255

258256
} // namespace shapes

‎perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp

+7-15
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,8 @@ BicycleTracker::BicycleTracker(const rclcpp::Time & time, const types::DynamicOb
5050
object_extension.y = object_model_.init_size.width;
5151
object_extension.z = object_model_.init_size.height;
5252
}
53+
object_.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
54+
5355
// set maximum and minimum size
5456
limitObjectExtension(object_model_);
5557

@@ -126,13 +128,6 @@ bool BicycleTracker::predict(const rclcpp::Time & time)
126128
return motion_model_.predictState(time);
127129
}
128130

129-
types::DynamicObject BicycleTracker::getUpdatingObject(
130-
const types::DynamicObject & object,
131-
const geometry_msgs::msg::Transform & /*self_transform*/) const
132-
{
133-
return object;
134-
}
135-
136131
bool BicycleTracker::measureWithPose(const types::DynamicObject & object)
137132
{
138133
// get measurement yaw angle to update
@@ -199,9 +194,7 @@ bool BicycleTracker::measureWithShape(const types::DynamicObject & object)
199194
return true;
200195
}
201196

202-
bool BicycleTracker::measure(
203-
const types::DynamicObject & object, const rclcpp::Time & time,
204-
const geometry_msgs::msg::Transform & self_transform)
197+
bool BicycleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
205198
{
206199
// check time gap
207200
const double dt = motion_model_.getDeltaTime(time);
@@ -214,9 +207,8 @@ bool BicycleTracker::measure(
214207
}
215208

216209
// update object
217-
const types::DynamicObject updating_object = getUpdatingObject(object, self_transform);
218-
measureWithPose(updating_object);
219-
measureWithShape(updating_object);
210+
measureWithPose(object);
211+
measureWithShape(object);
220212

221213
return true;
222214
}
@@ -225,12 +217,12 @@ bool BicycleTracker::getTrackedObject(
225217
const rclcpp::Time & time, types::DynamicObject & object) const
226218
{
227219
object = object_;
220+
221+
// predict from motion model
228222
auto & pose = object.pose;
229223
auto & pose_cov = object.pose_covariance;
230224
auto & twist = object.twist;
231225
auto & twist_cov = object.twist_covariance;
232-
233-
// predict from motion model
234226
if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) {
235227
RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state.");
236228
return false;

‎perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp

+3-5
Original file line numberDiff line numberDiff line change
@@ -37,12 +37,10 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time)
3737
return true;
3838
}
3939

40-
bool MultipleVehicleTracker::measure(
41-
const types::DynamicObject & object, const rclcpp::Time & time,
42-
const geometry_msgs::msg::Transform & self_transform)
40+
bool MultipleVehicleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
4341
{
44-
big_vehicle_tracker_.measure(object, time, self_transform);
45-
normal_vehicle_tracker_.measure(object, time, self_transform);
42+
big_vehicle_tracker_.measure(object, time);
43+
normal_vehicle_tracker_.measure(object, time);
4644

4745
return true;
4846
}

‎perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp

+1-4
Original file line numberDiff line numberDiff line change
@@ -52,9 +52,7 @@ bool PassThroughTracker::predict(const rclcpp::Time & time)
5252
return true;
5353
}
5454

55-
bool PassThroughTracker::measure(
56-
const types::DynamicObject & object, const rclcpp::Time & time,
57-
const geometry_msgs::msg::Transform & self_transform)
55+
bool PassThroughTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
5856
{
5957
prev_observed_object_ = object_;
6058
object_ = object;
@@ -68,7 +66,6 @@ bool PassThroughTracker::measure(
6866
}
6967
last_update_time_ = time;
7068

71-
(void)self_transform; // currently do not use self vehicle position
7269
return true;
7370
}
7471

‎perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp

+3-4
Original file line numberDiff line numberDiff line change
@@ -34,11 +34,10 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time)
3434
}
3535

3636
bool PedestrianAndBicycleTracker::measure(
37-
const types::DynamicObject & object, const rclcpp::Time & time,
38-
const geometry_msgs::msg::Transform & self_transform)
37+
const types::DynamicObject & object, const rclcpp::Time & time)
3938
{
40-
pedestrian_tracker_.measure(object, time, self_transform);
41-
bicycle_tracker_.measure(object, time, self_transform);
39+
pedestrian_tracker_.measure(object, time);
40+
bicycle_tracker_.measure(object, time);
4241

4342
return true;
4443
}

‎perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp

+8-19
Original file line numberDiff line numberDiff line change
@@ -116,13 +116,6 @@ bool PedestrianTracker::predict(const rclcpp::Time & time)
116116
return motion_model_.predictState(time);
117117
}
118118

119-
types::DynamicObject PedestrianTracker::getUpdatingObject(
120-
const types::DynamicObject & object,
121-
const geometry_msgs::msg::Transform & /*self_transform*/) const
122-
{
123-
return object;
124-
}
125-
126119
bool PedestrianTracker::measureWithPose(const types::DynamicObject & object)
127120
{
128121
// update motion model
@@ -170,16 +163,14 @@ bool PedestrianTracker::measureWithShape(const types::DynamicObject & object)
170163
return false;
171164
}
172165

166+
// update shape type
167+
object_.shape.type = object.shape.type;
168+
173169
return true;
174170
}
175171

176-
bool PedestrianTracker::measure(
177-
const types::DynamicObject & object, const rclcpp::Time & time,
178-
const geometry_msgs::msg::Transform & self_transform)
172+
bool PedestrianTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
179173
{
180-
// keep the latest input object
181-
object_ = object;
182-
183174
// check time gap
184175
const double dt = motion_model_.getDeltaTime(time);
185176
if (0.01 /*10msec*/ < dt) {
@@ -191,24 +182,22 @@ bool PedestrianTracker::measure(
191182
}
192183

193184
// update object
194-
const types::DynamicObject updating_object = getUpdatingObject(object, self_transform);
195-
measureWithPose(updating_object);
196-
measureWithShape(updating_object);
185+
measureWithPose(object);
186+
measureWithShape(object);
197187

198-
(void)self_transform; // currently do not use self vehicle position
199188
return true;
200189
}
201190

202191
bool PedestrianTracker::getTrackedObject(
203192
const rclcpp::Time & time, types::DynamicObject & object) const
204193
{
205194
object = object_;
195+
196+
// predict from motion model
206197
auto & pose = object.pose;
207198
auto & pose_cov = object.pose_covariance;
208199
auto & twist = object.twist;
209200
auto & twist_cov = object.twist_covariance;
210-
211-
// predict from motion model
212201
if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) {
213202
RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state.");
214203
return false;

‎perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,7 @@ void Tracker::initializeExistenceProbabilities(
7979

8080
bool Tracker::updateWithMeasurement(
8181
const types::DynamicObject & object, const rclcpp::Time & measurement_time,
82-
const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info)
82+
const types::InputChannel & channel_info)
8383
{
8484
// Update existence probability
8585
{
@@ -120,7 +120,7 @@ bool Tracker::updateWithMeasurement(
120120
}
121121

122122
// Update object
123-
measure(object, measurement_time, self_transform);
123+
measure(object, measurement_time);
124124

125125
return true;
126126
}

0 commit comments

Comments
 (0)