Skip to main content

rust_robotics_mapping/
gaussian_process.rs

1//! Gaussian Process regression for 2D-to-1D surface mapping.
2
3use nalgebra::{DMatrix, DVector};
4
5use rust_robotics_core::RoboticsError;
6
7/// Configuration for Gaussian Process regression.
8#[derive(Debug, Clone, Copy)]
9pub struct GaussianProcessConfig {
10    /// RBF kernel length scale.
11    pub length_scale: f64,
12    /// Kernel signal variance.
13    pub signal_variance: f64,
14    /// Observation noise variance.
15    pub noise_variance: f64,
16}
17
18impl Default for GaussianProcessConfig {
19    fn default() -> Self {
20        Self {
21            length_scale: 1.0,
22            signal_variance: 1.0,
23            noise_variance: 0.01,
24        }
25    }
26}
27
28/// Trained Gaussian Process model.
29#[derive(Debug, Clone)]
30pub struct GaussianProcessRegressor {
31    config: GaussianProcessConfig,
32    train_xy: Vec<(f64, f64)>,
33    alpha: Option<DVector<f64>>,
34    k_inv: Option<DMatrix<f64>>,
35}
36
37impl GaussianProcessRegressor {
38    pub fn new(config: GaussianProcessConfig) -> Self {
39        Self {
40            config,
41            train_xy: Vec::new(),
42            alpha: None,
43            k_inv: None,
44        }
45    }
46
47    /// Fit GP model from `(x, y, z)` samples.
48    pub fn fit(&mut self, samples: &[(f64, f64, f64)]) -> Result<(), RoboticsError> {
49        if samples.is_empty() {
50            return Err(RoboticsError::InvalidParameter(
51                "GaussianProcess: at least one training sample is required".to_string(),
52            ));
53        }
54        if self.config.length_scale <= 0.0 || self.config.signal_variance <= 0.0 {
55            return Err(RoboticsError::InvalidParameter(
56                "GaussianProcess: length_scale and signal_variance must be positive".to_string(),
57            ));
58        }
59        if self.config.noise_variance <= 0.0 {
60            return Err(RoboticsError::InvalidParameter(
61                "GaussianProcess: noise_variance must be positive".to_string(),
62            ));
63        }
64
65        let n = samples.len();
66        self.train_xy = samples.iter().map(|(x, y, _)| (*x, *y)).collect();
67        let z = DVector::from_iterator(n, samples.iter().map(|(_, _, z)| *z));
68
69        let mut k = self.kernel_matrix(&self.train_xy, &self.train_xy);
70        for i in 0..n {
71            k[(i, i)] += self.config.noise_variance;
72        }
73
74        let Some(cholesky) = k.clone().cholesky() else {
75            return Err(RoboticsError::NumericalError(
76                "GaussianProcess: kernel matrix is not positive definite".to_string(),
77            ));
78        };
79
80        let alpha = cholesky.solve(&z);
81        let identity = DMatrix::<f64>::identity(n, n);
82        let k_inv = cholesky.solve(&identity);
83
84        self.alpha = Some(alpha);
85        self.k_inv = Some(k_inv);
86        Ok(())
87    }
88
89    /// Predict `(mean, variance)` for query `(x, y)` points.
90    pub fn predict(
91        &self,
92        query_points: &[(f64, f64)],
93    ) -> Result<(Vec<f64>, Vec<f64>), RoboticsError> {
94        if query_points.is_empty() {
95            return Ok((Vec::new(), Vec::new()));
96        }
97        let Some(alpha) = self.alpha.as_ref() else {
98            return Err(RoboticsError::InvalidParameter(
99                "GaussianProcess: model must be fitted before predict".to_string(),
100            ));
101        };
102        let Some(k_inv) = self.k_inv.as_ref() else {
103            return Err(RoboticsError::InvalidParameter(
104                "GaussianProcess: inverse kernel state missing; call fit first".to_string(),
105            ));
106        };
107
108        let k_star = self.kernel_matrix(query_points, &self.train_xy); // m x n
109        let mean_vec = &k_star * alpha; // m x 1
110
111        let mut means = Vec::with_capacity(query_points.len());
112        let mut variances = Vec::with_capacity(query_points.len());
113
114        for i in 0..query_points.len() {
115            let query = query_points[i];
116            let k_ss = self.kernel(query, query);
117            let k_row = k_star.row(i).transpose();
118            let variance = (k_ss - (k_row.transpose() * k_inv * k_row)[(0, 0)]).max(0.0);
119            means.push(mean_vec[i]);
120            variances.push(variance);
121        }
122
123        Ok((means, variances))
124    }
125
126    fn kernel(&self, a: (f64, f64), b: (f64, f64)) -> f64 {
127        let dx = a.0 - b.0;
128        let dy = a.1 - b.1;
129        let r2 = dx * dx + dy * dy;
130        self.config.signal_variance
131            * (-0.5 * r2 / (self.config.length_scale * self.config.length_scale)).exp()
132    }
133
134    fn kernel_matrix(&self, lhs: &[(f64, f64)], rhs: &[(f64, f64)]) -> DMatrix<f64> {
135        let mut matrix = DMatrix::zeros(lhs.len(), rhs.len());
136        for i in 0..lhs.len() {
137            for j in 0..rhs.len() {
138                matrix[(i, j)] = self.kernel(lhs[i], rhs[j]);
139            }
140        }
141        matrix
142    }
143}
144
145#[cfg(test)]
146mod tests {
147    use super::*;
148
149    #[test]
150    fn test_gaussian_process_config_defaults() {
151        let config = GaussianProcessConfig::default();
152        assert_eq!(config.length_scale, 1.0);
153        assert_eq!(config.signal_variance, 1.0);
154        assert_eq!(config.noise_variance, 0.01);
155    }
156
157    #[test]
158    fn test_gaussian_process_predicts_known_function() {
159        let mut gp = GaussianProcessRegressor::new(GaussianProcessConfig::default());
160        let train = vec![
161            (0.0, 0.0, 1.0),
162            (1.0, 0.0, 1.8414709848),
163            (0.0, 1.0, 0.5403023059),
164            (1.0, 1.0, 1.3817732907),
165            (0.5, 0.5, 1.3570081005),
166        ];
167        gp.fit(&train).expect("fit should succeed");
168
169        let query = vec![(0.5, 0.5), (0.8, 0.2)];
170        let (mean, _) = gp.predict(&query).expect("predict should succeed");
171
172        assert!((mean[0] - 1.3570081005).abs() < 0.1);
173        let expected = 0.8f64.sin() + 0.2f64.cos();
174        assert!((mean[1] - expected).abs() < 0.2);
175    }
176
177    #[test]
178    fn test_gaussian_process_uncertainty_increases_away_from_data() {
179        let mut gp = GaussianProcessRegressor::new(GaussianProcessConfig::default());
180        let train = vec![
181            (0.0, 0.0, 0.0),
182            (0.5, 0.0, 0.5),
183            (0.0, 0.5, 0.5),
184            (0.5, 0.5, 1.0),
185        ];
186        gp.fit(&train).expect("fit should succeed");
187
188        let (_, var_near) = gp.predict(&[(0.25, 0.25)]).expect("predict near");
189        let (_, var_far) = gp.predict(&[(3.0, 3.0)]).expect("predict far");
190
191        assert!(var_far[0] > var_near[0]);
192    }
193}