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

85.71

/src/StateEstimation/Features/CompositeFeatureExtractor.hpp

#ifndef STATE_ESTIMATION_COMPOSITE_FEATURE_EXTRACTOR_HPP
#define STATE_ESTIMATION_COMPOSITE_FEATURE_EXTRACTOR_HPP

#include "IFeatureExtractor.hpp"

#include <Eigen/Dense>
#include <memory>
#include <string>
#include <vector>

namespace StateEstimation {

/**
 * @brief Feature extractor that chains multiple extractors together
 * 
 * This extractor allows combining multiple feature extractors to produce
 * a concatenated feature vector. The extractors are applied in the order
 * they are added, and their outputs are concatenated together.
 * 
 * The composite respects each feature's temporal behavior metadata:
 * - KINEMATIC_2D features: 2D measurement → 4D state (position + velocity)
 * - STATIC features: 1D measurement → 1D state (no velocity)
 * - SCALAR_DYNAMIC features: 1D measurement → 2D state (value + derivative)
 * 
 * Example: Combining centroid (KINEMATIC_2D) + length (STATIC):
 *   Measurements: [x_centroid, y_centroid, length] (3D)
 *   State: [x, y, vx, vy, length] (5D)
 * 
 * The initial state is constructed by concatenating the states from each extractor
 * and combining their covariances into a block-diagonal matrix.
 * 
 * @tparam DataType The raw data type to extract features from (e.g., Line2D)
 */
template<typename DataType>
class CompositeFeatureExtractor : public IFeatureExtractor<DataType> {
public:
    /**
     * @brief Construct an empty composite extractor
     */
    CompositeFeatureExtractor() = default;
    
    /**
     * @brief Construct a composite extractor from a list of extractors
     * 
     * @param extractors Vector of unique_ptrs to feature extractors (ownership transferred)
     */
    explicit CompositeFeatureExtractor(std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> extractors)
        : extractors_(std::move(extractors)) {}
    
    /**
     * @brief Add a feature extractor to the chain
     * 
     * Extractors are applied in the order they are added.
     * 
     * @param extractor Unique_ptr to the extractor to add (ownership transferred)
     */
    void addExtractor(std::unique_ptr<IFeatureExtractor<DataType>> extractor) {
        extractors_.push_back(std::move(extractor));
    }
    
    /**
     * @brief Extract concatenated filter features from all extractors
     * 
     * Applies each extractor in order and concatenates their outputs.
     * 
     * @param data The raw data object to extract features from
     * @return Concatenated feature vector from all extractors
     */
    Eigen::VectorXd getFilterFeatures(DataType const& data) const override {
        if (extractors_.empty()) {
            return Eigen::VectorXd(0);
        }
        
        // Calculate total size needed
        int total_size = 0;
        for (auto const& extractor : extractors_) {
            total_size += extractor->getFilterFeatures(data).size();
        }
        
        // Concatenate features
        Eigen::VectorXd result(total_size);
        int offset = 0;
        for (auto const& extractor : extractors_) {
            Eigen::VectorXd features = extractor->getFilterFeatures(data);
            result.segment(offset, features.size()) = features;
            offset += features.size();
        }
        
        return result;
    }
    
    /**
     * @brief Extract all features from all extractors
     * 
     * Creates a unified cache with features from all extractors.
     * The composite feature is stored under "composite_features".
     * Individual extractor features are also stored under their original names.
     * 
     * @param data The raw data object to extract features from
     * @return FeatureCache with all features from all extractors
     */
    FeatureCache getAllFeatures(DataType const& data) const override {
        FeatureCache cache;
        
        // Add the composite filter features
        cache[getFilterFeatureName()] = getFilterFeatures(data);
        
        // Add individual extractor features
        for (auto const& extractor : extractors_) {
            auto extractor_cache = extractor->getAllFeatures(data);
            for (auto const& [key, value] : extractor_cache) {
                cache[key] = value;
            }
        }
        
        return cache;
    }
    
    /**
     * @brief Get the name identifier for composite filter features
     * 
     * @return "composite_features"
     */
    std::string getFilterFeatureName() const override {
        return "composite_features";
    }
    
    /**
     * @brief Create initial filter state from first observation
     * 
     * Concatenates the initial states from all extractors and creates
     * a block-diagonal covariance matrix.
     * 
     * Example with 2 extractors (each 4D state: [x, y, vx, vy]):
     *   Result state: [x1, y1, vx1, vy1, x2, y2, vx2, vy2] (8D)
     *   Result covariance: Block diagonal with 4×4 blocks from each extractor
     * 
     * @param data The raw data object to initialize from
     * @return FilterState with concatenated state and block-diagonal covariance
     */
    FilterState getInitialState(DataType const& data) const override {
        if (extractors_.empty()) {
            return FilterState{
                .state_mean = Eigen::VectorXd(0),
                .state_covariance = Eigen::MatrixXd(0, 0)
            };
        }
        
        // Get initial states from all extractors
        std::vector<FilterState> individual_states;
        int total_state_size = 0;
        
        for (auto const& extractor : extractors_) {
            auto state = extractor->getInitialState(data);
            individual_states.push_back(state);
            total_state_size += state.state_mean.size();
        }
        
        // Concatenate state means
        Eigen::VectorXd combined_mean(total_state_size);
        int offset = 0;
        for (auto const& state : individual_states) {
            int size = state.state_mean.size();
            combined_mean.segment(offset, size) = state.state_mean;
            offset += size;
        }
        
        // Create block-diagonal covariance matrix
        Eigen::MatrixXd combined_cov = Eigen::MatrixXd::Zero(total_state_size, total_state_size);
        offset = 0;
        for (auto const& state : individual_states) {
            int size = state.state_covariance.rows();
            combined_cov.block(offset, offset, size, size) = state.state_covariance;
            offset += size;
        }
        
        return FilterState{
            .state_mean = combined_mean,
            .state_covariance = combined_cov
        };
    }
    
    /**
     * @brief Clone this composite extractor
     * 
     * Creates a deep copy with cloned versions of all child extractors.
     * 
     * @return A unique_ptr to a copy of this extractor
     */
    std::unique_ptr<IFeatureExtractor<DataType>> clone() const override {
        std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> cloned_extractors;
        for (auto const& extractor : extractors_) {
            cloned_extractors.push_back(extractor->clone());
        }
        return std::make_unique<CompositeFeatureExtractor<DataType>>(std::move(cloned_extractors));
    }
    
    /**
     * @brief Get the number of extractors in this composite
     * 
     * @return Number of child extractors
     */
    size_t getExtractorCount() const {
        return extractors_.size();
    }
    
    /**
     * @brief Get metadata for the composite feature
     * 
     * Creates aggregate metadata by combining information from all child extractors.
     * The measurement size is the sum of all child measurement sizes.
     * The state size is the sum of all child state sizes.
     * Type is marked as CUSTOM since it's a composition of multiple types.
     * 
     * @return FeatureMetadata describing the composite
     */
    FeatureMetadata getMetadata() const override {
        int total_measurement_size = 0;
        int total_state_size = 0;
        
        for (auto const& extractor : extractors_) {
            auto metadata = extractor->getMetadata();
            total_measurement_size += metadata.measurement_size;
            total_state_size += metadata.state_size;
        }
        
        return FeatureMetadata{
            .name = "composite_features",
            .measurement_size = total_measurement_size,
            .state_size = total_state_size,
            .temporal_type = FeatureTemporalType::CUSTOM
        };
    }
    
    /**
     * @brief Get metadata for all child extractors
     * 
     * Useful for building Kalman matrices with proper structure.
     * 
     * @return Vector of metadata from each child extractor in order
     */
    std::vector<FeatureMetadata> getChildMetadata() const {
        std::vector<FeatureMetadata> metadata_list;
        for (auto const& extractor : extractors_) {
            metadata_list.push_back(extractor->getMetadata());
        }
        return metadata_list;
    }
    
private:
    std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> extractors_;
};

} // namespace StateEstimation

#endif // STATE_ESTIMATION_COMPOSITE_FEATURE_EXTRACTOR_HPP

