18179930748

Committed 02 Oct 2025 12:16AM UTC coverage: 71.188% (+0.01%) from 71.174%

Build # 18179930748

Build Type

push

github

Committed by

paulmthompson

Commit Message

can specify if features have derivatives or not

Run Details

110 of 190 new or added lines in 7 files covered. (57.89%)

8 existing lines in 1 file now uncovered.

47410 of 66598 relevant lines covered (71.19%)

1081.81 hits per line

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

95.08

/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"

// --- 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);
}

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
13	// --- Test-Specific Mocks and Implementations ---
14
15	// A simple Line2D struct for this test, mirroring what CoreGeometry might provide.
16	struct TestLine2D {
17	EntityId id;
18	Eigen::Vector2d p1;
19	Eigen::Vector2d p2;
20
21	Eigen::Vector2d centroid() const {	22✔
22	return (p1 + p2) / 2.0;	44✔
23	}
24	};
25
26	// A concrete feature extractor for TestLine2D centroids.
27	class LineCentroidExtractor : public StateEstimation::IFeatureExtractor<TestLine2D> {
28	public:
29	Eigen::VectorXd getFilterFeatures(const TestLine2D& data) const override {	18✔
30	Eigen::Vector2d c = data.centroid();	18✔
31	Eigen::VectorXd features(2);	18✔
32	features << c.x(), c.y();	18✔
33	return features;	36✔
34	}	×
35
36	StateEstimation::FeatureCache getAllFeatures(const TestLine2D& data) const override {	8✔
37	StateEstimation::FeatureCache cache;	8✔
38	cache[getFilterFeatureName()] = getFilterFeatures(data);	8✔
39	return cache;	8✔
40	}	×
41
42	std::string getFilterFeatureName() const override {	8✔
43	return "kalman_features";	24✔
44	}
45
46	StateEstimation::FilterState getInitialState(const TestLine2D& data) const override {	4✔
47	Eigen::VectorXd initialState(4);	4✔
48	Eigen::Vector2d centroid = data.centroid();	4✔
49	initialState << centroid.x(), centroid.y(), 0, 0; // Position from centroid, velocity is zero	4✔
50
51	Eigen::MatrixXd p(4, 4);	4✔
52	p.setIdentity();	4✔
53	p *= 100.0; // High initial uncertainty	4✔
54
55	return {initialState, p};	4✔
56	}	4✔
57
58	std::unique_ptr<IFeatureExtractor<TestLine2D>> clone() const override {	×
59	return std::make_unique<LineCentroidExtractor>(*this);	×
60	}
61
NEW 62	StateEstimation::FeatureMetadata getMetadata() const override {	×
63	return StateEstimation::FeatureMetadata::create(
64	"kalman_features",
65	2, // 2D measurement
66	StateEstimation::FeatureTemporalType::KINEMATIC_2D
NEW 67	);	×
68	}
69	};
70
71
72	TEST_CASE("KalmanFilter tracking and smoothing", "[KalmanFilter]") {	1✔
73	using namespace StateEstimation;
74
75	// 1. --- SETUP ---
76
77	// Define a constant velocity Kalman Filter
78	double dt = 1.0;	1✔
79	Eigen::MatrixXd F(4, 4); // State Transition Matrix	1✔
80	F << 1, 0, dt, 0,	4✔
81	0, 1, 0, dt,	4✔
82	0, 0, 1, 0,	5✔
83	0, 0, 0, 1;	11✔
84
85	Eigen::MatrixXd H(2, 4); // Measurement Matrix	1✔
86	H << 1, 0, 0, 0,	5✔
87	0, 1, 0, 0;	5✔
88
89	Eigen::MatrixXd Q(4, 4); // Process Noise Covariance	1✔
90	Q.setIdentity();	1✔
91	Q *= 0.1;	1✔
92
93	Eigen::MatrixXd R(2, 2); // Measurement Noise Covariance	1✔
94	R.setIdentity();	1✔
95	R *= 5.0;	1✔
96
97	auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);	1✔
98	auto feature_extractor = std::make_unique<LineCentroidExtractor>();	1✔
99
100	// We run in smoothing-only mode, so no assigner is needed.
101	Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), nullptr);	1✔
102
103	// --- Generate Artificial Data ---
104	// New API: vector of tuples (DataType, EntityId, TimeFrameIndex)
105	std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;	1✔
106
107	EntityGroupManager group_manager;	1✔
108	GroupId group1 = group_manager.createGroup("Group 1");	5✔
109	GroupId group2 = group_manager.createGroup("Group 2");	5✔
110
111	for (int i = 0; i <= 10; ++i) {	12✔
112	// Line 1 moves from (10,10) to (60,60)
113	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✔
114	// Line 2 moves from (100,50) to (50,50)
115	TestLine2D line2 = { (EntityId)(i + 100), {100.0 - i * 5.0, 50.0}, {100.0 - i * 5.0, 50.0} };	11✔
116
117	data_source.emplace_back(line1, (EntityId)i, TimeFrameIndex(i));	11✔
118	data_source.emplace_back(line2, (EntityId)(i + 100), TimeFrameIndex(i));	11✔
119	}
120
121	// Ground truth map with TimeFrameIndex keys
122	Tracker<TestLine2D>::GroundTruthMap ground_truth;	1✔
123	ground_truth[TimeFrameIndex(0)] = {{group1, 0}, {group2, 100}};	1✔
124	ground_truth[TimeFrameIndex(10)] = {{group1, 10}, {group2, 110}};	1✔
125
126	// Populate the initial groups in EntityGroupManager
127	group_manager.addEntityToGroup(group1, 0);	1✔
128	group_manager.addEntityToGroup(group1, 10);	1✔
129	group_manager.addEntityToGroup(group2, 100);	1✔
130	group_manager.addEntityToGroup(group2, 110);	1✔
131
132
133	// 2. --- EXECUTION ---
134	SmoothedResults results = tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(10));	1✔
135
136	// 3. --- ASSERTIONS ---
137	REQUIRE(results.count(group1) == 1);	1✔
138	REQUIRE(results.count(group2) == 1);	1✔
139
140	// We have 11 frames of data (0-10 inclusive) for the forward pass, which get smoothed.
141	REQUIRE(results.at(group1).size() == 11);	1✔
142	REQUIRE(results.at(group2).size() == 11);	1✔
143
144	// Check if the smoothed results are reasonable.
145	// The smoothed estimate at the midpoint should be close to the true position.
146	const auto& smoothed_g1 = results.at(group1);	1✔
147	const auto& smoothed_g2 = results.at(group2);	1✔
148
149	// True position at frame 5 for line 1 is (10 + 55, 10 + 55) = (35, 35)
150	REQUIRE(std::abs(smoothed_g1[5].state_mean(0) - 35.0) < 5.0);	1✔
151	REQUIRE(std::abs(smoothed_g1[5].state_mean(1) - 35.0) < 5.0);	1✔
152
153	// True position at frame 5 for line 2 is (100 - 5*5, 50) = (75, 50)
154	REQUIRE(std::abs(smoothed_g2[5].state_mean(0) - 75.0) < 5.0);	1✔
155	REQUIRE(std::abs(smoothed_g2[5].state_mean(1) - 50.0) < 5.0);	1✔
156
157	// Check velocity estimates. Group 1 should be moving (+5, +5)
158	REQUIRE(smoothed_g1[5].state_mean(2) > 4.0); // vx	1✔
159	REQUIRE(smoothed_g1[5].state_mean(3) > 4.0); // vy	1✔
160
161	// Group 2 should be moving (-5, 0)
162	REQUIRE(smoothed_g2[5].state_mean(2) < -4.0); // vx	1✔
163	REQUIRE(std::abs(smoothed_g2[5].state_mean(3)) < 1.0); // vy	1✔
164	}	2✔
165
166	TEST_CASE("StateEstimation - KalmanFilter - assignment with Hungarian algorithm", "[KalmanFilter][Assignment]") {	1✔
167	using namespace StateEstimation;
168
169	// 1. --- SETUP ---
170	double dt = 1.0;	1✔
171	Eigen::MatrixXd F(4, 4); F << 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1;	1✔
172	Eigen::MatrixXd H(2, 4); H << 1, 0, 0, 0, 0, 1, 0, 0;	1✔
173	Eigen::MatrixXd Q(4, 4); Q.setIdentity(); Q *= 0.1;	1✔
174	Eigen::MatrixXd R(2, 2); R.setIdentity(); R *= 5.0;	1✔
175
176	auto kalman_filter = std::make_unique<KalmanFilter>(F, H, Q, R);	1✔
177	auto feature_extractor = std::make_unique<LineCentroidExtractor>();	1✔
178	auto assigner = std::make_unique<HungarianAssigner>(100.0, H, R, "kalman_features");	1✔
179
180	Tracker<TestLine2D> tracker(std::move(kalman_filter), std::move(feature_extractor), std::move(assigner));	1✔
181
182	// --- Generate Artificial Data ---
183	// New API: vector of tuples (DataType, EntityId, TimeFrameIndex)
184	std::vector<std::tuple<TestLine2D, EntityId, TimeFrameIndex>> data_source;	1✔
185
186	EntityGroupManager group_manager;	1✔
187	GroupId group1 = group_manager.createGroup("Group 1");	5✔
188	GroupId group2 = group_manager.createGroup("Group 2");	5✔
189
190	// Frame 0: Ground truth
191	data_source.emplace_back(TestLine2D{ (EntityId)1, {10, 10}, {10, 10} }, (EntityId)1, TimeFrameIndex(0));	1✔
192	data_source.emplace_back(TestLine2D{ (EntityId)2, {100, 50}, {100, 50} }, (EntityId)2, TimeFrameIndex(0));	1✔
193
194	Tracker<TestLine2D>::GroundTruthMap ground_truth;	1✔
195	ground_truth[TimeFrameIndex(0)] = {{group1, 1}, {group2, 2}};	1✔
196
197	group_manager.addEntityToGroup(group1, 1);	1✔
198	group_manager.addEntityToGroup(group2, 2);	1✔
199
200	// Frames 1-4: Ungrouped data that should be assigned
201	for (int i = 1; i <= 4; ++i) {	5✔
202	data_source.emplace_back(	12✔
203	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✔
204	(EntityId)(10 + i),	8✔
205	TimeFrameIndex(i)	8✔
206	);
207	data_source.emplace_back(	12✔
208	TestLine2D{ (EntityId)(20 + i), {100.0 - i * 5.0, 50.0}, {100.0 - i * 5.0, 50.0} },	8✔
209	(EntityId)(20 + i),	8✔
210	TimeFrameIndex(i)	8✔
211	);
212	}
213
214	// 2. --- EXECUTION ---
215	tracker.process(data_source, group_manager, ground_truth, TimeFrameIndex(0), TimeFrameIndex(4));	1✔
216
217	// 3. --- ASSERTIONS ---
218	// Verify groups exist
219	REQUIRE(group_manager.hasGroup(group1));	1✔
220	REQUIRE(group_manager.hasGroup(group2));	1✔
221
222	// Check that all entities were correctly assigned. Each group should have 5 entities now.
223	REQUIRE(group_manager.getGroupSize(group1) == 5); // 1 (initial) + 4 (assigned)	1✔
224	REQUIRE(group_manager.getGroupSize(group2) == 5);	1✔
225
226	// Verify the correct entities were assigned to group 1
227	std::vector<EntityId> expected_g1 = {1, 11, 12, 13, 14};	3✔
228	auto group1_entities = group_manager.getEntitiesInGroup(group1);	1✔
229	std::sort(group1_entities.begin(), group1_entities.end());	1✔
230	REQUIRE(group1_entities == expected_g1);	1✔
231
232	// Verify the correct entities were assigned to group 2
233	std::vector<EntityId> expected_g2 = {2, 21, 22, 23, 24};	3✔
234	auto group2_entities = group_manager.getEntitiesInGroup(group2);	1✔
235	std::sort(group2_entities.begin(), group2_entities.end());	1✔
236	REQUIRE(group2_entities == expected_g2);	1✔
237	}	2✔

paulmthompson / WhiskerToolbox / 18179930748

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