Skip to main content

rust_robotics_slam/
graph_based_slam.rs

1#![allow(dead_code, clippy::too_many_arguments)]
2
3// Graph-based SLAM
4// author: Atsushi Sakai (@Atsushi_twi)
5//         Rust port
6//
7// Ref:
8// [A Tutorial on Graph-Based SLAM]
9// http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
10
11use nalgebra::{DMatrix, DVector, Matrix3, Vector3};
12use rand_distr::{Distribution, Normal};
13use rust_robotics_core::normalize_angle;
14
15// Simulation parameters
16const DT: f64 = 2.0; // time step [s]
17const MAX_RANGE: f64 = 30.0; // maximum observation range [m]
18const STATE_SIZE: usize = 3; // State size [x, y, yaw]
19
20// Noise parameters
21const Q_SIM: [[f64; 2]; 2] = [[0.2, 0.0], [0.0, 0.00030462]]; // 0.00030462 ≈ deg2rad(1.0)^2
22const R_SIM: [[f64; 2]; 2] = [[0.1, 0.0], [0.0, 0.030462]]; // 0.030462 ≈ deg2rad(10.0)^2
23
24// Covariance parameters of Graph Based SLAM
25const C_SIGMA1: f64 = 0.1;
26const C_SIGMA2: f64 = 0.1;
27const C_SIGMA3: f64 = 0.017453; // deg2rad(1.0)
28
29const MAX_ITR: usize = 20; // Maximum iteration
30
31/// Observation data structure
32#[derive(Clone)]
33pub struct Observation {
34    pub d: f64,     // distance
35    pub angle: f64, // angle relative to robot
36    pub phi: f64,   // absolute angle
37    pub id: usize,  // landmark id
38}
39
40/// Edge represents a constraint between two poses
41#[derive(Clone)]
42struct Edge {
43    e: Vector3<f64>,     // error vector
44    omega: Matrix3<f64>, // information matrix
45    d1: f64,
46    d2: f64,
47    yaw1: f64,
48    yaw2: f64,
49    angle1: f64,
50    angle2: f64,
51    id1: usize,
52    id2: usize,
53}
54
55impl Edge {
56    fn new() -> Self {
57        Edge {
58            e: Vector3::zeros(),
59            omega: Matrix3::zeros(),
60            d1: 0.0,
61            d2: 0.0,
62            yaw1: 0.0,
63            yaw2: 0.0,
64            angle1: 0.0,
65            angle2: 0.0,
66            id1: 0,
67            id2: 0,
68        }
69    }
70}
71
72/// Calculate observation noise covariance
73fn cal_observation_sigma() -> Matrix3<f64> {
74    Matrix3::from_diagonal(&Vector3::new(
75        C_SIGMA1 * C_SIGMA1,
76        C_SIGMA2 * C_SIGMA2,
77        C_SIGMA3 * C_SIGMA3,
78    ))
79}
80
81/// Calculate 3D rotation matrix around z-axis
82fn calc_3d_rotational_matrix(angle: f64) -> Matrix3<f64> {
83    let c = angle.cos();
84    let s = angle.sin();
85    Matrix3::new(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0)
86}
87
88/// Calculate edge between two poses that observed the same landmark
89fn calc_edge(
90    x1: f64,
91    y1: f64,
92    yaw1: f64,
93    x2: f64,
94    y2: f64,
95    yaw2: f64,
96    d1: f64,
97    angle1: f64,
98    d2: f64,
99    angle2: f64,
100    t1: usize,
101    t2: usize,
102) -> Edge {
103    let mut edge = Edge::new();
104
105    let tangle1 = normalize_angle(yaw1 + angle1);
106    let tangle2 = normalize_angle(yaw2 + angle2);
107
108    let tmp1 = d1 * tangle1.cos();
109    let tmp2 = d2 * tangle2.cos();
110    let tmp3 = d1 * tangle1.sin();
111    let tmp4 = d2 * tangle2.sin();
112
113    // Error vector
114    edge.e[0] = x2 - x1 - tmp1 + tmp2;
115    edge.e[1] = y2 - y1 - tmp3 + tmp4;
116    edge.e[2] = 0.0;
117
118    // Information matrix
119    let rt1 = calc_3d_rotational_matrix(tangle1);
120    let rt2 = calc_3d_rotational_matrix(tangle2);
121
122    let sig1 = cal_observation_sigma();
123    let sig2 = cal_observation_sigma();
124
125    let cov = rt1 * sig1 * rt1.transpose() + rt2 * sig2 * rt2.transpose();
126    edge.omega = cov.try_inverse().unwrap_or(Matrix3::identity());
127
128    edge.d1 = d1;
129    edge.d2 = d2;
130    edge.yaw1 = yaw1;
131    edge.yaw2 = yaw2;
132    edge.angle1 = angle1;
133    edge.angle2 = angle2;
134    edge.id1 = t1;
135    edge.id2 = t2;
136
137    edge
138}
139
140/// Calculate all edges from pose and observation lists
141fn calc_edges(x_list: &DMatrix<f64>, z_list: &[Option<Vec<Observation>>]) -> (Vec<Edge>, f64) {
142    let mut edges = Vec::new();
143    let mut cost = 0.0;
144
145    let nt = z_list.len();
146
147    // Generate all pairs of time indices
148    for t1 in 0..nt {
149        for t2 in (t1 + 1)..nt {
150            let x1 = x_list[(0, t1)];
151            let y1 = x_list[(1, t1)];
152            let yaw1 = x_list[(2, t1)];
153            let x2 = x_list[(0, t2)];
154            let y2 = x_list[(1, t2)];
155            let yaw2 = x_list[(2, t2)];
156
157            // Check if both have observations
158            if let (Some(z1), Some(z2)) = (&z_list[t1], &z_list[t2]) {
159                // Find matching landmarks
160                for obs1 in z1 {
161                    for obs2 in z2 {
162                        if obs1.id == obs2.id {
163                            let edge = calc_edge(
164                                x1, y1, yaw1, x2, y2, yaw2, obs1.d, obs1.angle, obs2.d, obs2.angle,
165                                t1, t2,
166                            );
167
168                            // Calculate cost: e^T * omega * e
169                            let e_t_omega = edge.e.transpose() * edge.omega;
170                            cost += (e_t_omega * edge.e)[(0, 0)];
171
172                            edges.push(edge);
173                        }
174                    }
175                }
176            }
177        }
178    }
179
180    (edges, cost)
181}
182
183/// Calculate Jacobians for an edge
184fn calc_jacobian(edge: &Edge) -> (Matrix3<f64>, Matrix3<f64>) {
185    let t1 = edge.yaw1 + edge.angle1;
186    let a = Matrix3::new(
187        -1.0,
188        0.0,
189        edge.d1 * t1.sin(),
190        0.0,
191        -1.0,
192        -edge.d1 * t1.cos(),
193        0.0,
194        0.0,
195        0.0,
196    );
197
198    let t2 = edge.yaw2 + edge.angle2;
199    let b = Matrix3::new(
200        1.0,
201        0.0,
202        -edge.d2 * t2.sin(),
203        0.0,
204        1.0,
205        edge.d2 * t2.cos(),
206        0.0,
207        0.0,
208        0.0,
209    );
210
211    (a, b)
212}
213
214/// Fill H matrix and b vector for an edge
215fn fill_h_and_b(h: &mut DMatrix<f64>, b: &mut DVector<f64>, edge: &Edge) {
216    let (a, b_jac) = calc_jacobian(edge);
217
218    let id1 = edge.id1 * STATE_SIZE;
219    let id2 = edge.id2 * STATE_SIZE;
220
221    // H[id1:id1+3, id1:id1+3] += A^T * omega * A
222    let at_omega = a.transpose() * edge.omega;
223    let at_omega_a = at_omega * a;
224    let at_omega_b = at_omega * b_jac;
225
226    let bt_omega = b_jac.transpose() * edge.omega;
227    let bt_omega_a = bt_omega * a;
228    let bt_omega_b = bt_omega * b_jac;
229
230    for i in 0..STATE_SIZE {
231        for j in 0..STATE_SIZE {
232            h[(id1 + i, id1 + j)] += at_omega_a[(i, j)];
233            h[(id1 + i, id2 + j)] += at_omega_b[(i, j)];
234            h[(id2 + i, id1 + j)] += bt_omega_a[(i, j)];
235            h[(id2 + i, id2 + j)] += bt_omega_b[(i, j)];
236        }
237    }
238
239    // b[id1:id1+3] += A^T * omega * e
240    let at_omega_e = at_omega * edge.e;
241    let bt_omega_e = bt_omega * edge.e;
242
243    for i in 0..STATE_SIZE {
244        b[id1 + i] += at_omega_e[i];
245        b[id2 + i] += bt_omega_e[i];
246    }
247}
248
249/// Graph-based SLAM optimization
250pub fn graph_based_slam(x_init: &DMatrix<f64>, hz: &[Option<Vec<Observation>>]) -> DMatrix<f64> {
251    let mut x_opt = x_init.clone();
252    let nt = x_opt.ncols();
253    let n = nt * STATE_SIZE;
254
255    for _itr in 0..MAX_ITR {
256        let (edges, _cost) = calc_edges(&x_opt, hz);
257
258        let mut h = DMatrix::<f64>::zeros(n, n);
259        let mut b = DVector::<f64>::zeros(n);
260
261        for edge in &edges {
262            fill_h_and_b(&mut h, &mut b, edge);
263        }
264
265        // Fix origin by adding identity to first pose block
266        for i in 0..STATE_SIZE {
267            h[(i, i)] += 1.0;
268        }
269
270        // Solve H * dx = -b
271        let h_inv = match h.clone().try_inverse() {
272            Some(inv) => inv,
273            None => {
274                // Simple fallback - add regularization
275                let mut h_reg = h.clone();
276                for i in 0..n {
277                    h_reg[(i, i)] += 0.001;
278                }
279                h_reg.try_inverse().unwrap_or(DMatrix::identity(n, n))
280            }
281        };
282
283        let dx = -h_inv * &b;
284
285        // Update poses
286        for i in 0..nt {
287            for j in 0..STATE_SIZE {
288                x_opt[(j, i)] += dx[i * STATE_SIZE + j];
289            }
290        }
291
292        let diff = dx.dot(&dx);
293
294        if diff < 1.0e-5 {
295            break;
296        }
297    }
298
299    x_opt
300}
301
302/// Motion model for robot
303pub fn motion_model(x: &Vector3<f64>, u: &[f64; 2]) -> Vector3<f64> {
304    let yaw = x[2];
305    Vector3::new(
306        x[0] + u[0] * DT * yaw.cos(),
307        x[1] + u[0] * DT * yaw.sin(),
308        x[2] + u[1] * DT,
309    )
310}
311
312/// Calculate control input
313pub fn calc_input() -> [f64; 2] {
314    let v = 1.0; // [m/s]
315    let yaw_rate = 0.1; // [rad/s]
316    [v, yaw_rate]
317}
318
319/// Simulate observation and motion
320pub fn observation(
321    x_true: &Vector3<f64>,
322    xd: &Vector3<f64>,
323    u: &[f64; 2],
324    rfid: &[(f64, f64)],
325) -> (
326    Vector3<f64>,
327    Option<Vec<Observation>>,
328    Vector3<f64>,
329    [f64; 2],
330) {
331    let normal = Normal::new(0.0, 1.0).unwrap();
332    let mut rng = rand::rng();
333
334    // True state update
335    let x_true_new = motion_model(x_true, u);
336
337    // Observations
338    let mut z = Vec::new();
339    for (i, (lx, ly)) in rfid.iter().enumerate() {
340        let dx = lx - x_true_new[0];
341        let dy = ly - x_true_new[1];
342        let d = (dx * dx + dy * dy).sqrt();
343        let angle = normalize_angle(dy.atan2(dx) - x_true_new[2]);
344        let phi = normalize_angle(dy.atan2(dx));
345
346        if d <= MAX_RANGE {
347            // Add noise
348            let dn = d + normal.sample(&mut rng) * Q_SIM[0][0].sqrt();
349            let angle_noise = normal.sample(&mut rng) * Q_SIM[1][1].sqrt();
350            let angle_noisy = angle + angle_noise;
351            let phi_noisy = phi + angle_noise;
352
353            z.push(Observation {
354                d: dn,
355                angle: angle_noisy,
356                phi: phi_noisy,
357                id: i,
358            });
359        }
360    }
361
362    // Dead reckoning with noise
363    let ud = [
364        u[0] + normal.sample(&mut rng) * R_SIM[0][0].sqrt(),
365        u[1] + normal.sample(&mut rng) * R_SIM[1][1].sqrt(),
366    ];
367    let xd_new = motion_model(xd, &ud);
368
369    let z_opt = if z.is_empty() { None } else { Some(z) };
370
371    (x_true_new, z_opt, xd_new, ud)
372}
373
374#[cfg(test)]
375mod tests {
376    use super::*;
377
378    const EPS: f64 = 1e-10;
379
380    fn assert_near(a: f64, b: f64, eps: f64) {
381        assert!(
382            (a - b).abs() < eps,
383            "expected {b}, got {a} (diff={})",
384            (a - b).abs()
385        );
386    }
387
388    #[test]
389    fn test_motion_model() {
390        // Robot at origin facing +x, moving forward at 1 m/s with no rotation
391        let x = Vector3::new(0.0, 0.0, 0.0);
392        let u = [1.0, 0.0]; // v=1 m/s, yaw_rate=0
393        let x_new = motion_model(&x, &u);
394
395        // With DT=2.0, should move 2m in x, 0 in y, yaw unchanged
396        assert_near(x_new[0], 2.0, EPS);
397        assert_near(x_new[1], 0.0, EPS);
398        assert_near(x_new[2], 0.0, EPS);
399
400        // Robot facing PI/2 (north), moving forward
401        let x2 = Vector3::new(1.0, 2.0, std::f64::consts::FRAC_PI_2);
402        let u2 = [1.0, 0.0];
403        let x2_new = motion_model(&x2, &u2);
404
405        assert_near(x2_new[0], 1.0, 1e-9); // cos(pi/2) ~ 0
406        assert_near(x2_new[1], 4.0, 1e-9); // sin(pi/2) = 1, 2 + 1*2*1
407        assert_near(x2_new[2], std::f64::consts::FRAC_PI_2, EPS);
408    }
409
410    #[test]
411    fn test_calc_input() {
412        let u = calc_input();
413        assert_near(u[0], 1.0, EPS); // v = 1.0 m/s
414        assert_near(u[1], 0.1, EPS); // yaw_rate = 0.1 rad/s
415    }
416
417    #[test]
418    fn test_observation_struct() {
419        let obs = Observation {
420            d: 5.0,
421            angle: 0.3,
422            phi: 1.2,
423            id: 7,
424        };
425        assert_near(obs.d, 5.0, EPS);
426        assert_near(obs.angle, 0.3, EPS);
427        assert_near(obs.phi, 1.2, EPS);
428        assert_eq!(obs.id, 7);
429
430        // Test Clone
431        let obs2 = obs.clone();
432        assert_eq!(obs2.id, 7);
433        assert_near(obs2.d, 5.0, EPS);
434    }
435
436    #[test]
437    fn test_motion_model_zero_input() {
438        let x = Vector3::new(3.0, 4.0, 0.5);
439        let u = [0.0, 0.0];
440        let x_new = motion_model(&x, &u);
441
442        // Zero velocity and zero yaw rate should preserve state
443        assert_near(x_new[0], 3.0, EPS);
444        assert_near(x_new[1], 4.0, EPS);
445        assert_near(x_new[2], 0.5, EPS);
446    }
447
448    #[test]
449    fn test_graph_based_slam_small() {
450        // Minimal scenario: 2 poses, one with an observation, one without.
451        // This mainly verifies the function does not panic.
452        let n_poses = 3;
453        let mut x_init = DMatrix::<f64>::zeros(3, n_poses);
454        // Place poses along x-axis
455        for i in 0..n_poses {
456            x_init[(0, i)] = i as f64 * 2.0;
457            x_init[(1, i)] = 0.0;
458            x_init[(2, i)] = 0.0;
459        }
460
461        // Create observation data: poses 0 and 2 see the same landmark
462        let hz: Vec<Option<Vec<Observation>>> = vec![
463            Some(vec![Observation {
464                d: 5.0,
465                angle: 0.3,
466                phi: 0.3,
467                id: 0,
468            }]),
469            None,
470            Some(vec![Observation {
471                d: 3.0,
472                angle: -0.2,
473                phi: -0.2,
474                id: 0,
475            }]),
476        ];
477
478        let result = graph_based_slam(&x_init, &hz);
479
480        // Result should have same dimensions
481        assert_eq!(result.nrows(), 3);
482        assert_eq!(result.ncols(), n_poses);
483
484        // Values should be finite (no NaN or Inf)
485        for i in 0..result.nrows() {
486            for j in 0..result.ncols() {
487                assert!(
488                    result[(i, j)].is_finite(),
489                    "Result contains non-finite value at ({i}, {j})"
490                );
491            }
492        }
493    }
494}