216 {
217 auto& tracked_objects = frame->tracked_objects;
218 tracked_objects.clear();
219 size_t num_objects =
221 base::ObjectPool::Instance().BatchGet(num_objects, &tracked_objects);
222 size_t pos = 0;
223 size_t num_predict = 0;
224 size_t num_delay_output = 0;
225 size_t num_front_critical_reserve = 0;
226 size_t num_blind_trafficcone = 0;
227 auto collect = [&](std::vector<MlfTrackDataPtr>* tracks) {
228 for (auto& track_data : *tracks) {
229
230
231
232
233
234
235
236
237
238
239
241 if (track_data->is_front_critical_track_) {
242 ADEBUG <<
"[DelayOutput] track_id: " << track_data->track_id_
243 << " time is " << std::to_string(frame->timestamp)
244 << " not output";
245 }
246 ++num_delay_output;
247 continue;
248 }
249 track_data->is_reserve_blind_cone_ = false;
250
251 if (!track_data->is_current_state_predicted_) {
253 frame->timestamp, tracked_objects[pos], true)) {
254 AERROR <<
"Tracking failed";
255 continue;
256 }
257 ++pos;
258 ADEBUG <<
"track_id: " << track_data->track_id_
259 << " detetcted, obj-time is "
260 << std::to_string(frame->timestamp) << " and output";
261 continue;
262 } else {
263
266 frame->timestamp, tracked_objects[pos], true)) {
267 AERROR <<
"Tracking failed";
268 continue;
269 }
270 ++pos;
271 continue;
272 } else {
273
274
276 track_data->GetLatestObject().second;
277
278 if (latest_object != nullptr &&
279 track_data->is_front_critical_track_ &&
280 latest_object->output_velocity.head<2>().norm() < 0.01 &&
281 frame->timestamp -
283 ++num_front_critical_reserve;
284 AINFO <<
"track_id: " << track_data->track_id_
285 << " missed, obj-time is "
286 << std::to_string(track_data->GetLatestObject().first)
287 << " and predict output";
289 frame->timestamp, tracked_objects[pos], false)) {
290 AERROR <<
"Tracking failed";
291 continue;
292 }
293 ++pos;
294 continue;
297 frame->lidar2novatel_extrinsics)) {
298 ++num_blind_trafficcone;
300 frame->timestamp, tracked_objects[pos], false)) {
301 AERROR <<
"Tracking failed";
302 continue;
303 }
304 track_data->is_reserve_blind_cone_ = true;
305 ++pos;
306 continue;
307 } else {
308
309 ++num_predict;
310 continue;
311 }
312 }
313 }
314 }
315 };
318 AINFO <<
"MlfEngine, num_predict: " << num_predict
319 << " num delay_output: " << num_delay_output
320 << " num front_critical: " << num_front_critical_reserve
321 << " num blind trafficcone: " << num_blind_trafficcone
322 << " num_objects: " << num_objects;
323 if (num_predict > num_objects) {
324 AERROR <<
"num_predict > num_objects";
325 return;
326 }
327 tracked_objects.resize(num_objects - num_predict - num_delay_output);
328}
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)