Target association by id.
The radar driver already has the id of the target, so we choose to trust it and do this part of the match first. We match the detection targets in current frame with the targets has been tracked in the past.
43 {
44 size_t num_track = radar_tracks.size();
45 const auto &objects = radar_frame.objects;
46 double object_timestamp = radar_frame.timestamp;
47 size_t num_obj = objects.size();
48 if (num_track == 0 || num_obj == 0) {
49 unassigned_tracks->resize(num_track);
50 unassigned_objects->resize(num_obj);
51 std::iota(unassigned_tracks->begin(), unassigned_tracks->end(), 0);
52 std::iota(unassigned_objects->begin(), unassigned_objects->end(), 0);
53 return;
54 }
55 std::vector<bool> track_used(num_track, false);
56 std::vector<bool> object_used(num_obj, false);
57 for (size_t i = 0; i < num_track; ++i) {
58 const auto &track_object = radar_tracks[i]->GetObsRadar();
59 double track_timestamp = radar_tracks[i]->GetTimestamp();
60 if (track_object.get() == nullptr) {
61 AERROR <<
"track_object is not available";
62 continue;
63 }
64 int track_object_track_id = track_object->track_id;
65 for (size_t j = 0; j < num_obj; ++j) {
66 int object_track_id = objects[j]->track_id;
67 if (track_object_track_id == object_track_id &&
69 object_timestamp)) {
70 assignments->push_back(std::pair<size_t, size_t>(i, j));
71 track_used[i] = true;
72 object_used[j] = true;
73 }
74 }
75 }
76 for (size_t i = 0; i < track_used.size(); ++i) {
77 if (!track_used[i]) {
78 unassigned_tracks->push_back(i);
79 }
80 }
81 for (size_t i = 0; i < object_used.size(); ++i) {
82 if (!object_used[i]) {
83 unassigned_objects->push_back(i);
84 }
85 }
86}
virtual bool RefinedTrack(const base::ObjectPtr &track_object, double track_timestamp, const base::ObjectPtr &radar_object, double radar_timestamp)
After the IDMatch, then track targets based on their features.