Apollo 11.0
自动驾驶开放平台
mlf_track_data.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2018 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
19#include "cyber/common/log.h"
21
22namespace apollo {
23namespace perception {
24namespace lidar {
25
26const double MlfTrackData::kMaxHistoryTime = 2.0;
27
44
45void MlfTrackData::Reset(TrackedObjectPtr obj, int track_id) {
46 Reset();
47 track_id_ = track_id;
49}
50
52 // double timestamp = obj->object_ptr->latest_tracked_time;
53 double timestamp = obj->timestamp;
54 if (history_objects_.find(timestamp) == history_objects_.end()) {
55 auto pair = std::make_pair(timestamp, obj);
56 history_objects_.insert(pair);
57 sensor_history_objects_[obj->sensor_info.name].insert(pair);
58 age_++;
59 if (age_ == 1) { // the first timestamp
60 if (obj->is_fake) {
61 AERROR << "obj is fake";
62 return;
63 }
64 latest_visible_time_ = timestamp;
65 first_tracked_time_ = timestamp;
66 }
67 obj->track_id = track_id_;
68 obj->tracking_time = timestamp - first_tracked_time_;
69 duration_ = obj->tracking_time;
70 if (obj->is_fake) {
73 } else {
77 latest_visible_time_ = timestamp;
78 }
80 } else {
81 AINFO << "Push object timestamp " << timestamp << " from sensor "
82 << obj->sensor_info.name << " already exist in track, ignore push.";
83 }
84}
85
87 double timestamp = obj->object_ptr->latest_tracked_time;
88 if (cached_objects_.find(timestamp) == cached_objects_.end()) {
89 cached_objects_.insert(std::make_pair(timestamp, obj));
90 latest_cached_time_ = timestamp;
91 } else {
92 AINFO << "Push object timestamp " << timestamp << " from sensor "
93 << obj->sensor_info.name << " already exist in cache, ignore push.";
94 }
95}
96
97bool MlfTrackData::ToObject(const Eigen::Vector3d& local_to_global_offset,
98 double timestamp, base::ObjectPtr object, bool update_time) const {
99 if (history_objects_.empty()) {
100 return false;
101 }
102 auto latest_iter = history_objects_.rbegin();
103 const double latest_time = latest_iter->first;
104 const auto& latest_object = latest_iter->second;
105 latest_object->ToObject(object);
106 // predict object
107 double time_diff = timestamp - latest_time;
108 double offset_x = time_diff * object->velocity(0) + local_to_global_offset(0);
109 double offset_y = time_diff * object->velocity(1) + local_to_global_offset(1);
110 double offset_z = time_diff * object->velocity(2) + local_to_global_offset(2);
111 // a). update polygon
112 for (auto& pt : object->polygon) {
113 pt.x += offset_x;
114 pt.y += offset_y;
115 pt.z += offset_z;
116 }
117
118 // b). update center
119 object->center(0) += offset_x;
120 object->center(1) += offset_y;
121 object->center(2) += offset_z;
122 // c). update anchor point
123 object->anchor_point(0) += offset_x;
124 object->anchor_point(1) += offset_y;
125 object->anchor_point(2) += offset_z;
126 if (update_time) {
127 // d). update tracking timestamp
128 object->tracking_time += time_diff;
129 // e). update latest track timestamp
130 object->latest_tracked_time += time_diff;
131 }
132 // f). update cloud world in lidar supplement
133 for (auto& pt : object->lidar_supplement.cloud_world) {
134 pt.x += offset_x;
135 pt.y += offset_y;
136 pt.z += offset_z;
137 }
138 return true;
139}
140
141void MlfTrackData::PredictState(double timestamp) const {
142 if (history_objects_.empty()) {
143 return;
144 }
145 auto latest_iter = history_objects_.rbegin();
146 const double latest_time = latest_iter->first;
147 const auto& latest_object = latest_iter->second;
148 double time_diff = timestamp - latest_time;
149
150 const Eigen::Vector3d& latest_anchor_point =
151 latest_object->belief_anchor_point;
152 const Eigen::Vector3d& latest_velocity = latest_object->output_velocity;
153
154 predict_.state.resize(6);
155 predict_.state(0) = static_cast<float>(latest_anchor_point(0) +
156 latest_velocity(0) * time_diff);
157 predict_.state(1) = static_cast<float>(latest_anchor_point(1) +
158 latest_velocity(1) * time_diff);
159 predict_.state(2) = static_cast<float>(latest_anchor_point(2) +
160 latest_velocity(2) * time_diff);
161 predict_.state(3) = static_cast<float>(latest_velocity(0));
162 predict_.state(4) = static_cast<float>(latest_velocity(1));
163 predict_.state(5) = static_cast<float>(latest_velocity(2));
164
165 predict_.timestamp = timestamp;
166 // TODO(.): predict cloud and polygon if needed in future.
167}
168
170 std::vector<TrackedObjectPtr>* objects) {
171 objects->clear();
172 auto iter = cached_objects_.begin();
173 while (iter != cached_objects_.end()) {
174 const auto& timestamp = iter->first;
175 if (timestamp <= latest_visible_time_) {
176 cached_objects_.erase(iter++);
177 } else if (timestamp <= latest_cached_time_) {
178 objects->push_back(iter->second);
179 cached_objects_.erase(iter++);
180 } else {
181 break;
182 }
183 }
184}
185
186void RemoveStaleDataFromMap(double timestamp,
187 std::map<double, TrackedObjectPtr>* data) {
188 auto iter = data->begin();
189 while (iter != data->end()) {
190 if (iter->first < timestamp) {
191 data->erase(iter++);
192 } else {
193 break;
194 }
195 }
196}
197
198void MlfTrackData::RemoveStaleHistory(double timestamp) {
200 for (auto& map : sensor_history_objects_) {
201 RemoveStaleDataFromMap(timestamp, &map.second);
202 }
203}
204
206 std::vector<TrackedObjectPtr>* objects) {
207 objects->clear();
208 size_t i = 0;
209 for (auto obj_it = history_objects_.rbegin();
210 obj_it != history_objects_.rend(); ++obj_it) {
211 if (i < k) {
212 objects->push_back(obj_it->second);
213 ++i;
214 } else {
215 break;
216 }
217 }
218}
219
221 std::vector<TrackedObjectConstPtr>* objects) const {
222 objects->clear();
223 size_t i = 0;
224 for (auto obj_it = history_objects_.rbegin();
225 obj_it != history_objects_.rend(); ++obj_it) {
226 if (i < k) {
227 objects->push_back(obj_it->second);
228 ++i;
229 } else {
230 break;
231 }
232 }
233}
234
236 std::vector<TrackedObjectPtr>* objects) {
237 objects->clear();
238 for (auto obj_it = history_objects_.rbegin();
239 obj_it != history_objects_.rend(); ++obj_it) {
240 if (obj_it->second->timestamp >= time) {
241 objects->push_back(obj_it->second);
242 } else {
243 break;
244 }
245 }
246}
247
249 std::vector<TrackedObjectConstPtr>* objects) const {
250 objects->clear();
251 for (auto obj_it = history_objects_.rbegin();
252 obj_it != history_objects_.rend(); ++obj_it) {
253 if (obj_it->second->timestamp >= time) {
254 objects->push_back(obj_it->second);
255 } else {
256 break;
257 }
258 }
259}
260
262 std::vector<TrackedObjectConstPtr>* objects) {
263 for (auto obj_it = history_objects_.begin();
264 obj_it != history_objects_.end(); ++obj_it) {
265 if (obj_it->second->timestamp >= time) {
266 objects->push_back(obj_it->second);
267 }
268 }
269}
270
272 std::vector<TrackedObjectConstPtr>* objects) const {
273 for (auto obj_it = history_objects_.begin();
274 obj_it != history_objects_.end(); ++obj_it) {
275 if (obj_it->second->timestamp >= time) {
276 objects->push_back(obj_it->second);
277 }
278 }
279}
280
282 if (age_ == 0) {
284 obj->object_ptr->lidar_supplement.is_clustered ? 0.0 : 1.0;
285 } else {
287 obj->object_ptr->lidar_supplement.is_background = false;
288 obj->is_background = false;
289 }
290 if (!obj->object_ptr->lidar_supplement.is_clustered) {
292 } else {
294 }
295 }
296}
297
299 return (foreground_track_prob_ > 0.5);
300}
301
303 bool is_background_movable =
304 (obj->object_ptr->lidar_supplement.dynamic_state ==
306 (obj->object_ptr->lidar_supplement.semantic_type ==
308
309 return is_background_movable;
310}
311
312} // namespace lidar
313} // namespace perception
314} // namespace apollo
bool IsBackgroundMovable(TrackedObjectPtr obj)
void GetObjectsInIntervalByOrder(double time, std::vector< TrackedObjectConstPtr > *objects)
void PushTrackedObjectToTrack(TrackedObjectPtr obj)
Push tracked object to track
void RemoveStaleHistory(double timestamp)
Remove stale history data
void GetLatestKObjects(size_t k, std::vector< TrackedObjectPtr > *objects)
void GetObjectsInInterval(double time, std::vector< TrackedObjectPtr > *objects)
void UpdateTrackableState(TrackedObjectPtr obj)
void PredictState(double timestamp) const
Predict state
std::map< std::string, TimedObjects > sensor_history_objects_
bool ToObject(const Eigen::Vector3d &local_to_global_offset, double timestamp, base::ObjectPtr object, bool update_time=true) const
Convert tracked object to base object
void PushTrackedObjectToCache(TrackedObjectPtr obj)
Push tracked object to cache
void GetAndCleanCachedObjectsInTimeInterval(std::vector< TrackedObjectPtr > *objects)
Get and clean cached objects in time interval
std::map< double, TrackedObjectPtr > history_objects_
Definition track_data.h:134
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
std::shared_ptr< Object > ObjectPtr
Definition object.h:127
void RemoveStaleDataFromMap(double timestamp, std::map< double, TrackedObjectPtr > *data)
std::shared_ptr< TrackedObject > TrackedObjectPtr
class register implement
Definition arena_queue.h:37