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

97.65

/src/StateEstimation/Kalman/KalmanFilter.test.cpp

#include <catch2/catch_test_macros.hpp>
#include <Eigen/Dense>
#include <iostream>

#include "Kalman/KalmanFilter.hpp"
#include "Tracker.hpp"
#include "Features/IFeatureExtractor.hpp"
#include "Assignment/HungarianAssigner.hpp"
#include "Entity/EntityTypes.hpp"
#include "Entity/EntityGroupManager.hpp"
#include "TimeFrame/TimeFrame.hpp"
#include "IdentityConfidence.hpp"

// --- Test-Specific Mocks and Implementations ---

// A simple Line2D struct for this test, mirroring what CoreGeometry might provide.
struct TestLine2D {
    EntityId id;
    Eigen::Vector2d p1;
    Eigen::Vector2d p2;

    Eigen::Vector2d centroid() const {
        return (p1 + p2) / 2.0;
    }
};

// A concrete feature extractor for TestLine2D centroids.
class LineCentroidExtractor : public StateEstimation::IFeatureExtractor<TestLine2D> {
public:
    Eigen::VectorXd getFilterFeatures(const TestLine2D& data) const override {
        Eigen::Vector2d c = data.centroid();
        Eigen::VectorXd features(2);
        features << c.x(), c.y();
        return features;
    }

    StateEstimation::FeatureCache getAllFeatures(const TestLine2D& data) const override {
        StateEstimation::FeatureCache cache;
        cache[getFilterFeatureName()] = getFilterFeatures(data);
        return cache;
    }

    std::string getFilterFeatureName() const override {
        return "kalman_features";
    }

    StateEstimation::FilterState getInitialState(const TestLine2D& data) const override {
        Eigen::VectorXd initialState(4);
        Eigen::Vector2d centroid = data.centroid();
        initialState << centroid.x(), centroid.y(), 0, 0; // Position from centroid, velocity is zero

        Eigen::MatrixXd p(4, 4);
        p.setIdentity();
        p *= 100.0; // High initial uncertainty

        return {initialState, p};
    }

    std::unique_ptr<IFeatureExtractor<TestLine2D>> clone() const override {
        return std::make_unique<LineCentroidExtractor>(*this);
    }

    StateEstimation::FeatureMetadata getMetadata() const override {
        return StateEstimation::FeatureMetadata::create(
            "kalman_features",
            2,  // 2D measurement
            StateEstimation::FeatureTemporalType::KINEMATIC_2D
        );
    }
};


TEST_CASE("KalmanFilter tracking and smoothing", "[KalmanFilter]") {
    using namespace StateEstimation;

    // 1. --- SETUP ---

    // Define a constant velocity Kalman Filter
    double dt = 1.0;
    Eigen::MatrixXd F(4, 4); // State Transition Matrix
    F << 1, 0, dt, 0,
         0, 1, 0, dt,
         0, 0, 1, 0,
         0, 0, 0, 1;

    Eigen::MatrixXd H(2, 4); // Measurement Matrix
    H << 1, 0, 0, 0,
         0, 1, 0, 0;

    Eigen::MatrixXd Q(4, 4); // Process Noise Covariance
    Q.setIdentity();
    Q *= 0.1;

    Eigen::MatrixXd R(2, 2); // Measurement Noise Covariance
    R.setIdentity();
    R *= 5.0;

    auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);
    auto feature_extractor = std::make_unique<LineCentroidExtractor>();

    // We run in smoothing-only mode, so no assigner is needed.
    Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), nullptr);

    // --- Generate Artificial Data ---
    // New API: vector of tuples (DataType, EntityId, TimeFrameIndex)
    std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;
    
    EntityGroupManager group_manager;
    GroupId group1 = group_manager.createGroup("Group 1");
    GroupId group2 = group_manager.createGroup("Group 2");

    for (int i = 0; i <= 10; ++i) {
        // Line 1 moves from (10,10) to (60,60)
        TestLine2D line1 = { (EntityId)i, {10.0 + i * 5.0, 10.0 + i * 5.0}, {10.0 + i * 5.0, 10.0 + i * 5.0} };
        // Line 2 moves from (100,50) to (50,50)
        TestLine2D line2 = { (EntityId)(i + 100), {100.0 - i * 5.0, 50.0}, {100.0 - i * 5.0, 50.0} };
        
        data_source.emplace_back(line1, (EntityId)i, TimeFrameIndex(i));
        data_source.emplace_back(line2, (EntityId)(i + 100), TimeFrameIndex(i));
    }

    // Ground truth map with TimeFrameIndex keys
    Tracker<TestLine2D>::GroundTruthMap ground_truth;
    ground_truth[TimeFrameIndex(0)] = {{group1, 0}, {group2, 100}};
    ground_truth[TimeFrameIndex(10)] = {{group1, 10}, {group2, 110}};
    
    // Populate the initial groups in EntityGroupManager
    group_manager.addEntityToGroup(group1, 0);
    group_manager.addEntityToGroup(group1, 10);
    group_manager.addEntityToGroup(group2, 100);
    group_manager.addEntityToGroup(group2, 110);


    // 2. --- EXECUTION ---
    SmoothedResults results = tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(10));

    // 3. --- ASSERTIONS ---
    REQUIRE(results.count(group1) == 1);
    REQUIRE(results.count(group2) == 1);

    // We have 11 frames of data (0-10 inclusive) for the forward pass, which get smoothed.
    REQUIRE(results.at(group1).size() == 11);
    REQUIRE(results.at(group2).size() == 11);

    // Check if the smoothed results are reasonable.
    // The smoothed estimate at the midpoint should be close to the true position.
    const auto& smoothed_g1 = results.at(group1);
    const auto& smoothed_g2 = results.at(group2);
    
    // True position at frame 5 for line 1 is (10 + 5*5, 10 + 5*5) = (35, 35)
    REQUIRE(std::abs(smoothed_g1[5].state_mean(0) - 35.0) < 5.0);
    REQUIRE(std::abs(smoothed_g1[5].state_mean(1) - 35.0) < 5.0);
    
    // True position at frame 5 for line 2 is (100 - 5*5, 50) = (75, 50)
    REQUIRE(std::abs(smoothed_g2[5].state_mean(0) - 75.0) < 5.0);
    REQUIRE(std::abs(smoothed_g2[5].state_mean(1) - 50.0) < 5.0);

    // Check velocity estimates. Group 1 should be moving (+5, +5)
    REQUIRE(smoothed_g1[5].state_mean(2) > 4.0); // vx
    REQUIRE(smoothed_g1[5].state_mean(3) > 4.0); // vy

    // Group 2 should be moving (-5, 0)
    REQUIRE(smoothed_g2[5].state_mean(2) < -4.0); // vx
    REQUIRE(std::abs(smoothed_g2[5].state_mean(3)) < 1.0); // vy
}

