18246927847

Committed 04 Oct 2025 04:44PM UTC coverage: 71.826% (+0.6%) from 71.188%

Build # 18246927847

Build Type

push

github

Committed by

paulmthompson

Commit Message

refactor out media producer consumer pipeline

Run Details

0 of 120 new or added lines in 2 files covered. (0.0%)

646 existing lines in 14 files now uncovered.

48895 of 68074 relevant lines covered (71.83%)

1193.51 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

77.61

/src/StateEstimation/Assignment/HungarianAssigner.cpp

#include "HungarianAssigner.hpp"

#include "Assignment/hungarian.hpp"// Correctly include your Munkres implementation

#include <cmath>
#include <vector>

namespace StateEstimation {

namespace {
// Helper function to calculate the squared Mahalanobis distance.
double calculateMahalanobisDistanceSq(Eigen::VectorXd const & observation,
                                      Eigen::VectorXd const & predicted_mean,
                                      Eigen::MatrixXd const & predicted_covariance,
                                      Eigen::MatrixXd const & H,
                                      Eigen::MatrixXd const & R) {
    Eigen::VectorXd innovation = observation - (H * predicted_mean);
    Eigen::MatrixXd innovation_covariance = H * predicted_covariance * H.transpose() + R;
    return innovation.transpose() * innovation_covariance.inverse() * innovation;
}
}// namespace

HungarianAssigner::HungarianAssigner(double max_assignment_distance,
                                     Eigen::MatrixXd const & measurement_matrix,
                                     Eigen::MatrixXd const & measurement_noise_covariance,
                                     std::string feature_name)
    : max_assignment_distance_(max_assignment_distance),
      H_(measurement_matrix),
      R_(measurement_noise_covariance),
      feature_name_(std::move(feature_name)) {}

Assignment HungarianAssigner::solve(
        std::vector<Prediction> const & predictions,
        std::vector<Observation> const & observations,
        std::map<EntityId, FeatureCache> const & feature_cache) {

    if (predictions.empty() || observations.empty()) {
        return {};
    }

    // 1. Build the cost matrix with integer costs for the Munkres library.
    int const cost_scaling_factor = 1000;
    int const max_cost = static_cast<int>(max_assignment_distance_ * cost_scaling_factor);
    std::vector<std::vector<int>> cost_matrix(observations.size(), std::vector<int>(predictions.size()));
    // Track per-observation best candidate for fallback when solver yields no assignment (single prediction case)
    std::vector<int> best_col_for_row(observations.size(), -1);
    std::vector<int> best_cost_for_row(observations.size(), std::numeric_limits<int>::max());

    for (size_t i = 0; i < observations.size(); ++i) {
        auto cache_it = feature_cache.find(observations[i].entity_id);
        if (cache_it == feature_cache.end()) {
            throw std::runtime_error("Feature cache not found for observation.");
        }

        auto feature_it = cache_it->second.find(feature_name_);
        if (feature_it == cache_it->second.end()) {
            throw std::runtime_error("Required feature '" + feature_name_ + "' not in cache.");
        }

        auto const & observation_vec = std::any_cast<Eigen::VectorXd const &>(feature_it->second);

        for (size_t j = 0; j < predictions.size(); ++j) {
            double dist_sq = calculateMahalanobisDistanceSq(
                    observation_vec,
                    predictions[j].filter_state.state_mean,
                    predictions[j].filter_state.state_covariance,
                    H_,
                    R_);

            double distance = std::sqrt(dist_sq);
            int cost = static_cast<int>(distance * cost_scaling_factor);
            // Keep original costs to allow solver; cap at INT_MAX - 1 to avoid overflow
            if (cost >= std::numeric_limits<int>::max()) {
                cost = std::numeric_limits<int>::max() - 1;
            }
            cost_matrix[i][j] = cost;
            if (cost < best_cost_for_row[i]) {
                best_cost_for_row[i] = cost;
                best_col_for_row[i] = static_cast<int>(j);
            }
        }
    }

    // 2. Solve the assignment problem using the Munkres implementation
    std::vector<std::vector<int>> assignment_matrix;
    Munkres::hungarian_with_assignment(cost_matrix, assignment_matrix);

    // 3. Format the results
    Assignment result;
    result.cost_threshold = max_assignment_distance_;
    
    for (size_t i = 0; i < assignment_matrix.size(); ++i) {
        for (size_t j = 0; j < assignment_matrix[i].size(); ++j) {
            // A value of 1 in the assignment matrix indicates a match
            if (assignment_matrix[i][j] == 1) {
                // Ensure the assignment is not an "infinite" cost one
                if (cost_matrix[i][j] < std::numeric_limits<int>::max()) {
                    result.observation_to_prediction[i] = j;
                    // Store the actual cost (convert back from scaled integer)
                    double actual_cost = static_cast<double>(cost_matrix[i][j]) / cost_scaling_factor;
                    result.assignment_costs[i] = actual_cost;
                }
                break;// Move to the next observation row
            }
        }
    }

    // Fallback for single-prediction case: if solver produced no assignment, pick the best observation greedily
    if (result.observation_to_prediction.empty() && predictions.size() == 1 && !observations.empty()) {
        // Select the observation row with the minimal cost
        int best_row = -1;
        int best_cost = std::numeric_limits<int>::max();
        for (size_t i = 0; i < best_cost_for_row.size(); ++i) {
            if (best_cost_for_row[i] < best_cost && best_col_for_row[i] != -1) {
                best_cost = best_cost_for_row[i];
                best_row = static_cast<int>(i);
            }
        }
        if (best_row != -1) {
            result.observation_to_prediction[best_row] = 0;
            result.assignment_costs[best_row] = static_cast<double>(best_cost) / cost_scaling_factor;
        }
    }

    return result;
}

std::unique_ptr<IAssigner> HungarianAssigner::clone() const {
    return std::make_unique<HungarianAssigner>(*this);
}

}// namespace StateEstimation

