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

70.27

/src/StateEstimation/Features/LineLengthExtractor.hpp

#ifndef STATE_ESTIMATION_LINE_LENGTH_EXTRACTOR_HPP
#define STATE_ESTIMATION_LINE_LENGTH_EXTRACTOR_HPP

#include "IFeatureExtractor.hpp"
#include "CoreGeometry/lines.hpp"

#include <Eigen/Dense>
#include <cmath>

namespace StateEstimation {

/**
 * @brief Feature extractor that computes line length
 * 
 * This extractor computes the total arc length of a line by summing
 * the Euclidean distances between consecutive points. Line length is
 * typically time-invariant or slowly varying, so it's treated as a
 * STATIC feature with no velocity tracking.
 * 
 * Features returned:
 * - Filter features: [length] (1D scalar)
 * - Initial state: [length] (1D, no velocity component)
 */
class LineLengthExtractor : public IFeatureExtractor<Line2D> {
public:
    /**
     * @brief Extract line length for Kalman filtering
     * 
     * Computes the total arc length of the line.
     * 
     * @param line The Line2D to extract features from (zero-copy reference)
     * @return 1D vector containing [length]
     */
    Eigen::VectorXd getFilterFeatures(Line2D const& line) const override {
        double length = computeLength(line);
        Eigen::VectorXd features(1);
        features << length;
        return features;
    }
    
    /**
     * @brief Extract all available features for assignment
     * 
     * For this extractor, we only compute length, so this returns
     * the same features as getFilterFeatures() but wrapped in a cache.
     * 
     * @param line The Line2D to extract features from (zero-copy reference)
     * @return FeatureCache with "line_length" feature
     */
    FeatureCache getAllFeatures(Line2D const& line) const override {
        FeatureCache cache;
        cache[getFilterFeatureName()] = getFilterFeatures(line);
        return cache;
    }
    
    /**
     * @brief Get the name identifier for filter features
     * 
     * @return "line_length"
     */
    std::string getFilterFeatureName() const override {
        return "line_length";
    }
    
    /**
     * @brief Create initial filter state from first observation
     * 
     * Initializes a 1D state vector [length] with moderate covariance
     * to indicate uncertainty in the length estimate.
     * 
     * @param line The Line2D to initialize from (zero-copy reference)
     * @return FilterState with 1D state (no velocity)
     */
    FilterState getInitialState(Line2D const& line) const override {
        double length = computeLength(line);
        
        Eigen::VectorXd state(1);
        state << length;
        
        Eigen::MatrixXd covariance(1, 1);
        covariance(0, 0) = 25.0;  // Moderate initial uncertainty (stddev ~5 pixels)
        
        return FilterState{
            .state_mean = state,
            .state_covariance = covariance
        };
    }
    
    /**
     * @brief Clone this feature extractor
     * 
     * @return A unique_ptr to a copy of this extractor
     */
    std::unique_ptr<IFeatureExtractor<Line2D>> clone() const override {
        return std::make_unique<LineLengthExtractor>(*this);
    }
    
    /**
     * @brief Get metadata for this feature
     * 
     * Line length is a STATIC feature (time-invariant, no velocity).
     * 
     * @return FeatureMetadata with STATIC type
     */
    FeatureMetadata getMetadata() const override {
        return FeatureMetadata::create("line_length", 1, FeatureTemporalType::STATIC);
    }
    
private:
    /**
     * @brief Compute the arc length of a line
     * 
     * Sums the Euclidean distances between consecutive points.
     * If the line has fewer than 2 points, returns 0.
     * 
     * @param line The Line2D to compute length for
     * @return Total arc length
     */
    static double computeLength(Line2D const& line) {
        if (line.size() < 2) {
            return 0.0;
        }
        
        double total_length = 0.0;
        
        for (size_t i = 1; i < line.size(); ++i) {
            auto const& p1 = line[i - 1];
            auto const& p2 = line[i];
            
            double dx = static_cast<double>(p2.x - p1.x);
            double dy = static_cast<double>(p2.y - p1.y);
            
            total_length += std::sqrt(dx * dx + dy * dy);
        }
        
        return total_length;
    }
};

} // namespace StateEstimation

#endif // STATE_ESTIMATION_LINE_LENGTH_EXTRACTOR_HPP

