Branch data Line data Source code
1 : : // Copyright 2021 Apex.AI, Inc.
2 : : //
3 : : // Licensed under the Apache License, Version 2.0 (the "License");
4 : : // you may not use this file except in compliance with the License.
5 : : // You may obtain a copy of the License at
6 : : //
7 : : // http://www.apache.org/licenses/LICENSE-2.0
8 : : //
9 : : // Unless required by applicable law or agreed to in writing, software
10 : : // distributed under the License is distributed on an "AS IS" BASIS,
11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 : : // See the License for the specific language governing permissions and
13 : : // limitations under the License.
14 : : //
15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : :
17 : : #include <tracking/detected_object_associator.hpp>
18 : :
19 : : #include <common/types.hpp>
20 : : #include <geometry/common_2d.hpp>
21 : : #include <helper_functions/mahalanobis_distance.hpp>
22 : :
23 : : #include <algorithm>
24 : : #include <limits>
25 : : #include <vector>
26 : :
27 : : namespace autoware
28 : : {
29 : : namespace perception
30 : : {
31 : : namespace tracking
32 : : {
33 : :
34 : : using autoware_auto_perception_msgs::msg::DetectedObjects;
35 : : using autoware::common::state_vector::variable::X;
36 : : using autoware::common::state_vector::variable::Y;
37 : : using autoware::common::types::float32_t;
38 : :
39 : : constexpr std::size_t AssociatorResult::UNASSIGNED;
40 : :
41 : 0 : DataAssociationConfig::DataAssociationConfig(
42 : : const float32_t max_distance,
43 : : const float32_t max_area_ratio,
44 : 0 : const bool consider_edge_for_big_detections)
45 : 0 : : m_max_distance(max_distance), m_max_distance_squared(max_distance * max_distance),
46 : 0 : m_max_area_ratio(max_area_ratio), m_max_area_ratio_inv(1.F / max_area_ratio),
47 : 0 : m_consider_edge_for_big_detections(consider_edge_for_big_detections) {}
48 : :
49 : 0 : DetectedObjectAssociator::DetectedObjectAssociator(const DataAssociationConfig & association_cfg)
50 : 0 : : m_association_cfg(association_cfg) {}
51 : :
52 : 0 : Associations DetectedObjectAssociator::assign(
53 : : const DetectedObjects & detections, const TrackedObjects & tracks)
54 : : {
55 : 0 : if (tracks.frame_id != detections.header.frame_id) {
56 : : throw std::runtime_error(
57 : 0 : "Cannot associate tracks with detections - they are in different frames");
58 : : }
59 : 0 : reset();
60 : 0 : m_track_associations = Associations(tracks.objects.size(), {Matched::kNothing, 0UL});
61 : 0 : m_num_detections = detections.objects.size();
62 : 0 : m_num_tracks = tracks.objects.size();
63 : 0 : m_are_tracks_rows = (m_num_tracks <= m_num_detections);
64 : 0 : if (m_are_tracks_rows) {
65 : 0 : m_assigner.set_size(
66 : 0 : static_cast<assigner_idx_t>(tracks.objects.size()),
67 : 0 : static_cast<assigner_idx_t>(detections.objects.size()));
68 : : } else {
69 : 0 : m_assigner.set_size(
70 : 0 : static_cast<assigner_idx_t>(detections.objects.size()),
71 : 0 : static_cast<assigner_idx_t>(tracks.objects.size()));
72 : : }
73 : 0 : compute_weights(detections, tracks);
74 : : // TODO(gowtham.ranganathan): Revisit this after #979 since till then assigner will always
75 : : // return true
76 : 0 : (void)m_assigner.assign();
77 : :
78 : 0 : return extract_result();
79 : : }
80 : :
81 : 0 : void DetectedObjectAssociator::reset()
82 : : {
83 : 0 : m_assigner.reset();
84 : :
85 : 0 : m_num_tracks = 0U;
86 : 0 : m_num_detections = 0U;
87 : :
88 : 0 : m_had_errors = false;
89 : 0 : }
90 : :
91 : 0 : void DetectedObjectAssociator::compute_weights(
92 : : const DetectedObjects & detections, const TrackedObjects & tracks)
93 : : {
94 : 0 : for (size_t detection_index = 0U; detection_index < detections.objects.size();
95 : : ++detection_index)
96 : : {
97 : 0 : for (size_t track_index = 0U; track_index < tracks.objects.size(); ++track_index) {
98 : 0 : const auto & track = tracks.objects[track_index];
99 : 0 : const auto & detection = detections.objects[detection_index];
100 : :
101 : : try {
102 : 0 : if (consider_associating(detection, track)) {
103 : 0 : Eigen::Matrix<float32_t, NUM_OBJ_POSE_DIM, 1> sample;
104 : 0 : sample(0, 0) =
105 : 0 : static_cast<float32_t>(detection.kinematics.pose_with_covariance.pose.position.x);
106 : 0 : sample(1, 0) =
107 : 0 : static_cast<float32_t>(detection.kinematics.pose_with_covariance.pose.position.y);
108 : :
109 : 0 : Eigen::Matrix<float32_t, NUM_OBJ_POSE_DIM, 1> mean{track.centroid().cast<float32_t>()};
110 : :
111 : : Eigen::Matrix<float32_t, NUM_OBJ_POSE_DIM,
112 : 0 : NUM_OBJ_POSE_DIM> cov = track.position_covariance().cast<float32_t>();
113 : :
114 : 0 : const auto dist = autoware::common::helper_functions::calculate_mahalanobis_distance(
115 : : sample, mean, cov);
116 : :
117 : 0 : set_weight(dist, detection_index, track_index);
118 : : }
119 : 0 : } catch (const std::runtime_error & e) {
120 : 0 : m_had_errors = true;
121 : 0 : } catch (const std::domain_error & e) {
122 : 0 : m_had_errors = true;
123 : : }
124 : : }
125 : : }
126 : 0 : }
127 : :
128 : 0 : bool DetectedObjectAssociator::consider_associating(
129 : : const autoware_auto_perception_msgs::msg::DetectedObject & detection,
130 : : const TrackedObject & track) const
131 : : {
132 : 0 : const auto get_shortest_edge_size_squared = [&]() -> float32_t {
133 : 0 : float32_t retval = std::numeric_limits<float32_t>::max();
134 : 0 : for (auto current = detection.shape.polygon.points.begin();
135 : 0 : current != detection.shape.polygon.points.end(); ++current)
136 : : {
137 : : auto next = common::geometry::details::circular_next(
138 : 0 : detection.shape.polygon.points.begin(), detection.shape.polygon.points.end(), current);
139 : 0 : retval = std::min(retval, common::geometry::squared_distance_2d(*current, *next));
140 : : }
141 : 0 : return retval;
142 : 0 : };
143 : :
144 : 0 : const float32_t det_area = common::geometry::area_checked_2d(
145 : 0 : detection.shape.polygon.points.begin(), detection.shape.polygon.points.end());
146 : :
147 : : // TODO(gowtham.ranganathan): Add support for articulated objects
148 : 0 : const float32_t track_area = common::geometry::area_checked_2d(
149 : 0 : track.shape().polygon.points.begin(), track.shape().polygon.points.end());
150 : : static constexpr float32_t kAreaEps = 1e-3F;
151 : :
152 : 0 : if (common::helper_functions::comparisons::abs_eq_zero(det_area, kAreaEps) ||
153 : 0 : common::helper_functions::comparisons::abs_eq_zero(track_area, kAreaEps))
154 : : {
155 : 0 : throw std::runtime_error("Detection or track area is zero");
156 : : }
157 : :
158 : 0 : const float32_t area_ratio = det_area / track_area;
159 : :
160 : 0 : geometry_msgs::msg::Point track_centroid{};
161 : 0 : track_centroid.x = track.centroid().x();
162 : 0 : track_centroid.y = track.centroid().y();
163 : :
164 : 0 : const auto compute_distance_threshold = [&get_shortest_edge_size_squared, this]() -> float32_t {
165 : 0 : if (m_association_cfg.consider_edge_for_big_detections()) {
166 : : return std::max(
167 : 0 : m_association_cfg.get_max_distance_squared(),
168 : 0 : get_shortest_edge_size_squared());
169 : : } else {
170 : 0 : return m_association_cfg.get_max_distance_squared();
171 : : }
172 : 0 : };
173 : :
174 : 0 : if (common::geometry::squared_distance_2d(
175 : 0 : detection.kinematics.pose_with_covariance.pose.position,
176 : 0 : track_centroid) > compute_distance_threshold())
177 : : {
178 : 0 : return false;
179 : : }
180 : :
181 : 0 : if (area_ratio < m_association_cfg.get_max_area_ratio()) {
182 : 0 : if (area_ratio > m_association_cfg.get_max_area_ratio_inv()) {
183 : 0 : return true;
184 : : }
185 : : }
186 : 0 : return false;
187 : : }
188 : :
189 : 0 : void DetectedObjectAssociator::set_weight(
190 : : const float32_t weight,
191 : : const size_t detection_index, const size_t track_index)
192 : : {
193 : 0 : if (m_are_tracks_rows) {
194 : 0 : m_assigner.set_weight(
195 : : weight, static_cast<assigner_idx_t>(track_index),
196 : : static_cast<assigner_idx_t>(detection_index));
197 : : } else {
198 : 0 : m_assigner.set_weight(
199 : : weight, static_cast<assigner_idx_t>(detection_index),
200 : : static_cast<assigner_idx_t>(track_index));
201 : : }
202 : 0 : }
203 : :
204 : 0 : Associations DetectedObjectAssociator::extract_result()
205 : : {
206 : 0 : auto object_associations = Associations(m_num_detections, {Matched::kNothing, 0UL});
207 : 0 : if (m_are_tracks_rows) {
208 : 0 : for (size_t track_index = 0U; track_index < m_num_tracks; track_index++) {
209 : : const auto detection_index =
210 : 0 : static_cast<size_t>(m_assigner.get_assignment(static_cast<assigner_idx_t>(track_index)));
211 : 0 : const auto track_has_detection_assigned = (detection_index != Assigner::UNASSIGNED);
212 : 0 : if (!track_has_detection_assigned) {continue;}
213 : : const auto detection_has_no_assignment_yet =
214 : 0 : (object_associations[detection_index].matched == Matched::kNothing);
215 : 0 : if (detection_has_no_assignment_yet) {
216 : 0 : object_associations[detection_index] = {Matched::kExistingTrack, track_index};
217 : 0 : m_track_associations[track_index] = {Matched::kOtherDetection, detection_index};
218 : : }
219 : : }
220 : : } else {
221 : 0 : for (size_t detection_index = 0U; detection_index < m_num_detections; detection_index++) {
222 : : const auto track_index = static_cast<size_t>(
223 : 0 : m_assigner.get_assignment(static_cast<assigner_idx_t>(detection_index)));
224 : 0 : const auto track_got_assigned = (track_index != Assigner::UNASSIGNED);
225 : : const auto detection_has_no_assignment =
226 : 0 : (object_associations[detection_index].matched == Matched::kNothing);
227 : 0 : if (track_got_assigned && detection_has_no_assignment) {
228 : 0 : object_associations[detection_index] = {Matched::kExistingTrack, track_index};
229 : 0 : m_track_associations[track_index] = {Matched::kOtherDetection, detection_index};
230 : : }
231 : : }
232 : : }
233 : :
234 : 0 : return object_associations;
235 : : }
236 : :
237 : : } // namespace tracking
238 : : } // namespace perception
239 : : } // namespace autoware
|
