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