TEST_CASE("StateEstimation - KalmanFilter - assignment with Hungarian algorithm", "[KalmanFilter][Assignment]") {
    using namespace StateEstimation;

    // 1. --- SETUP ---
    double dt = 1.0;
    Eigen::MatrixXd F(4, 4); F << 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1;
    Eigen::MatrixXd H(2, 4); H << 1, 0, 0, 0, 0, 1, 0, 0;
    Eigen::MatrixXd Q(4, 4); Q.setIdentity(); Q *= 0.1;
    Eigen::MatrixXd R(2, 2); R.setIdentity(); R *= 5.0;

    auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);
    auto feature_extractor = std::make_unique<LineCentroidExtractor>();
    auto assigner = std::make_unique<HungarianAssigner>(100.0, H, R, "kalman_features");

    Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), std::move(assigner));

    // --- Generate Artificial Data ---
    // New API: vector of tuples (DataType, EntityId, TimeFrameIndex)
    std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;
    
    EntityGroupManager group_manager;
    GroupId group1 = group_manager.createGroup("Group 1");
    GroupId group2 = group_manager.createGroup("Group 2");

    // Frame 0: Ground truth
    data_source.emplace_back(TestLine2D{ (EntityId)1, {10, 10}, {10, 10} }, (EntityId)1, TimeFrameIndex(0));
    data_source.emplace_back(TestLine2D{ (EntityId)2, {100, 50}, {100, 50} }, (EntityId)2, TimeFrameIndex(0));
    
    Tracker<TestLine2D>::GroundTruthMap ground_truth;
    ground_truth[TimeFrameIndex(0)] = {{group1, 1}, {group2, 2}};
    
    group_manager.addEntityToGroup(group1, 1);
    group_manager.addEntityToGroup(group2, 2);

    // Frames 1-4: Ungrouped data that should be assigned
    for (int i = 1; i <= 4; ++i) {
        data_source.emplace_back(
            TestLine2D{ (EntityId)(10 + i), {10.0 + i * 5.0, 10.0 + i * 5.0}, {10.0 + i * 5.0, 10.0 + i * 5.0} },
            (EntityId)(10 + i),
            TimeFrameIndex(i)
        );
        data_source.emplace_back(
            TestLine2D{ (EntityId)(20 + i), {100.0 - i * 5.0, 50.0}, {100.0 - i * 5.0, 50.0} },
            (EntityId)(20 + i),
            TimeFrameIndex(i)
        );
    }

    // 2. --- EXECUTION ---
    tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(4));

    // 3. --- ASSERTIONS ---
    // Verify groups exist
    REQUIRE(group_manager.hasGroup(group1));
    REQUIRE(group_manager.hasGroup(group2));

    // Check that all entities were correctly assigned. Each group should have 5 entities now.
    REQUIRE(group_manager.getGroupSize(group1) == 5); // 1 (initial) + 4 (assigned)
    REQUIRE(group_manager.getGroupSize(group2) == 5);

    // Verify the correct entities were assigned to group 1
    std::vector<EntityId> expected_g1 = {1, 11, 12, 13, 14};
    auto group1_entities = group_manager.getEntitiesInGroup(group1);
    std::sort(group1_entities.begin(), group1_entities.end());
    REQUIRE(group1_entities == expected_g1);

    // Verify the correct entities were assigned to group 2
    std::vector<EntityId> expected_g2 = {2, 21, 22, 23, 24};
    auto group2_entities = group_manager.getEntitiesInGroup(group2);
    std::sort(group2_entities.begin(), group2_entities.end());
    REQUIRE(group2_entities == expected_g2);
}

TEST_CASE("StateEstimation - KalmanFilter - Identity confidence with ambiguous assignments", "[KalmanFilter][IdentityConfidence]") {
    using namespace StateEstimation;

    // 1. --- SETUP ---
    double dt = 1.0;
    Eigen::MatrixXd F(4, 4); F << 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1;
    Eigen::MatrixXd H(2, 4); H << 1, 0, 0, 0, 0, 1, 0, 0;
    Eigen::MatrixXd Q(4, 4); Q.setIdentity(); Q *= 0.1;
    Eigen::MatrixXd R(2, 2); R.setIdentity(); R *= 5.0;

    auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);
    auto feature_extractor = std::make_unique<LineCentroidExtractor>();
    auto assigner = std::make_unique<HungarianAssigner>(100.0, H, R, "kalman_features");

    Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), std::move(assigner));

    // --- Generate Data for Assignment Testing ---
    std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;
    
    EntityGroupManager group_manager;
    GroupId group1 = group_manager.createGroup("Group 1");
    GroupId group2 = group_manager.createGroup("Group 2");

    // Frame 0: Ground truth - establish initial tracks
    data_source.emplace_back(TestLine2D{ (EntityId)1, {10, 10}, {10, 10} }, (EntityId)1, TimeFrameIndex(0));
    data_source.emplace_back(TestLine2D{ (EntityId)2, {100, 10}, {100, 10} }, (EntityId)2, TimeFrameIndex(0));
    
    Tracker<TestLine2D>::GroundTruthMap ground_truth;
    ground_truth[TimeFrameIndex(0)] = {{group1, 1}, {group2, 2}};

    // Frames 1-3: Add ungrouped data that should be assigned
    // These are close to the predicted positions, so assignment should be easy initially
    for (int i = 1; i <= 3; ++i) {
        // Easy assignments - close to predicted positions
        double g1_x = 10.0 + i * 5.0; // Group 1 moves right
        double g2_x = 100.0 - i * 5.0; // Group 2 moves left
        
        data_source.emplace_back(
            TestLine2D{ (EntityId)(10 + i), {g1_x, 10.0}, {g1_x, 10.0} },
            (EntityId)(10 + i),
            TimeFrameIndex(i)
        );
        data_source.emplace_back(
            TestLine2D{ (EntityId)(20 + i), {g2_x, 10.0}, {g2_x, 10.0} },
            (EntityId)(20 + i),
            TimeFrameIndex(i)
        );
    }

    // Frames 4-5: Make assignments more difficult by bringing lines closer
    for (int i = 4; i <= 5; ++i) {
        // Much harder assignments - lines are very close, making assignment ambiguous
        double g1_x = 10.0 + i * 12.0; // Much faster movement
        double g2_x = 100.0 - i * 12.0; // Much faster movement
        
        data_source.emplace_back(
            TestLine2D{ (EntityId)(10 + i), {g1_x, 10.0}, {g1_x, 10.0} },
            (EntityId)(10 + i),
            TimeFrameIndex(i)
        );
        data_source.emplace_back(
            TestLine2D{ (EntityId)(20 + i), {g2_x, 10.0}, {g2_x, 10.0} },
            (EntityId)(20 + i),
            TimeFrameIndex(i)
        );
    }

    // Frame 6: No ground truth - let confidence accumulate
    data_source.emplace_back(TestLine2D{ (EntityId)3, {50, 10}, {50, 10} }, (EntityId)3, TimeFrameIndex(6));
    data_source.emplace_back(TestLine2D{ (EntityId)4, {52, 10}, {52, 10} }, (EntityId)4, TimeFrameIndex(6));
    // No ground truth at frame 6 - let confidence accumulate

    // 2. --- EXECUTION ---
    tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(6));

    // 3. --- ASSERTIONS ---
    // Check that groups exist and have been populated
    REQUIRE(group_manager.hasGroup(group1));
    REQUIRE(group_manager.hasGroup(group2));
    
    std::cout << "Group 1 size: " << group_manager.getGroupSize(group1) << std::endl;
    std::cout << "Group 2 size: " << group_manager.getGroupSize(group2) << std::endl;

    // Get identity confidence after processing
    double conf_group1 = tracker.getIdentityConfidence(group1);
    double conf_group2 = tracker.getIdentityConfidence(group2);
    
    // Get measurement noise scaling
    double noise_scale_group1 = tracker.getMeasurementNoiseScale(group1);
    double noise_scale_group2 = tracker.getMeasurementNoiseScale(group2);
    
    // Get minimum confidence since last anchor
    double min_conf_group1 = tracker.getMinConfidenceSinceAnchor(group1);
    double min_conf_group2 = tracker.getMinConfidenceSinceAnchor(group2);

    std::cout << "Group 1 - Confidence: " << conf_group1 
              << ", Noise Scale: " << noise_scale_group1 
              << ", Min Confidence: " << min_conf_group1 << std::endl;
    std::cout << "Group 2 - Confidence: " << conf_group2 
              << ", Noise Scale: " << noise_scale_group2 
              << ", Min Confidence: " << min_conf_group2 << std::endl;

    // Verify confidence values are valid
    REQUIRE(conf_group1 >= 0.1); // Should not go below floor
    REQUIRE(conf_group1 <= 1.0);
    REQUIRE(conf_group2 >= 0.1);
    REQUIRE(conf_group2 <= 1.0);

    // Verify noise scaling is reasonable
    REQUIRE(noise_scale_group1 >= 1.0);
    REQUIRE(noise_scale_group2 >= 1.0);

    // Minimum confidence should be <= current confidence
    REQUIRE(min_conf_group1 <= conf_group1);
    REQUIRE(min_conf_group2 <= conf_group2);
}

TEST_CASE("StateEstimation - IdentityConfidence - Basic functionality", "[IdentityConfidence]") {
    using namespace StateEstimation;

    IdentityConfidence confidence;
    
    // Test initial state
    REQUIRE(confidence.getConfidence() == 1.0);
    REQUIRE(confidence.getMeasurementNoiseScale() == 1.0);
    REQUIRE(confidence.getMinConfidenceSinceAnchor() == 1.0);
    
    // Test confidence degradation with poor assignment
    confidence.updateOnAssignment(0.8, 1.0); // High cost, at threshold
    double conf_after_poor = confidence.getConfidence();
    REQUIRE(conf_after_poor < 1.0);
    REQUIRE(conf_after_poor > 0.1); // Should not go below floor
    
    // Test measurement noise scaling
    double noise_scale = confidence.getMeasurementNoiseScale();
    REQUIRE(noise_scale > 1.0); // Should be inflated
    
    std::cout << "After poor assignment - Confidence: " << conf_after_poor 
              << ", Noise Scale: " << noise_scale << std::endl;
    
    // Test slow recovery with excellent assignment
    confidence.allowSlowRecovery(0.05, 0.1); // Very good assignment
    double conf_after_recovery = confidence.getConfidence();
    REQUIRE(conf_after_recovery > conf_after_poor);
    
    std::cout << "After recovery - Confidence: " << conf_after_recovery << std::endl;
    
    // Test ground truth reset
    confidence.resetOnGroundTruth();
    REQUIRE(confidence.getConfidence() == 1.0);
    REQUIRE(confidence.getMinConfidenceSinceAnchor() == 1.0);
    
    std::cout << "After ground truth reset - Confidence: " << confidence.getConfidence() << std::endl;
}

TEST_CASE("StateEstimation - KalmanFilter - High assignment costs", "[KalmanFilter][IdentityConfidence]") {
    using namespace StateEstimation;

    // 1. --- SETUP ---
    double dt = 1.0;
    Eigen::MatrixXd F(4, 4); F << 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1;
    Eigen::MatrixXd H(2, 4); H << 1, 0, 0, 0, 0, 1, 0, 0;
    Eigen::MatrixXd Q(4, 4); Q.setIdentity(); Q *= 0.1;
    Eigen::MatrixXd R(2, 2); R.setIdentity(); R *= 5.0;

    auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);
    auto feature_extractor = std::make_unique<LineCentroidExtractor>();
    // Use a very low threshold to force high assignment costs
    auto assigner = std::make_unique<HungarianAssigner>(10.0, H, R, "kalman_features");

    Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), std::move(assigner));

    // --- Generate Data with High Assignment Costs ---
    std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;
    
    EntityGroupManager group_manager;
    GroupId group1 = group_manager.createGroup("Group 1");
    GroupId group2 = group_manager.createGroup("Group 2");

    // Frame 0: Ground truth - establish initial tracks
    data_source.emplace_back(TestLine2D{ (EntityId)1, {10, 10}, {10, 10} }, (EntityId)1, TimeFrameIndex(0));
    data_source.emplace_back(TestLine2D{ (EntityId)2, {100, 10}, {100, 10} }, (EntityId)2, TimeFrameIndex(0));
    
    Tracker<TestLine2D>::GroundTruthMap ground_truth;
    ground_truth[TimeFrameIndex(0)] = {{group1, 1}, {group2, 2}};

    // Frames 1-3: Add data that will have high assignment costs
    for (int i = 1; i <= 3; ++i) {
        // Place observations far from predicted positions to force high costs
        double g1_x = 10.0 + i * 20.0; // Very far from prediction
        double g2_x = 100.0 - i * 20.0; // Very far from prediction
        
        data_source.emplace_back(
            TestLine2D{ (EntityId)(10 + i), {g1_x, 10.0}, {g1_x, 10.0} },
            (EntityId)(10 + i),
            TimeFrameIndex(i)
        );
        data_source.emplace_back(
            TestLine2D{ (EntityId)(20 + i), {g2_x, 10.0}, {g2_x, 10.0} },
            (EntityId)(20 + i),
            TimeFrameIndex(i)
        );
    }

    // 2. --- EXECUTION ---
    tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(3));

    // 3. --- ASSERTIONS ---
    double conf_group1 = tracker.getIdentityConfidence(group1);
    double conf_group2 = tracker.getIdentityConfidence(group2);
    
    double noise_scale_group1 = tracker.getMeasurementNoiseScale(group1);
    double noise_scale_group2 = tracker.getMeasurementNoiseScale(group2);

    std::cout << "High cost test - Group 1 - Confidence: " << conf_group1 
              << ", Noise Scale: " << noise_scale_group1 << std::endl;
    std::cout << "High cost test - Group 2 - Confidence: " << conf_group2 
              << ", Noise Scale: " << noise_scale_group2 << std::endl;

    // With high assignment costs, we should see confidence degradation
    // Note: This might still be 1.0 if the assignment costs are below the threshold
    REQUIRE(conf_group1 >= 0.1);
    REQUIRE(conf_group1 <= 1.0);
    REQUIRE(conf_group2 >= 0.1);
    REQUIRE(conf_group2 <= 1.0);
}