1	#include "HungarianAssigner.hpp"
2
3	#include "Assignment/hungarian.hpp"// Correctly include your Munkres implementation
4
5	#include <cmath>
6	#include <vector>
7
8	namespace StateEstimation {
9
10	namespace {
11	// Helper function to calculate the squared Mahalanobis distance.
12	double calculateMahalanobisDistanceSq(Eigen::VectorXd const & observation,	10,996✔
13	Eigen::VectorXd const & predicted_mean,
14	Eigen::MatrixXd const & predicted_covariance,
15	Eigen::MatrixXd const & H,
16	Eigen::MatrixXd const & R) {
17	Eigen::VectorXd innovation = observation - (H * predicted_mean);	10,996✔
18	Eigen::MatrixXd innovation_covariance = H * predicted_covariance * H.transpose() + R;	10,996✔
19	return innovation.transpose() * innovation_covariance.inverse() * innovation;	21,992✔
20	}	10,996✔
21	}// namespace
22
23	HungarianAssigner::HungarianAssigner(double max_assignment_distance,	12✔
24	Eigen::MatrixXd const & measurement_matrix,
25	Eigen::MatrixXd const & measurement_noise_covariance,
26	std::string feature_name)	12✔
27	: max_assignment_distance_(max_assignment_distance),	12✔
28	H_(measurement_matrix),	12✔
29	R_(measurement_noise_covariance),	12✔
30	feature_name_(std::move(feature_name)) {}	24✔
31
32	Assignment HungarianAssigner::solve(	4,765✔
33	std::vector<Prediction> const & predictions,
34	std::vector<Observation> const & observations,
35	std::map<EntityId, FeatureCache> const & feature_cache) {
36
37	if (predictions.empty() \|\| observations.empty()) {	4,765✔
38	return {};	×
39	}
40
41	// 1. Build the cost matrix with integer costs for the Munkres library.
42	int const cost_scaling_factor = 1000;	4,765✔
43	int const max_cost = static_cast<int>(max_assignment_distance_ * cost_scaling_factor);	4,765✔
44	std::vector<std::vector<int>> cost_matrix(observations.size(), std::vector<int>(predictions.size()));	23,825✔
45	// Track per-observation best candidate for fallback when solver yields no assignment (single prediction case)
46	std::vector<int> best_col_for_row(observations.size(), -1);	14,295✔
47	std::vector<int> best_cost_for_row(observations.size(), std::numeric_limits<int>::max());	14,295✔
48
49	for (size_t i = 0; i < observations.size(); ++i) {	14,295✔
50	auto cache_it = feature_cache.find(observations[i].entity_id);	9,530✔
51	if (cache_it == feature_cache.end()) {	9,530✔
UNCOV 52	throw std::runtime_error("Feature cache not found for observation.");	×
53	}
54
55	auto feature_it = cache_it->second.find(feature_name_);	9,530✔
56	if (feature_it == cache_it->second.end()) {	9,530✔
UNCOV 57	throw std::runtime_error("Required feature '" + feature_name_ + "' not in cache.");	×
58	}
59
60	auto const & observation_vec = std::any_cast<Eigen::VectorXd const &>(feature_it->second);	9,530✔
61
62	for (size_t j = 0; j < predictions.size(); ++j) {	20,526✔
63	double dist_sq = calculateMahalanobisDistanceSq(	21,992✔
64	observation_vec,
65	predictions[j].filter_state.state_mean,	10,996✔
66	predictions[j].filter_state.state_covariance,	10,996✔
67	H_,	10,996✔
68	R_);	10,996✔
69
70	double distance = std::sqrt(dist_sq);	10,996✔
71	int cost = static_cast<int>(distance * cost_scaling_factor);	10,996✔
72	// Keep original costs to allow solver; cap at INT_MAX - 1 to avoid overflow
73	if (cost >= std::numeric_limits<int>::max()) {	10,996✔
UNCOV 74	cost = std::numeric_limits<int>::max() - 1;	×
75	}
76	cost_matrix[i][j] = cost;	10,996✔
77	if (cost < best_cost_for_row[i]) {	10,996✔
78	best_cost_for_row[i] = cost;	10,175✔
79	best_col_for_row[i] = static_cast<int>(j);	10,175✔
80	}
81	}
82	}
83
84	// 2. Solve the assignment problem using the Munkres implementation
85	std::vector<std::vector<int>> assignment_matrix;	4,765✔
86	Munkres::hungarian_with_assignment(cost_matrix, assignment_matrix);	4,765✔
87
88	// 3. Format the results
89	Assignment result;	4,765✔
90	result.cost_threshold = max_assignment_distance_;	4,765✔
91
92	for (size_t i = 0; i < assignment_matrix.size(); ++i) {	14,295✔
93	for (size_t j = 0; j < assignment_matrix[i].size(); ++j) {	14,295✔
94	// A value of 1 in the assignment matrix indicates a match
95	if (assignment_matrix[i][j] == 1) {	10,263✔
96	// Ensure the assignment is not an "infinite" cost one
97	if (cost_matrix[i][j] < std::numeric_limits<int>::max()) {	5,498✔
98	result.observation_to_prediction[i] = j;	5,498✔
99	// Store the actual cost (convert back from scaled integer)
100	double actual_cost = static_cast<double>(cost_matrix[i][j]) / cost_scaling_factor;	5,498✔
101	result.assignment_costs[i] = actual_cost;	5,498✔
102	}
103	break;// Move to the next observation row	5,498✔
104	}
105	}
106	}
107
108	// Fallback for single-prediction case: if solver produced no assignment, pick the best observation greedily
109	if (result.observation_to_prediction.empty() && predictions.size() == 1 && !observations.empty()) {	4,765✔
110	// Select the observation row with the minimal cost
UNCOV 111	int best_row = -1;	×
UNCOV 112	int best_cost = std::numeric_limits<int>::max();	×
UNCOV 113	for (size_t i = 0; i < best_cost_for_row.size(); ++i) {	×
UNCOV 114	if (best_cost_for_row[i] < best_cost && best_col_for_row[i] != -1) {	×
UNCOV 115	best_cost = best_cost_for_row[i];	×
UNCOV 116	best_row = static_cast<int>(i);	×
117	}
118	}
UNCOV 119	if (best_row != -1) {	×
UNCOV 120	result.observation_to_prediction[best_row] = 0;	×
UNCOV 121	result.assignment_costs[best_row] = static_cast<double>(best_cost) / cost_scaling_factor;	×
122	}
123	}
124
125	return result;	4,765✔
126	}	4,765✔
127
UNCOV 128	std::unique_ptr<IAssigner> HungarianAssigner::clone() const {	×
UNCOV 129	return std::make_unique<HungarianAssigner>(*this);	×
130	}
131
132	}// namespace StateEstimation

paulmthompson / WhiskerToolbox / 18246927847

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous