18389801194

Committed 09 Oct 2025 09:35PM UTC coverage: 71.943% (+0.1%) from 71.826%

Build # 18389801194

Build Type

push

github

Committed by

paulmthompson

Commit Message

add correlation matrix to filtering interface

Run Details

207 of 337 new or added lines in 5 files covered. (61.42%)

867 existing lines in 31 files now uncovered.

49964 of 69449 relevant lines covered (71.94%)

1103.53 hits per line

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

93.75

/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 <cmath>
#include <map>
#include <memory>
#include <string>
#include <utility>
#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;
    }
    
    /**
     * @brief Configuration for cross-feature covariance in initial state
     * 
     * Allows modeling correlations between different features, e.g., when
     * a static feature (length) correlates with position due to measurement
     * artifacts like camera clipping.
     */
    struct CrossCovarianceConfig {
        /// Correlation coefficient between features (-1 to 1)
        /// Example: position-length correlation when camera clips
        std::map<std::pair<int, int>, double> feature_correlations;
        
        /// State-level covariance entries (for fine-grained control)
        /// Maps (state_index_1, state_index_2) -> covariance value
        std::map<std::pair<int, int>, double> state_covariances;
    };
    
    /**
     * @brief Set cross-feature covariance configuration
     * 
     * This allows the initial state covariance to include off-diagonal terms
     * modeling known correlations between features.
     * 
     * Example: If position (feature 0) affects measured length (feature 2):
     *   config.feature_correlations[{0, 2}] = 0.3;  // 30% correlation
     * 
     * @param config Cross-covariance configuration
     */
    void setCrossCovarianceConfig(CrossCovarianceConfig config) {
        cross_cov_config_ = std::move(config);
    }
    
    /**
     * @brief Create initial filter state with optional cross-feature covariance
     * 
     * Extends the base implementation to add cross-feature covariance terms
     * based on the configured correlations. This allows modeling cases where
     * features are statistically dependent, such as when camera clipping causes
     * measured length to correlate with position.
     * 
     * @param data The raw data object to initialize from
     * @return FilterState with full covariance (including off-diagonal terms)
     */
    FilterState getInitialStateWithCrossCovariance(DataType const& data) const {
        // Get base state with block-diagonal covariance
        FilterState base_state = getInitialState(data);
        
        if (cross_cov_config_.feature_correlations.empty() && 
            cross_cov_config_.state_covariances.empty()) {
            return base_state;  // No cross-covariance requested
        }
        
        // Apply cross-feature correlations
        auto metadata_list = getChildMetadata();
        std::vector<int> feature_state_offsets;
        int offset = 0;
        for (auto const& meta : metadata_list) {
            feature_state_offsets.push_back(offset);
            offset += meta.state_size;
        }
        
        // Add feature-level correlations (position components of different features)
        for (auto const& [feature_pair, correlation] : cross_cov_config_.feature_correlations) {
            int feat_i = feature_pair.first;
            int feat_j = feature_pair.second;
            
            if (feat_i >= static_cast<int>(metadata_list.size()) || 
                feat_j >= static_cast<int>(metadata_list.size())) {
                continue;  // Invalid feature indices
            }
            
            auto const& meta_i = metadata_list[feat_i];
            auto const& meta_j = metadata_list[feat_j];
            
            int offset_i = feature_state_offsets[feat_i];
            int offset_j = feature_state_offsets[feat_j];
            
            // Apply correlation to position components
            // For KINEMATIC features: position is first components
            // For STATIC features: the value itself is the position
            int pos_dim_i = (meta_i.temporal_type == FeatureTemporalType::KINEMATIC_2D) ? 2 : meta_i.measurement_size;
            int pos_dim_j = (meta_j.temporal_type == FeatureTemporalType::KINEMATIC_2D) ? 2 : meta_j.measurement_size;
            
            for (int pi = 0; pi < pos_dim_i; ++pi) {
                for (int pj = 0; pj < pos_dim_j; ++pj) {
                    int si = offset_i + pi;
                    int sj = offset_j + pj;
                    
                    // Covariance = correlation * sqrt(var_i * var_j)
                    double std_i = std::sqrt(base_state.state_covariance(si, si));
                    double std_j = std::sqrt(base_state.state_covariance(sj, sj));
                    double cov = correlation * std_i * std_j;
                    
                    base_state.state_covariance(si, sj) = cov;
                    base_state.state_covariance(sj, si) = cov;  // Symmetric
                }
            }
        }
        
        // Add explicit state-level covariances
        for (auto const& [state_pair, cov_value] : cross_cov_config_.state_covariances) {
            int si = state_pair.first;
            int sj = state_pair.second;
            
            if (si < base_state.state_covariance.rows() && 
                sj < base_state.state_covariance.cols()) {
                base_state.state_covariance(si, sj) = cov_value;
                base_state.state_covariance(sj, si) = cov_value;  // Symmetric
            }
        }
        
        return base_state;
    }
    