1	#include <catch2/catch_test_macros.hpp>
2	#include <Eigen/Dense>
3	#include <iostream>
4
5	#include "Kalman/KalmanFilter.hpp"
6	#include "Tracker.hpp"
7	#include "Features/IFeatureExtractor.hpp"
8	#include "Assignment/HungarianAssigner.hpp"
9	#include "Entity/EntityTypes.hpp"
10	#include "Entity/EntityGroupManager.hpp"
11	#include "TimeFrame/TimeFrame.hpp"
12	#include "IdentityConfidence.hpp"
13
14	// --- Test-Specific Mocks and Implementations ---
15
16	// A simple Line2D struct for this test, mirroring what CoreGeometry might provide.
17	struct TestLine2D {
18	EntityId id;
19	Eigen::Vector2d p1;
20	Eigen::Vector2d p2;
21
22	Eigen::Vector2d centroid() const {	62✔
23	return (p1 + p2) / 2.0;	124✔
24	}
25	};
26
27	// A concrete feature extractor for TestLine2D centroids.
28	class LineCentroidExtractor : public StateEstimation::IFeatureExtractor<TestLine2D> {
29	public:
30	Eigen::VectorXd getFilterFeatures(const TestLine2D& data) const override {	54✔
31	Eigen::Vector2d c = data.centroid();	54✔
32	Eigen::VectorXd features(2);	54✔
33	features << c.x(), c.y();	54✔
34	return features;	108✔
UNCOV 35	}	×
36
37	StateEstimation::FeatureCache getAllFeatures(const TestLine2D& data) const override {	26✔
38	StateEstimation::FeatureCache cache;	26✔
39	cache[getFilterFeatureName()] = getFilterFeatures(data);	26✔
40	return cache;	26✔
UNCOV 41	}	×
42
43	std::string getFilterFeatureName() const override {	26✔
44	return "kalman_features";	78✔
45	}
46
47	StateEstimation::FilterState getInitialState(const TestLine2D& data) const override {	8✔
48	Eigen::VectorXd initialState(4);	8✔
49	Eigen::Vector2d centroid = data.centroid();	8✔
50	initialState << centroid.x(), centroid.y(), 0, 0; // Position from centroid, velocity is zero	8✔
51
52	Eigen::MatrixXd p(4, 4);	8✔
53	p.setIdentity();	8✔
54	p *= 100.0; // High initial uncertainty	8✔
55
56	return {initialState, p};	8✔
57	}	8✔
58
59	std::unique_ptr<IFeatureExtractor<TestLine2D>> clone() const override {	×
UNCOV 60	return std::make_unique<LineCentroidExtractor>(*this);	×
61	}
62
UNCOV 63	StateEstimation::FeatureMetadata getMetadata() const override {	×
64	return StateEstimation::FeatureMetadata::create(
65	"kalman_features",
66	2, // 2D measurement
67	StateEstimation::FeatureTemporalType::KINEMATIC_2D
UNCOV 68	);	×
69	}
70	};
71
72
73	TEST_CASE("KalmanFilter tracking and smoothing", "[KalmanFilter]") {	1✔
74	using namespace StateEstimation;
75
76	// 1. --- SETUP ---
77
78	// Define a constant velocity Kalman Filter
79	double dt = 1.0;	1✔
80	Eigen::MatrixXd F(4, 4); // State Transition Matrix	1✔
81	F << 1, 0, dt, 0,	4✔
82	0, 1, 0, dt,	4✔
83	0, 0, 1, 0,	5✔
84	0, 0, 0, 1;	11✔
85
86	Eigen::MatrixXd H(2, 4); // Measurement Matrix	1✔
87	H << 1, 0, 0, 0,	5✔
88	0, 1, 0, 0;	5✔
89
90	Eigen::MatrixXd Q(4, 4); // Process Noise Covariance	1✔
91	Q.setIdentity();	1✔
92	Q *= 0.1;	1✔
93
94	Eigen::MatrixXd R(2, 2); // Measurement Noise Covariance	1✔
95	R.setIdentity();	1✔
96	R *= 5.0;	1✔
97
98	auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);	1✔
99	auto feature_extractor = std::make_unique<LineCentroidExtractor>();	1✔
100
101	// We run in smoothing-only mode, so no assigner is needed.
102	Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), nullptr);	1✔
103
104	// --- Generate Artificial Data ---
105	// New API: vector of tuples (DataType, EntityId, TimeFrameIndex)
106	std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;	1✔
107
108	EntityGroupManager group_manager;	1✔
109	GroupId group1 = group_manager.createGroup("Group 1");	5✔
110	GroupId group2 = group_manager.createGroup("Group 2");	5✔
111
112	for (int i = 0; i <= 10; ++i) {	12✔
113	// Line 1 moves from (10,10) to (60,60)
114	TestLine2D line1 = { (EntityId)i, {10.0 + i * 5.0, 10.0 + i * 5.0}, {10.0 + i * 5.0, 10.0 + i * 5.0} };	11✔
115	// Line 2 moves from (100,50) to (50,50)
116	TestLine2D line2 = { (EntityId)(i + 100), {100.0 - i * 5.0, 50.0}, {100.0 - i * 5.0, 50.0} };	11✔
117
118	data_source.emplace_back(line1, (EntityId)i, TimeFrameIndex(i));	11✔
119	data_source.emplace_back(line2, (EntityId)(i + 100), TimeFrameIndex(i));	11✔
120	}
121
122	// Ground truth map with TimeFrameIndex keys
123	Tracker<TestLine2D>::GroundTruthMap ground_truth;	1✔
124	ground_truth[TimeFrameIndex(0)] = {{group1, 0}, {group2, 100}};	1✔
125	ground_truth[TimeFrameIndex(10)] = {{group1, 10}, {group2, 110}};	1✔
126
127	// Populate the initial groups in EntityGroupManager
128	group_manager.addEntityToGroup(group1, 0);	1✔
129	group_manager.addEntityToGroup(group1, 10);	1✔
130	group_manager.addEntityToGroup(group2, 100);	1✔
131	group_manager.addEntityToGroup(group2, 110);	1✔
132
133
134	// 2. --- EXECUTION ---
135	SmoothedResults results = tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(10));	1✔
136
137	// 3. --- ASSERTIONS ---
138	REQUIRE(results.count(group1) == 1);	1✔
139	REQUIRE(results.count(group2) == 1);	1✔
140
141	// We have 11 frames of data (0-10 inclusive) for the forward pass, which get smoothed.
142	REQUIRE(results.at(group1).size() == 11);	1✔
143	REQUIRE(results.at(group2).size() == 11);	1✔
144
145	// Check if the smoothed results are reasonable.
146	// The smoothed estimate at the midpoint should be close to the true position.
147	const auto& smoothed_g1 = results.at(group1);	1✔
148	const auto& smoothed_g2 = results.at(group2);	1✔
149
150	// True position at frame 5 for line 1 is (10 + 55, 10 + 55) = (35, 35)
151	REQUIRE(std::abs(smoothed_g1[5].state_mean(0) - 35.0) < 5.0);	1✔
152	REQUIRE(std::abs(smoothed_g1[5].state_mean(1) - 35.0) < 5.0);	1✔
153
154	// True position at frame 5 for line 2 is (100 - 5*5, 50) = (75, 50)
155	REQUIRE(std::abs(smoothed_g2[5].state_mean(0) - 75.0) < 5.0);	1✔
156	REQUIRE(std::abs(smoothed_g2[5].state_mean(1) - 50.0) < 5.0);	1✔
157
158	// Check velocity estimates. Group 1 should be moving (+5, +5)
159	REQUIRE(smoothed_g1[5].state_mean(2) > 4.0); // vx	1✔
160	REQUIRE(smoothed_g1[5].state_mean(3) > 4.0); // vy	1✔
161
162	// Group 2 should be moving (-5, 0)
163	REQUIRE(smoothed_g2[5].state_mean(2) < -4.0); // vx	1✔
164	REQUIRE(std::abs(smoothed_g2[5].state_mean(3)) < 1.0); // vy	1✔
165	}	2✔
166
167	TEST_CASE("StateEstimation - KalmanFilter - assignment with Hungarian algorithm", "[KalmanFilter][Assignment]") {	1✔
168	using namespace StateEstimation;
169
170	// 1. --- SETUP ---
171	double dt = 1.0;	1✔
172	Eigen::MatrixXd F(4, 4); F << 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1;	1✔
173	Eigen::MatrixXd H(2, 4); H << 1, 0, 0, 0, 0, 1, 0, 0;	1✔
174	Eigen::MatrixXd Q(4, 4); Q.setIdentity(); Q *= 0.1;	1✔
175	Eigen::MatrixXd R(2, 2); R.setIdentity(); R *= 5.0;	1✔
176
177	auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);	1✔
178	auto feature_extractor = std::make_unique<LineCentroidExtractor>();	1✔
179	auto assigner = std::make_unique<HungarianAssigner>(100.0, H, R, "kalman_features");	1✔
180
181	Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), std::move(assigner));	1✔
182
183	// --- Generate Artificial Data ---
184	// New API: vector of tuples (DataType, EntityId, TimeFrameIndex)
185	std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;	1✔
186
187	EntityGroupManager group_manager;	1✔
188	GroupId group1 = group_manager.createGroup("Group 1");	5✔
189	GroupId group2 = group_manager.createGroup("Group 2");	5✔
190
191	// Frame 0: Ground truth
192	data_source.emplace_back(TestLine2D{ (EntityId)1, {10, 10}, {10, 10} }, (EntityId)1, TimeFrameIndex(0));	1✔
193	data_source.emplace_back(TestLine2D{ (EntityId)2, {100, 50}, {100, 50} }, (EntityId)2, TimeFrameIndex(0));	1✔
194
195	Tracker<TestLine2D>::GroundTruthMap ground_truth;	1✔
196	ground_truth[TimeFrameIndex(0)] = {{group1, 1}, {group2, 2}};	1✔
197
198	group_manager.addEntityToGroup(group1, 1);	1✔
199	group_manager.addEntityToGroup(group2, 2);	1✔
200
201	// Frames 1-4: Ungrouped data that should be assigned
202	for (int i = 1; i <= 4; ++i) {	5✔
203	data_source.emplace_back(	12✔
204	TestLine2D{ (EntityId)(10 + i), {10.0 + i * 5.0, 10.0 + i * 5.0}, {10.0 + i * 5.0, 10.0 + i * 5.0} },	8✔
205	(EntityId)(10 + i),	8✔
206	TimeFrameIndex(i)	8✔
207	);
208	data_source.emplace_back(	12✔
209	TestLine2D{ (EntityId)(20 + i), {100.0 - i * 5.0, 50.0}, {100.0 - i * 5.0, 50.0} },	8✔
210	(EntityId)(20 + i),	8✔
211	TimeFrameIndex(i)	8✔
212	);
213	}
214
215	// 2. --- EXECUTION ---
216	tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(4));	1✔
217
218	// 3. --- ASSERTIONS ---
219	// Verify groups exist
220	REQUIRE(group_manager.hasGroup(group1));	1✔
221	REQUIRE(group_manager.hasGroup(group2));	1✔
222
223	// Check that all entities were correctly assigned. Each group should have 5 entities now.
224	REQUIRE(group_manager.getGroupSize(group1) == 5); // 1 (initial) + 4 (assigned)	1✔
225	REQUIRE(group_manager.getGroupSize(group2) == 5);	1✔
226
227	// Verify the correct entities were assigned to group 1
228	std::vector<EntityId> expected_g1 = {1, 11, 12, 13, 14};	3✔
229	auto group1_entities = group_manager.getEntitiesInGroup(group1);	1✔
230	std::sort(group1_entities.begin(), group1_entities.end());	1✔
231	REQUIRE(group1_entities == expected_g1);	1✔
232
233	// Verify the correct entities were assigned to group 2
234	std::vector<EntityId> expected_g2 = {2, 21, 22, 23, 24};	3✔
235	auto group2_entities = group_manager.getEntitiesInGroup(group2);	1✔
236	std::sort(group2_entities.begin(), group2_entities.end());	1✔
237	REQUIRE(group2_entities == expected_g2);	1✔
238	}	2✔
239
240	TEST_CASE("StateEstimation - KalmanFilter - Identity confidence with ambiguous assignments", "[KalmanFilter][IdentityConfidence]") {	1✔
241	using namespace StateEstimation;
242
243	// 1. --- SETUP ---
244	double dt = 1.0;	1✔
245	Eigen::MatrixXd F(4, 4); F << 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1;	1✔
246	Eigen::MatrixXd H(2, 4); H << 1, 0, 0, 0, 0, 1, 0, 0;	1✔
247	Eigen::MatrixXd Q(4, 4); Q.setIdentity(); Q *= 0.1;	1✔
248	Eigen::MatrixXd R(2, 2); R.setIdentity(); R *= 5.0;	1✔
249
250	auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);	1✔
251	auto feature_extractor = std::make_unique<LineCentroidExtractor>();	1✔
252	auto assigner = std::make_unique<HungarianAssigner>(100.0, H, R, "kalman_features");	1✔
253
254	Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), std::move(assigner));	1✔
255
256	// --- Generate Data for Assignment Testing ---
257	std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;	1✔
258
259	EntityGroupManager group_manager;	1✔
260	GroupId group1 = group_manager.createGroup("Group 1");	5✔
261	GroupId group2 = group_manager.createGroup("Group 2");	5✔
262
263	// Frame 0: Ground truth - establish initial tracks
264	data_source.emplace_back(TestLine2D{ (EntityId)1, {10, 10}, {10, 10} }, (EntityId)1, TimeFrameIndex(0));	1✔
265	data_source.emplace_back(TestLine2D{ (EntityId)2, {100, 10}, {100, 10} }, (EntityId)2, TimeFrameIndex(0));	1✔
266
267	Tracker<TestLine2D>::GroundTruthMap ground_truth;	1✔
268	ground_truth[TimeFrameIndex(0)] = {{group1, 1}, {group2, 2}};	1✔
269
270	// Frames 1-3: Add ungrouped data that should be assigned
271	// These are close to the predicted positions, so assignment should be easy initially
272	for (int i = 1; i <= 3; ++i) {	4✔
273	// Easy assignments - close to predicted positions
274	double g1_x = 10.0 + i * 5.0; // Group 1 moves right	3✔
275	double g2_x = 100.0 - i * 5.0; // Group 2 moves left	3✔
276
277	data_source.emplace_back(	9✔
278	TestLine2D{ (EntityId)(10 + i), {g1_x, 10.0}, {g1_x, 10.0} },	6✔
279	(EntityId)(10 + i),	6✔
280	TimeFrameIndex(i)	6✔
281	);
282	data_source.emplace_back(	9✔
283	TestLine2D{ (EntityId)(20 + i), {g2_x, 10.0}, {g2_x, 10.0} },	6✔
284	(EntityId)(20 + i),	6✔
285	TimeFrameIndex(i)	6✔
286	);
287	}
288
289	// Frames 4-5: Make assignments more difficult by bringing lines closer
290	for (int i = 4; i <= 5; ++i) {	3✔
291	// Much harder assignments - lines are very close, making assignment ambiguous
292	double g1_x = 10.0 + i * 12.0; // Much faster movement	2✔
293	double g2_x = 100.0 - i * 12.0; // Much faster movement	2✔
294
295	data_source.emplace_back(	6✔
296	TestLine2D{ (EntityId)(10 + i), {g1_x, 10.0}, {g1_x, 10.0} },	4✔
297	(EntityId)(10 + i),	4✔
298	TimeFrameIndex(i)	4✔
299	);
300	data_source.emplace_back(	6✔
301	TestLine2D{ (EntityId)(20 + i), {g2_x, 10.0}, {g2_x, 10.0} },	4✔
302	(EntityId)(20 + i),	4✔
303	TimeFrameIndex(i)	4✔
304	);
305	}
306
307	// Frame 6: No ground truth - let confidence accumulate
308	data_source.emplace_back(TestLine2D{ (EntityId)3, {50, 10}, {50, 10} }, (EntityId)3, TimeFrameIndex(6));	1✔
309	data_source.emplace_back(TestLine2D{ (EntityId)4, {52, 10}, {52, 10} }, (EntityId)4, TimeFrameIndex(6));	1✔
310	// No ground truth at frame 6 - let confidence accumulate
311
312	// 2. --- EXECUTION ---
313	tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(6));	1✔
314
315	// 3. --- ASSERTIONS ---
316	// Check that groups exist and have been populated
317	REQUIRE(group_manager.hasGroup(group1));	1✔
318	REQUIRE(group_manager.hasGroup(group2));	1✔
319
320	std::cout << "Group 1 size: " << group_manager.getGroupSize(group1) << std::endl;	1✔
321	std::cout << "Group 2 size: " << group_manager.getGroupSize(group2) << std::endl;	1✔
322
323	// Get identity confidence after processing
324	double conf_group1 = tracker.getIdentityConfidence(group1);	1✔
325	double conf_group2 = tracker.getIdentityConfidence(group2);	1✔
326
327	// Get measurement noise scaling
328	double noise_scale_group1 = tracker.getMeasurementNoiseScale(group1);	1✔
329	double noise_scale_group2 = tracker.getMeasurementNoiseScale(group2);	1✔
330
331	// Get minimum confidence since last anchor
332	double min_conf_group1 = tracker.getMinConfidenceSinceAnchor(group1);	1✔
333	double min_conf_group2 = tracker.getMinConfidenceSinceAnchor(group2);	1✔
334
335	std::cout << "Group 1 - Confidence: " << conf_group1	1✔
336	<< ", Noise Scale: " << noise_scale_group1	1✔
337	<< ", Min Confidence: " << min_conf_group1 << std::endl;	1✔
338	std::cout << "Group 2 - Confidence: " << conf_group2	1✔
339	<< ", Noise Scale: " << noise_scale_group2	1✔
340	<< ", Min Confidence: " << min_conf_group2 << std::endl;	1✔
341
342	// Verify confidence values are valid
343	REQUIRE(conf_group1 >= 0.1); // Should not go below floor	1✔
344	REQUIRE(conf_group1 <= 1.0);	1✔
345	REQUIRE(conf_group2 >= 0.1);	1✔
346	REQUIRE(conf_group2 <= 1.0);	1✔
347
348	// Verify noise scaling is reasonable
349	REQUIRE(noise_scale_group1 >= 1.0);	1✔
350	REQUIRE(noise_scale_group2 >= 1.0);	1✔
351
352	// Minimum confidence should be <= current confidence
353	REQUIRE(min_conf_group1 <= conf_group1);	1✔
354	REQUIRE(min_conf_group2 <= conf_group2);	1✔
355	}	2✔
356
357	TEST_CASE("StateEstimation - IdentityConfidence - Basic functionality", "[IdentityConfidence]") {	1✔
358	using namespace StateEstimation;
359
360	IdentityConfidence confidence;	1✔
361
362	// Test initial state
363	REQUIRE(confidence.getConfidence() == 1.0);	1✔
364	REQUIRE(confidence.getMeasurementNoiseScale() == 1.0);	1✔
365	REQUIRE(confidence.getMinConfidenceSinceAnchor() == 1.0);	1✔
366
367	// Test confidence degradation with poor assignment
368	confidence.updateOnAssignment(0.8, 1.0); // High cost, at threshold	1✔
369	double conf_after_poor = confidence.getConfidence();	1✔
370	REQUIRE(conf_after_poor < 1.0);	1✔
371	REQUIRE(conf_after_poor > 0.1); // Should not go below floor	1✔
372
373	// Test measurement noise scaling
374	double noise_scale = confidence.getMeasurementNoiseScale();	1✔
375	REQUIRE(noise_scale > 1.0); // Should be inflated	1✔
376
377	std::cout << "After poor assignment - Confidence: " << conf_after_poor	1✔
378	<< ", Noise Scale: " << noise_scale << std::endl;	1✔
379
380	// Test slow recovery with excellent assignment
381	confidence.allowSlowRecovery(0.05, 0.1); // Very good assignment	1✔
382	double conf_after_recovery = confidence.getConfidence();	1✔
383	REQUIRE(conf_after_recovery > conf_after_poor);	1✔
384
385	std::cout << "After recovery - Confidence: " << conf_after_recovery << std::endl;	1✔
386
387	// Test ground truth reset
388	confidence.resetOnGroundTruth();	1✔
389	REQUIRE(confidence.getConfidence() == 1.0);	1✔
390	REQUIRE(confidence.getMinConfidenceSinceAnchor() == 1.0);	1✔
391
392	std::cout << "After ground truth reset - Confidence: " << confidence.getConfidence() << std::endl;	1✔
393	}	1✔
394
395	TEST_CASE("StateEstimation - KalmanFilter - High assignment costs", "[KalmanFilter][IdentityConfidence]") {	1✔
396	using namespace StateEstimation;
397
398	// 1. --- SETUP ---
399	double dt = 1.0;	1✔
400	Eigen::MatrixXd F(4, 4); F << 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1;	1✔
401	Eigen::MatrixXd H(2, 4); H << 1, 0, 0, 0, 0, 1, 0, 0;	1✔
402	Eigen::MatrixXd Q(4, 4); Q.setIdentity(); Q *= 0.1;	1✔
403	Eigen::MatrixXd R(2, 2); R.setIdentity(); R *= 5.0;	1✔
404
405	auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);	1✔
406	auto feature_extractor = std::make_unique<LineCentroidExtractor>();	1✔
407	// Use a very low threshold to force high assignment costs
408	auto assigner = std::make_unique<HungarianAssigner>(10.0, H, R, "kalman_features");	1✔
409
410	Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), std::move(assigner));	1✔
411
412	// --- Generate Data with High Assignment Costs ---
413	std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;	1✔
414
415	EntityGroupManager group_manager;	1✔
416	GroupId group1 = group_manager.createGroup("Group 1");	5✔
417	GroupId group2 = group_manager.createGroup("Group 2");	5✔
418
419	// Frame 0: Ground truth - establish initial tracks
420	data_source.emplace_back(TestLine2D{ (EntityId)1, {10, 10}, {10, 10} }, (EntityId)1, TimeFrameIndex(0));	1✔
421	data_source.emplace_back(TestLine2D{ (EntityId)2, {100, 10}, {100, 10} }, (EntityId)2, TimeFrameIndex(0));	1✔
422
423	Tracker<TestLine2D>::GroundTruthMap ground_truth;	1✔
424	ground_truth[TimeFrameIndex(0)] = {{group1, 1}, {group2, 2}};	1✔
425
426	// Frames 1-3: Add data that will have high assignment costs
427	for (int i = 1; i <= 3; ++i) {	4✔
428	// Place observations far from predicted positions to force high costs
429	double g1_x = 10.0 + i * 20.0; // Very far from prediction	3✔
430	double g2_x = 100.0 - i * 20.0; // Very far from prediction	3✔
431
432	data_source.emplace_back(	9✔
433	TestLine2D{ (EntityId)(10 + i), {g1_x, 10.0}, {g1_x, 10.0} },	6✔
434	(EntityId)(10 + i),	6✔
435	TimeFrameIndex(i)	6✔
436	);
437	data_source.emplace_back(	9✔
438	TestLine2D{ (EntityId)(20 + i), {g2_x, 10.0}, {g2_x, 10.0} },	6✔
439	(EntityId)(20 + i),	6✔
440	TimeFrameIndex(i)	6✔
441	);
442	}
443
444	// 2. --- EXECUTION ---
445	tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(3));	1✔
446
447	// 3. --- ASSERTIONS ---
448	double conf_group1 = tracker.getIdentityConfidence(group1);	1✔
449	double conf_group2 = tracker.getIdentityConfidence(group2);	1✔
450
451	double noise_scale_group1 = tracker.getMeasurementNoiseScale(group1);	1✔
452	double noise_scale_group2 = tracker.getMeasurementNoiseScale(group2);	1✔
453
454	std::cout << "High cost test - Group 1 - Confidence: " << conf_group1	1✔
455	<< ", Noise Scale: " << noise_scale_group1 << std::endl;	1✔
456	std::cout << "High cost test - Group 2 - Confidence: " << conf_group2	1✔
457	<< ", Noise Scale: " << noise_scale_group2 << std::endl;	1✔
458
459	// With high assignment costs, we should see confidence degradation
460	// Note: This might still be 1.0 if the assignment costs are below the threshold
461	REQUIRE(conf_group1 >= 0.1);	1✔
462	REQUIRE(conf_group1 <= 1.0);	1✔
463	REQUIRE(conf_group2 >= 0.1);	1✔
464	REQUIRE(conf_group2 <= 1.0);	1✔
465	}	2✔

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