228 {
229 auto& tracked_objects = frame->tracked_objects;
230 tracked_objects.clear();
231 size_t num_objects =
233 base::ObjectPool::Instance().BatchGet(num_objects, &tracked_objects);
234 size_t pos = 0;
235 size_t num_predict = 0;
236 size_t num_delay_output = 0;
237 size_t num_front_critical_reserve = 0;
238 size_t num_blind_trafficcone = 0;
239 auto collect = [&](std::vector<MlfTrackDataPtr>* tracks) {
240 for (auto& track_data : *tracks) {
241
242
243
244
245
246
247
248
249
250
251
253 if (track_data->is_front_critical_track_) {
254 ADEBUG <<
"[DelayOutput] track_id: " << track_data->track_id_
255 << " time is " << std::to_string(frame->timestamp)
256 << " not output";
257 }
258 ++num_delay_output;
259 continue;
260 }
261 track_data->is_reserve_blind_cone_ = false;
262
263 if (!track_data->is_current_state_predicted_) {
265 frame->timestamp, tracked_objects[pos], true)) {
266 AERROR <<
"Tracking failed";
267 continue;
268 }
269 ++pos;
270 ADEBUG <<
"track_id: " << track_data->track_id_
271 << " detetcted, obj-time is "
272 << std::to_string(frame->timestamp) << " and output";
273 continue;
274 } else {
275
278 frame->timestamp, tracked_objects[pos], true)) {
279 AERROR <<
"Tracking failed";
280 continue;
281 }
282 ++pos;
283 continue;
284 } else {
285
286
288 track_data->GetLatestObject().second;
289
290 if (latest_object != nullptr &&
291 track_data->is_front_critical_track_ &&
292 latest_object->output_velocity.head<2>().norm() < 0.01 &&
293 frame->timestamp -
295 ++num_front_critical_reserve;
296 AINFO <<
"track_id: " << track_data->track_id_
297 << " missed, obj-time is "
298 << std::to_string(track_data->GetLatestObject().first)
299 << " and predict output";
301 frame->timestamp, tracked_objects[pos], false)) {
302 AERROR <<
"Tracking failed";
303 continue;
304 }
305 ++pos;
306 continue;
309 frame->lidar2novatel_extrinsics)) {
310 ++num_blind_trafficcone;
312 frame->timestamp, tracked_objects[pos], false)) {
313 AERROR <<
"Tracking failed";
314 continue;
315 }
316 track_data->is_reserve_blind_cone_ = true;
317 ++pos;
318 continue;
319 } else {
320
321 ++num_predict;
322 continue;
323 }
324 }
325 }
326 }
327 };
330 AINFO <<
"MlfEngine, num_predict: " << num_predict
331 << " num delay_output: " << num_delay_output
332 << " num front_critical: " << num_front_critical_reserve
333 << " num blind trafficcone: " << num_blind_trafficcone
334 << " num_objects: " << num_objects;
335 if (num_predict > num_objects) {
336 AERROR <<
"num_predict > num_objects";
337 return;
338 }
339 tracked_objects.resize(num_objects - num_predict - num_delay_output);
340}
Eigen::Vector3d global_to_local_offset_
bool output_predict_objects_
std::shared_ptr< const TrackedObject > TrackedObjectConstPtr
bool JudgeBlindTrafficCone(const MlfTrackDataConstPtr &track_data, double frame_timestamp, const Eigen::Vector3d &local_to_global_offset, Eigen::Affine3d &lidar2world_pose, Eigen::Affine3d &lidar2novatel_pose)