564{
565 static constexpr mot::Visitor make_track_visitor{
566 [](const mot::CtrvDetection & d, const mot::Uuid & u) {
567 return Track{mot::make_track<mot::CtrvTrack>(d, u)};
568 },
569 [](const mot::CtraDetection & d, const mot::Uuid & u) {
570 return Track{mot::make_track<mot::CtraTrack>(d, u)};
571 },
572 [](const auto &) {
573
574 throw std::runtime_error("cannot make track from given detection");
575 },
576 };
577
579 RCLCPP_DEBUG(
580 get_logger(), "List of tracks is empty. Converting detections to tentative tracks");
581
582
583
584 const auto clusters{mot::cluster_detections(
detections_, 0.75)};
585 for (const auto & cluster : clusters) {
586 const auto detection{std::cbegin(cluster.get_detections())->second};
587 const auto uuid_str{mot::get_uuid(detection).value()};
588
589
590 const mot::Uuid new_uuid{
592 uuid_str.substr(std::size(uuid_str) - 4, 4)};
594 std::visit(make_track_visitor, detection, std::variant<mot::Uuid>(new_uuid)));
595 }
596
597 track_list_pub_->publish(carma_cooperative_perception_interfaces::msg::TrackList{});
598
601 return;
602 }
603
604 const units::time::second_t current_time{this->now().seconds()};
605
607
609 auto scores{
610 mot::score_tracks_and_detections(predicted_tracks,
detections_, SemanticDistance2dScore{})};
611
612
613
614 mot::prune_track_and_detection_scores_if(scores, [](const auto & score) { return score > 1.0; });
615
616 const auto associations{
617 mot::associate_detections_to_tracks(scores, mot::gnn_association_visitor)};
618
620
621 std::unordered_map<mot::Uuid, Detection> detection_map;
623 detection_map[mot::get_uuid(detection)] = detection;
624 }
625
626 const mot::HasAssociation has_association{associations};
628 if (has_association(track)) {
629 const auto detection_uuids{associations.at(get_uuid(track))};
630 const auto first_detection{detection_map[detection_uuids.at(0)]};
631 const auto fused_track{
632 std::visit(mot::covariance_intersection_visitor, track, first_detection)};
634 }
635 }
636
637
638
639 std::vector<Detection> unassociated_detections;
640 for (const auto & [uuid, detection] : detection_map) {
641 if (!has_association(detection)) {
642 unassociated_detections.push_back(detection);
643 }
644 }
645
646
647
648
649 auto remove_start{std::remove_if(
650 std::begin(unassociated_detections), std::end(unassociated_detections),
651 [&scores](const auto & detection) {
652 const auto uuid{mot::get_uuid(detection)};
653 auto min_score{std::numeric_limits<float>::infinity()};
654 for (const auto & [uuid_pair, score] : scores) {
655 if (uuid_pair.second == uuid) {
656 min_score = std::min(min_score, score);
657 }
658 }
659
660
661
662 return min_score < 1.0;
663 })};
664
665 unassociated_detections.erase(remove_start, std::end(unassociated_detections));
666
667
668
669 const auto clusters{mot::cluster_detections(unassociated_detections, 0.75, MetricSe2{})};
670 for (const auto & cluster : clusters) {
671 const auto detection{std::cbegin(cluster.get_detections())->second};
672 const auto uuid_str{mot::get_uuid(detection).value()};
673
674
675 const mot::Uuid new_uuid{
677 uuid_str.substr(std::size(uuid_str) - 4, 4)};
679 std::visit(make_track_visitor, detection, std::variant<mot::Uuid>(new_uuid)));
680 }
681
682 carma_cooperative_perception_interfaces::msg::TrackList track_list;
683 for (
const auto & track :
track_manager_.get_confirmed_tracks()) {
684 track_list.tracks.push_back(
to_ros_msg(track));
685 }
686
688
691}
rclcpp_lifecycle::LifecyclePublisher< carma_cooperative_perception_interfaces::msg::TrackList >::SharedPtr track_list_pub_
std::size_t lifetime_generated_track_count_
multiple_object_tracking::FixedThresholdTrackManager< Track > track_manager_
std::vector< Detection > detections_
std::unordered_map< multiple_object_tracking::Uuid, std::size_t > uuid_index_map_
static auto temporally_align_detections(std::vector< Detection > &detections, units::time::second_t end_time) -> void
auto to_string(const UtmZone &zone) -> std::string
static auto predict_track_states(std::vector< Track > tracks, units::time::second_t end_time)
std::variant< multiple_object_tracking::CtrvTrack, multiple_object_tracking::CtraTrack > Track
static auto to_ros_msg(const mot::CtraTrack &track)