1	#ifndef STATE_ESTIMATION_COMPOSITE_FEATURE_EXTRACTOR_HPP
2	#define STATE_ESTIMATION_COMPOSITE_FEATURE_EXTRACTOR_HPP
3
4	#include "IFeatureExtractor.hpp"
5
6	#include <Eigen/Dense>
7	#include <memory>
8	#include <string>
9	#include <vector>
10
11	namespace StateEstimation {
12
13	/**
14	* @brief Feature extractor that chains multiple extractors together
15	*
16	* This extractor allows combining multiple feature extractors to produce
17	* a concatenated feature vector. The extractors are applied in the order
18	* they are added, and their outputs are concatenated together.
19	*
20	* The composite respects each feature's temporal behavior metadata:
21	* - KINEMATIC_2D features: 2D measurement → 4D state (position + velocity)
22	* - STATIC features: 1D measurement → 1D state (no velocity)
23	* - SCALAR_DYNAMIC features: 1D measurement → 2D state (value + derivative)
24	*
25	* Example: Combining centroid (KINEMATIC_2D) + length (STATIC):
26	* Measurements: [x_centroid, y_centroid, length] (3D)
27	* State: [x, y, vx, vy, length] (5D)
28	*
29	* The initial state is constructed by concatenating the states from each extractor
30	* and combining their covariances into a block-diagonal matrix.
31	*
32	* @tparam DataType The raw data type to extract features from (e.g., Line2D)
33	*/
34	template<typename DataType>
35	class CompositeFeatureExtractor : public IFeatureExtractor<DataType> {
36	public:
37	/**
38	* @brief Construct an empty composite extractor
39	*/
40	CompositeFeatureExtractor() = default;	5✔
41
42	/**
43	* @brief Construct a composite extractor from a list of extractors
44	*
45	* @param extractors Vector of unique_ptrs to feature extractors (ownership transferred)
46	*/
47	explicit CompositeFeatureExtractor(std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> extractors)	1✔
48	: extractors_(std::move(extractors)) {}	1✔
49
50	/**
51	* @brief Add a feature extractor to the chain
52	*
53	* Extractors are applied in the order they are added.
54	*
55	* @param extractor Unique_ptr to the extractor to add (ownership transferred)
56	*/
57	void addExtractor(std::unique_ptr<IFeatureExtractor<DataType>> extractor) {	18✔
58	extractors_.push_back(std::move(extractor));	18✔
59	}	18✔
60
61	/**
62	* @brief Extract concatenated filter features from all extractors
63	*
64	* Applies each extractor in order and concatenates their outputs.
65	*
66	* @param data The raw data object to extract features from
67	* @return Concatenated feature vector from all extractors
68	*/
69	Eigen::VectorXd getFilterFeatures(DataType const& data) const override {	1,717✔
70	if (extractors_.empty()) {	1,717✔
71	return Eigen::VectorXd(0);	2✔
72	}
73
74	// Calculate total size needed
75	int total_size = 0;	1,716✔
76	for (auto const& extractor : extractors_) {	5,148✔
77	total_size += extractor->getFilterFeatures(data).size();	3,432✔
78	}
79
80	// Concatenate features
81	Eigen::VectorXd result(total_size);	1,716✔
82	int offset = 0;	1,716✔
83	for (auto const& extractor : extractors_) {	5,148✔
84	Eigen::VectorXd features = extractor->getFilterFeatures(data);	3,432✔
85	result.segment(offset, features.size()) = features;	3,432✔
86	offset += features.size();	3,432✔
87	}
88
89	return result;	1,716✔
90	}	1,716✔
91
92	/**
93	* @brief Extract all features from all extractors
94	*
95	* Creates a unified cache with features from all extractors.
96	* The composite feature is stored under "composite_features".
97	* Individual extractor features are also stored under their original names.
98	*
99	* @param data The raw data object to extract features from
100	* @return FeatureCache with all features from all extractors
101	*/
102	FeatureCache getAllFeatures(DataType const& data) const override {	921✔
103	FeatureCache cache;	921✔
104
105	// Add the composite filter features
106	cache[getFilterFeatureName()] = getFilterFeatures(data);	921✔
107
108	// Add individual extractor features
109	for (auto const& extractor : extractors_) {	4,605✔
110	auto extractor_cache = extractor->getAllFeatures(data);	1,842✔
111	for (auto const& [key, value] : extractor_cache) {	3,684✔
112	cache[key] = value;	1,842✔
113	}
114	}
115
116	return cache;	921✔
117	}	×
118
119	/**
120	* @brief Get the name identifier for composite filter features
121	*
122	* @return "composite_features"
123	*/
124	std::string getFilterFeatureName() const override {	921✔
125	return "composite_features";	2,763✔
126	}
127
128	/**
129	* @brief Create initial filter state from first observation
130	*
131	* Concatenates the initial states from all extractors and creates
132	* a block-diagonal covariance matrix.
133	*
134	* Example with 2 extractors (each 4D state: [x, y, vx, vy]):
135	* Result state: [x1, y1, vx1, vy1, x2, y2, vx2, vy2] (8D)
136	* Result covariance: Block diagonal with 4×4 blocks from each extractor
137	*
138	* @param data The raw data object to initialize from
139	* @return FilterState with concatenated state and block-diagonal covariance
140	*/
141	FilterState getInitialState(DataType const& data) const override {	10✔
142	if (extractors_.empty()) {	10✔
143	return FilterState{
144	.state_mean = Eigen::VectorXd(0),	1✔
145	.state_covariance = Eigen::MatrixXd(0, 0)	1✔
146	};	1✔
147	}
148
149	// Get initial states from all extractors
150	std::vector<FilterState> individual_states;	9✔
151	int total_state_size = 0;	9✔
152
153	for (auto const& extractor : extractors_) {	27✔
154	auto state = extractor->getInitialState(data);	18✔
155	individual_states.push_back(state);	18✔
156	total_state_size += state.state_mean.size();	18✔
157	}
158
159	// Concatenate state means
160	Eigen::VectorXd combined_mean(total_state_size);	9✔
161	int offset = 0;	9✔
162	for (auto const& state : individual_states) {	27✔
163	int size = state.state_mean.size();	18✔
164	combined_mean.segment(offset, size) = state.state_mean;	18✔
165	offset += size;	18✔
166	}
167
168	// Create block-diagonal covariance matrix
169	Eigen::MatrixXd combined_cov = Eigen::MatrixXd::Zero(total_state_size, total_state_size);	9✔
170	offset = 0;	9✔
171	for (auto const& state : individual_states) {	27✔
172	int size = state.state_covariance.rows();	18✔
173	combined_cov.block(offset, offset, size, size) = state.state_covariance;	18✔
174	offset += size;	18✔
175	}
176
177	return FilterState{
178	.state_mean = combined_mean,
179	.state_covariance = combined_cov
180	};	9✔
181	}	10✔
182
183	/**
184	* @brief Clone this composite extractor
185	*
186	* Creates a deep copy with cloned versions of all child extractors.
187	*
188	* @return A unique_ptr to a copy of this extractor
189	*/
190	std::unique_ptr<IFeatureExtractor<DataType>> clone() const override {	1✔
191	std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> cloned_extractors;	1✔
192	for (auto const& extractor : extractors_) {	3✔
193	cloned_extractors.push_back(extractor->clone());	2✔
194	}
195	return std::make_unique<CompositeFeatureExtractor<DataType>>(std::move(cloned_extractors));	2✔
196	}	1✔
197
198	/**
199	* @brief Get the number of extractors in this composite
200	*
201	* @return Number of child extractors
202	*/
203	size_t getExtractorCount() const {
204	return extractors_.size();
205	}
206
207	/**
208	* @brief Get metadata for the composite feature
209	*
210	* Creates aggregate metadata by combining information from all child extractors.
211	* The measurement size is the sum of all child measurement sizes.
212	* The state size is the sum of all child state sizes.
213	* Type is marked as CUSTOM since it's a composition of multiple types.
214	*
215	* @return FeatureMetadata describing the composite
216	*/
NEW 217	FeatureMetadata getMetadata() const override {	×
NEW 218	int total_measurement_size = 0;	×
NEW 219	int total_state_size = 0;	×
220
NEW 221	for (auto const& extractor : extractors_) {	×
NEW 222	auto metadata = extractor->getMetadata();	×
NEW 223	total_measurement_size += metadata.measurement_size;	×
NEW 224	total_state_size += metadata.state_size;	×
225	}
226
227	return FeatureMetadata{
228	.name = "composite_features",
229	.measurement_size = total_measurement_size,
230	.state_size = total_state_size,
231	.temporal_type = FeatureTemporalType::CUSTOM
NEW 232	};	×
NEW 233	}	×
234
235	/**
236	* @brief Get metadata for all child extractors
237	*
238	* Useful for building Kalman matrices with proper structure.
239	*
240	* @return Vector of metadata from each child extractor in order
241	*/
242	std::vector<FeatureMetadata> getChildMetadata() const {	5✔
243	std::vector<FeatureMetadata> metadata_list;	5✔
244	for (auto const& extractor : extractors_) {	15✔
245	metadata_list.push_back(extractor->getMetadata());	10✔
246	}
247	return metadata_list;	5✔
NEW 248	}	×
249
250	private:
251	std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> extractors_;
252	};
253
254	} // namespace StateEstimation
255
256	#endif // STATE_ESTIMATION_COMPOSITE_FEATURE_EXTRACTOR_HPP

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