1#![allow(dead_code, clippy::too_many_arguments)]
2
3use nalgebra::{DMatrix, DVector, Matrix3, Vector3};
12use rand_distr::{Distribution, Normal};
13use rust_robotics_core::normalize_angle;
14
15const DT: f64 = 2.0; const MAX_RANGE: f64 = 30.0; const STATE_SIZE: usize = 3; const Q_SIM: [[f64; 2]; 2] = [[0.2, 0.0], [0.0, 0.00030462]]; const R_SIM: [[f64; 2]; 2] = [[0.1, 0.0], [0.0, 0.030462]]; const C_SIGMA1: f64 = 0.1;
26const C_SIGMA2: f64 = 0.1;
27const C_SIGMA3: f64 = 0.017453; const MAX_ITR: usize = 20; #[derive(Clone)]
33pub struct Observation {
34 pub d: f64, pub angle: f64, pub phi: f64, pub id: usize, }
39
40#[derive(Clone)]
42struct Edge {
43 e: Vector3<f64>, omega: Matrix3<f64>, 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
72fn 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
81fn 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
88fn 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 edge.e[0] = x2 - x1 - tmp1 + tmp2;
115 edge.e[1] = y2 - y1 - tmp3 + tmp4;
116 edge.e[2] = 0.0;
117
118 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
140fn 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 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 if let (Some(z1), Some(z2)) = (&z_list[t1], &z_list[t2]) {
159 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 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
183fn 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
214fn 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 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 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
249pub 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 for i in 0..STATE_SIZE {
267 h[(i, i)] += 1.0;
268 }
269
270 let h_inv = match h.clone().try_inverse() {
272 Some(inv) => inv,
273 None => {
274 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 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
302pub 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
312pub fn calc_input() -> [f64; 2] {
314 let v = 1.0; let yaw_rate = 0.1; [v, yaw_rate]
317}
318
319pub 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 let x_true_new = motion_model(x_true, u);
336
337 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 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 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 let x = Vector3::new(0.0, 0.0, 0.0);
392 let u = [1.0, 0.0]; let x_new = motion_model(&x, &u);
394
395 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 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); assert_near(x2_new[1], 4.0, 1e-9); 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); assert_near(u[1], 0.1, EPS); }
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 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 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 let n_poses = 3;
453 let mut x_init = DMatrix::<f64>::zeros(3, n_poses);
454 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 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 assert_eq!(result.nrows(), 3);
482 assert_eq!(result.ncols(), n_poses);
483
484 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}