private:
    std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> extractors_;
    CrossCovarianceConfig cross_cov_config_;
};

} // 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 <cmath>
8	#include <map>
9	#include <memory>
10	#include <string>
11	#include <utility>
12	#include <vector>
13
14	namespace StateEstimation {
15
16	/**
17	* @brief Feature extractor that chains multiple extractors together
18	*
19	* This extractor allows combining multiple feature extractors to produce
20	* a concatenated feature vector. The extractors are applied in the order
21	* they are added, and their outputs are concatenated together.
22	*
23	* The composite respects each feature's temporal behavior metadata:
24	* - KINEMATIC_2D features: 2D measurement → 4D state (position + velocity)
25	* - STATIC features: 1D measurement → 1D state (no velocity)
26	* - SCALAR_DYNAMIC features: 1D measurement → 2D state (value + derivative)
27	*
28	* Example: Combining centroid (KINEMATIC_2D) + length (STATIC):
29	* Measurements: [x_centroid, y_centroid, length] (3D)
30	* State: [x, y, vx, vy, length] (5D)
31	*
32	* The initial state is constructed by concatenating the states from each extractor
33	* and combining their covariances into a block-diagonal matrix.
34	*
35	* @tparam DataType The raw data type to extract features from (e.g., Line2D)
36	*/
37	template<typename DataType>
38	class CompositeFeatureExtractor : public IFeatureExtractor<DataType> {
39	public:
40	/**
41	* @brief Construct an empty composite extractor
42	*/
43	CompositeFeatureExtractor() = default;	16✔
44
45	/**
46	* @brief Construct a composite extractor from a list of extractors
47	*
48	* @param extractors Vector of unique_ptrs to feature extractors (ownership transferred)
49	*/
50	explicit CompositeFeatureExtractor(std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> extractors)	1✔
51	: extractors_(std::move(extractors)) {}	1✔
52
53	/**
54	* @brief Add a feature extractor to the chain
55	*
56	* Extractors are applied in the order they are added.
57	*
58	* @param extractor Unique_ptr to the extractor to add (ownership transferred)
59	*/
60	void addExtractor(std::unique_ptr<IFeatureExtractor<DataType>> extractor) {	39✔
61	extractors_.push_back(std::move(extractor));	39✔
62	}	39✔
63
64	/**
65	* @brief Extract concatenated filter features from all extractors
66	*
67	* Applies each extractor in order and concatenates their outputs.
68	*
69	* @param data The raw data object to extract features from
70	* @return Concatenated feature vector from all extractors
71	*/
72	Eigen::VectorXd getFilterFeatures(DataType const& data) const override {	7,184✔
73	if (extractors_.empty()) {	7,184✔
74	return Eigen::VectorXd(0);	2✔
75	}
76
77	// Calculate total size needed
78	int total_size = 0;	7,183✔
79	for (auto const& extractor : extractors_) {	28,727✔
80	total_size += extractor->getFilterFeatures(data).size();	21,544✔
81	}
82
83	// Concatenate features
84	Eigen::VectorXd result(total_size);	7,183✔
85	int offset = 0;	7,183✔
86	for (auto const& extractor : extractors_) {	28,727✔
87	Eigen::VectorXd features = extractor->getFilterFeatures(data);	21,544✔
88	result.segment(offset, features.size()) = features;	21,544✔
89	offset += features.size();	21,544✔
90	}
91
92	return result;	7,183✔
93	}	7,183✔
94
95	/**
96	* @brief Extract all features from all extractors
97	*
98	* Creates a unified cache with features from all extractors.
99	* The composite feature is stored under "composite_features".
100	* Individual extractor features are also stored under their original names.
101	*
102	* @param data The raw data object to extract features from
103	* @return FeatureCache with all features from all extractors
104	*/
105	FeatureCache getAllFeatures(DataType const& data) const override {	1✔
106	FeatureCache cache;	1✔
107
108	// Add the composite filter features
109	cache[getFilterFeatureName()] = getFilterFeatures(data);	1✔
110
111	// Add individual extractor features
112	for (auto const& extractor : extractors_) {	5✔
113	auto extractor_cache = extractor->getAllFeatures(data);	2✔
114	for (auto const& [key, value] : extractor_cache) {	4✔
115	cache[key] = value;	2✔
116	}
117	}
118
119	return cache;	1✔
120	}	×
121
122	/**
123	* @brief Get the name identifier for composite filter features
124	*
125	* @return "composite_features"
126	*/
127	std::string getFilterFeatureName() const override {	1✔
128	return "composite_features";	3✔
129	}
130
131	/**
132	* @brief Create initial filter state from first observation
133	*
134	* Concatenates the initial states from all extractors and creates
135	* a block-diagonal covariance matrix.
136	*
137	* Example with 2 extractors (each 4D state: [x, y, vx, vy]):
138	* Result state: [x1, y1, vx1, vy1, x2, y2, vx2, vy2] (8D)
139	* Result covariance: Block diagonal with 4×4 blocks from each extractor
140	*
141	* @param data The raw data object to initialize from
142	* @return FilterState with concatenated state and block-diagonal covariance
143	*/
144	FilterState getInitialState(DataType const& data) const override {	573✔
145	if (extractors_.empty()) {	573✔
146	return FilterState{
147	.state_mean = Eigen::VectorXd(0),	1✔
148	.state_covariance = Eigen::MatrixXd(0, 0)	1✔
149	};	1✔
150	}
151
152	// Get initial states from all extractors
153	std::vector<FilterState> individual_states;	572✔
154	int total_state_size = 0;	572✔
155
156	for (auto const& extractor : extractors_) {	2,286✔
157	auto state = extractor->getInitialState(data);	1,714✔
158	individual_states.push_back(state);	1,714✔
159	total_state_size += state.state_mean.size();	1,714✔
160	}
161
162	// Concatenate state means
163	Eigen::VectorXd combined_mean(total_state_size);	572✔
164	int offset = 0;	572✔
165	for (auto const& state : individual_states) {	2,286✔
166	int size = state.state_mean.size();	1,714✔
167	combined_mean.segment(offset, size) = state.state_mean;	1,714✔
168	offset += size;	1,714✔
169	}
170
171	// Create block-diagonal covariance matrix
172	Eigen::MatrixXd combined_cov = Eigen::MatrixXd::Zero(total_state_size, total_state_size);	572✔
173	offset = 0;	572✔
174	for (auto const& state : individual_states) {	2,286✔
175	int size = state.state_covariance.rows();	1,714✔
176	combined_cov.block(offset, offset, size, size) = state.state_covariance;	1,714✔
177	offset += size;	1,714✔
178	}
179
180	return FilterState{
181	.state_mean = combined_mean,
182	.state_covariance = combined_cov
183	};	572✔
184	}	573✔
185
186	/**
187	* @brief Clone this composite extractor
188	*
189	* Creates a deep copy with cloned versions of all child extractors.
190	*
191	* @return A unique_ptr to a copy of this extractor
192	*/
193	std::unique_ptr<IFeatureExtractor<DataType>> clone() const override {	1✔
194	std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> cloned_extractors;	1✔
195	for (auto const& extractor : extractors_) {	3✔
196	cloned_extractors.push_back(extractor->clone());	2✔
197	}
198	return std::make_unique<CompositeFeatureExtractor<DataType>>(std::move(cloned_extractors));	2✔
199	}	1✔
200
201	/**
202	* @brief Get the number of extractors in this composite
203	*
204	* @return Number of child extractors
205	*/
206	size_t getExtractorCount() const {
207	return extractors_.size();
208	}
209
210	/**
211	* @brief Get metadata for the composite feature
212	*
213	* Creates aggregate metadata by combining information from all child extractors.
214	* The measurement size is the sum of all child measurement sizes.
215	* The state size is the sum of all child state sizes.
216	* Type is marked as CUSTOM since it's a composition of multiple types.
217	*
218	* @return FeatureMetadata describing the composite
219	*/
220	FeatureMetadata getMetadata() const override {	1✔
221	int total_measurement_size = 0;	1✔
222	int total_state_size = 0;	1✔
223
224	for (auto const& extractor : extractors_) {	3✔
225	auto metadata = extractor->getMetadata();	2✔
226	total_measurement_size += metadata.measurement_size;	2✔
227	total_state_size += metadata.state_size;	2✔
228	}
229
230	return FeatureMetadata{
231	.name = "composite_features",
232	.measurement_size = total_measurement_size,
233	.state_size = total_state_size,
234	.temporal_type = FeatureTemporalType::CUSTOM
235	};	1✔
236	}	3✔
237
238	/**
239	* @brief Get metadata for all child extractors
240	*
241	* Useful for building Kalman matrices with proper structure.
242	*
243	* @return Vector of metadata from each child extractor in order
244	*/
245	std::vector<FeatureMetadata> getChildMetadata() const {	10✔
246	std::vector<FeatureMetadata> metadata_list;	10✔
247	for (auto const& extractor : extractors_) {	39✔
248	metadata_list.push_back(extractor->getMetadata());	29✔
249	}
250	return metadata_list;	10✔
251	}	×
252
253	/**
254	* @brief Configuration for cross-feature covariance in initial state
255	*
256	* Allows modeling correlations between different features, e.g., when
257	* a static feature (length) correlates with position due to measurement
258	* artifacts like camera clipping.
259	*/
260	struct CrossCovarianceConfig {
261	/// Correlation coefficient between features (-1 to 1)
262	/// Example: position-length correlation when camera clips
263	std::map<std::pair<int, int>, double> feature_correlations;
264
265	/// State-level covariance entries (for fine-grained control)
266	/// Maps (state_index_1, state_index_2) -> covariance value
267	std::map<std::pair<int, int>, double> state_covariances;
268	};
269
270	/**
271	* @brief Set cross-feature covariance configuration
272	*
273	* This allows the initial state covariance to include off-diagonal terms
274	* modeling known correlations between features.
275	*
276	* Example: If position (feature 0) affects measured length (feature 2):
277	* config.feature_correlations[{0, 2}] = 0.3; // 30% correlation
278	*
279	* @param config Cross-covariance configuration
280	*/
NEW 281	void setCrossCovarianceConfig(CrossCovarianceConfig config) {	×
NEW 282	cross_cov_config_ = std::move(config);	×
NEW 283	}	×
284
285	/**
286	* @brief Create initial filter state with optional cross-feature covariance
287	*
288	* Extends the base implementation to add cross-feature covariance terms
289	* based on the configured correlations. This allows modeling cases where
290	* features are statistically dependent, such as when camera clipping causes
291	* measured length to correlate with position.
292	*
293	* @param data The raw data object to initialize from
294	* @return FilterState with full covariance (including off-diagonal terms)
295	*/
296	FilterState getInitialStateWithCrossCovariance(DataType const& data) const {
297	// Get base state with block-diagonal covariance
298	FilterState base_state = getInitialState(data);
299
300	if (cross_cov_config_.feature_correlations.empty() &&
301	cross_cov_config_.state_covariances.empty()) {
302	return base_state; // No cross-covariance requested
303	}
304
305	// Apply cross-feature correlations
306	auto metadata_list = getChildMetadata();
307	std::vector<int> feature_state_offsets;
308	int offset = 0;
309	for (auto const& meta : metadata_list) {
310	feature_state_offsets.push_back(offset);
311	offset += meta.state_size;
312	}
313
314	// Add feature-level correlations (position components of different features)
315	for (auto const& [feature_pair, correlation] : cross_cov_config_.feature_correlations) {
316	int feat_i = feature_pair.first;
317	int feat_j = feature_pair.second;
318
319	if (feat_i >= static_cast<int>(metadata_list.size()) \|\|
320	feat_j >= static_cast<int>(metadata_list.size())) {
321	continue; // Invalid feature indices
322	}
323
324	auto const& meta_i = metadata_list[feat_i];
325	auto const& meta_j = metadata_list[feat_j];
326
327	int offset_i = feature_state_offsets[feat_i];
328	int offset_j = feature_state_offsets[feat_j];
329
330	// Apply correlation to position components
331	// For KINEMATIC features: position is first components
332	// For STATIC features: the value itself is the position
333	int pos_dim_i = (meta_i.temporal_type == FeatureTemporalType::KINEMATIC_2D) ? 2 : meta_i.measurement_size;
334	int pos_dim_j = (meta_j.temporal_type == FeatureTemporalType::KINEMATIC_2D) ? 2 : meta_j.measurement_size;
335
336	for (int pi = 0; pi < pos_dim_i; ++pi) {
337	for (int pj = 0; pj < pos_dim_j; ++pj) {
338	int si = offset_i + pi;
339	int sj = offset_j + pj;
340
341	// Covariance = correlation * sqrt(var_i * var_j)
342	double std_i = std::sqrt(base_state.state_covariance(si, si));
343	double std_j = std::sqrt(base_state.state_covariance(sj, sj));
344	double cov = correlation * std_i * std_j;
345
346	base_state.state_covariance(si, sj) = cov;
347	base_state.state_covariance(sj, si) = cov; // Symmetric
348	}
349	}
350	}
351
352	// Add explicit state-level covariances
353	for (auto const& [state_pair, cov_value] : cross_cov_config_.state_covariances) {
354	int si = state_pair.first;
355	int sj = state_pair.second;
356
357	if (si < base_state.state_covariance.rows() &&
358	sj < base_state.state_covariance.cols()) {
359	base_state.state_covariance(si, sj) = cov_value;
360	base_state.state_covariance(sj, si) = cov_value; // Symmetric
361	}
362	}
363
364	return base_state;
365	}
366
367	private:
368	std::vector<std::unique_ptr<IFeatureExtractor<DataType>>> extractors_;
369	CrossCovarianceConfig cross_cov_config_;
370	};
371
372	} // namespace StateEstimation
373
374	#endif // STATE_ESTIMATION_COMPOSITE_FEATURE_EXTRACTOR_HPP

paulmthompson / WhiskerToolbox / 18389801194

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