1	#ifndef STATE_ESTIMATION_LINE_LENGTH_EXTRACTOR_HPP
2	#define STATE_ESTIMATION_LINE_LENGTH_EXTRACTOR_HPP
3
4	#include "IFeatureExtractor.hpp"
5	#include "CoreGeometry/lines.hpp"
6
7	#include <Eigen/Dense>
8	#include <cmath>
9
10	namespace StateEstimation {
11
12	/**
13	* @brief Feature extractor that computes line length
14	*
15	* This extractor computes the total arc length of a line by summing
16	* the Euclidean distances between consecutive points. Line length is
17	* typically time-invariant or slowly varying, so it's treated as a
18	* STATIC feature with no velocity tracking.
19	*
20	* Features returned:
21	* - Filter features: [length] (1D scalar)
22	* - Initial state: [length] (1D, no velocity component)
23	*/
24	class LineLengthExtractor : public IFeatureExtractor<Line2D> {
25	public:
26	/**
27	* @brief Extract line length for Kalman filtering
28	*
29	* Computes the total arc length of the line.
30	*
31	* @param line The Line2D to extract features from (zero-copy reference)
32	* @return 1D vector containing [length]
33	*/
34	Eigen::VectorXd getFilterFeatures(Line2D const& line) const override {	14,378✔
35	double length = computeLength(line);	14,378✔
36	Eigen::VectorXd features(1);	14,378✔
37	features << length;	14,378✔
38	return features;	28,756✔
39	}	×
40
41	/**
42	* @brief Extract all available features for assignment
43	*
44	* For this extractor, we only compute length, so this returns
45	* the same features as getFilterFeatures() but wrapped in a cache.
46	*
47	* @param line The Line2D to extract features from (zero-copy reference)
48	* @return FeatureCache with "line_length" feature
49	*/
UNCOV 50	FeatureCache getAllFeatures(Line2D const& line) const override {	×
UNCOV 51	FeatureCache cache;	×
UNCOV 52	cache[getFilterFeatureName()] = getFilterFeatures(line);	×
UNCOV 53	return cache;	×
54	}	×
55
56	/**
57	* @brief Get the name identifier for filter features
58	*
59	* @return "line_length"
60	*/
UNCOV 61	std::string getFilterFeatureName() const override {	×
UNCOV 62	return "line_length";	×
63	}
64
65	/**
66	* @brief Create initial filter state from first observation
67	*
68	* Initializes a 1D state vector [length] with moderate covariance
69	* to indicate uncertainty in the length estimate.
70	*
71	* @param line The Line2D to initialize from (zero-copy reference)
72	* @return FilterState with 1D state (no velocity)
73	*/
74	FilterState getInitialState(Line2D const& line) const override {	571✔
75	double length = computeLength(line);	571✔
76
77	Eigen::VectorXd state(1);	571✔
78	state << length;	571✔
79
80	Eigen::MatrixXd covariance(1, 1);	571✔
81	covariance(0, 0) = 25.0; // Moderate initial uncertainty (stddev ~5 pixels)	571✔
82
83	return FilterState{	571✔
84	.state_mean = state,
85	.state_covariance = covariance
86	};	1,142✔
87	}	571✔
88
89	/**
90	* @brief Clone this feature extractor
91	*
92	* @return A unique_ptr to a copy of this extractor
93	*/
94	std::unique_ptr<IFeatureExtractor<Line2D>> clone() const override {	×
95	return std::make_unique<LineLengthExtractor>(*this);	×
96	}
97
98	/**
99	* @brief Get metadata for this feature
100	*
101	* Line length is a STATIC feature (time-invariant, no velocity).
102	*
103	* @return FeatureMetadata with STATIC type
104	*/
105	FeatureMetadata getMetadata() const override {	9✔
106	return FeatureMetadata::create("line_length", 1, FeatureTemporalType::STATIC);	27✔
107	}
108
109	private:
110	/**
111	* @brief Compute the arc length of a line
112	*
113	* Sums the Euclidean distances between consecutive points.
114	* If the line has fewer than 2 points, returns 0.
115	*
116	* @param line The Line2D to compute length for
117	* @return Total arc length
118	*/
119	static double computeLength(Line2D const& line) {	14,949✔
120	if (line.size() < 2) {	14,949✔
121	return 0.0;	×
122	}
123
124	double total_length = 0.0;	14,949✔
125
126	for (size_t i = 1; i < line.size(); ++i) {	149,469✔
127	auto const& p1 = line[i - 1];	134,520✔
128	auto const& p2 = line[i];	134,520✔
129
130	double dx = static_cast<double>(p2.x - p1.x);	134,520✔
131	double dy = static_cast<double>(p2.y - p1.y);	134,520✔
132
133	total_length += std::sqrt(dx * dx + dy * dy);	134,520✔
134	}
135
136	return total_length;	14,949✔
137	}
138	};
139
140	} // namespace StateEstimation
141
142	#endif // STATE_ESTIMATION_LINE_LENGTH_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