├── .gitignore
├── Cargo.toml
├── README.md
├── examples
    └── dummy.rs
└── src
    ├── codegen
        └── mod.rs
    ├── core
        ├── group.rs
        ├── manifold.rs
        ├── matrix.rs
        ├── mod.rs
        └── vector.rs
    ├── geometry
        ├── mod.rs
        ├── se3.rs
        └── so3.rs
    ├── inference
        ├── cluster_tree.rs
        ├── conditional.rs
        ├── expression
        │   ├── factor_graph.rs
        │   └── mod.rs
        ├── factor.rs
        ├── factor_graph.rs
        ├── junction_tree.rs
        └── mod.rs
    ├── lib.rs
    ├── linear
        ├── gaussian.rs
        ├── gaussian_factor_graph.rs
        ├── gaussian_like.rs
        ├── jacobian.rs
        ├── mod.rs
        └── noise_model
        │   ├── diagonal.rs
        │   ├── gaussian.rs
        │   ├── isotropic.rs
        │   ├── mod.rs
        │   └── unit.rs
    └── nonlinear
        ├── mod.rs
        └── nonlinear_factor.rs


/.gitignore:
--------------------------------------------------------------------------------
1 | /target
2 | **/*.rs.bk
3 | Cargo.lock
4 | 
5 | /.idea
6 | 
7 | .vscode/
8 | 


--------------------------------------------------------------------------------
/Cargo.toml:
--------------------------------------------------------------------------------
 1 | [package]
 2 | name = "rtsam"
 3 | version = "0.1.4"
 4 | authors = ["Fan Jiang <i@fanjiang.me>"]
 5 | edition = "2018"
 6 | repository = "https://github.com/ProfFan/rtsam"
 7 | description = "Real Time Smoothing and Mapping (RTSAM) in Rust."
 8 | license = "BSD-3-Clause"
 9 | 
10 | [dependencies]
11 | nalgebra = "0.31.1"
12 | typenum = "1.14.0"
13 | alga = "0.9.3"
14 | num = "0.4.0"
15 | approx = "0.5.0"
16 | inkwell = { version = "0.1.0-beta4", optional = true }
17 | llvm-sys = { version = "130.0.0", optional = true }
18 | 
19 | [dev-dependencies]
20 | finitediff = "0.1.4"
21 | 
22 | [features]
23 | # The default set of optional packages. Most people will want to use these
24 | # packages, but they are strictly optional. Note that `session` is not a package
25 | # but rather another feature listed in this manifest.
26 | default = []
27 | codegen = ["inkwell", "llvm-sys"]
28 | 
29 | [package.metadata.docs.rs]
30 | rustdoc-args = [
31 |     "--html-in-header",
32 |     "katex.html",
33 | ]
34 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # RTSAM
 2 | 
 3 | ![crate](https://img.shields.io/crates/v/rtsam.svg)
 4 | 
 5 | Real Time Smoothing and Mapping (RTSAM) in Rust.
 6 | 
 7 | # Development Plan
 8 | 
 9 | - [x] Standard Lie algebra in Rust
10 | - [ ] Working simple factor graph generation
11 | - [ ] Working LLVM-based JIT for CPU/GPU targets (WIP branch `dev-jit`)
12 | - [ ] Working symbolic evaluation and autograd
13 | - [ ] Design a continous storage strategy for factors
14 | - [ ] Dynamic scheduling of JIT'ed code fragments
15 | - [ ] GPU LM/GS Kernels with JIT'ed lazy evaluation
16 | 
17 | # Warning
18 | 
19 | This work is a WIP.
20 | 
21 | # LICENSE
22 | 
23 | BSD 3-Clause
24 | 
25 | Copyright (c) 2020, Fan Jiang
26 | 
27 | Code from the nalgebra project: Copyright (c) 2013, Sébastien Crozet


--------------------------------------------------------------------------------
/examples/dummy.rs:
--------------------------------------------------------------------------------
 1 | use nalgebra;
 2 | 
 3 | use nalgebra::Vector6;
 4 | use rtsam::core::group::LieGroup;
 5 | use rtsam::geometry::SE3;
 6 | 
 7 | fn main() -> Result<(), Box<dyn std::error::Error>> {
 8 |     println!("Dummy Example PRoGraM");
 9 | 
10 |     let w = Vector6::new(1., 1.2, 1.3, 1., 1.4, 1.3);
11 | 
12 |     let exp = SE3::expmap_with_derivative(&w, None);
13 | 
14 |     let log = SE3::logmap(&exp, None);
15 | 
16 |     println!("w: {}", w.transpose());
17 |     println!("Expmap: {}", exp);
18 |     println!("Logmap: {}", log.transpose());
19 | 
20 |     Ok(())
21 | }
22 | 


--------------------------------------------------------------------------------
/src/codegen/mod.rs:
--------------------------------------------------------------------------------
 1 | #[cfg(all(codegen,test))]
 2 | mod tests {
 3 |     // use super::*;
 4 | 
 5 |     use inkwell;
 6 |     use inkwell::builder::Builder;
 7 |     use inkwell::context::Context;
 8 |     use inkwell::execution_engine::{ExecutionEngine, JitFunction};
 9 |     use inkwell::module::Module;
10 |     use inkwell::targets::Target;
11 |     use inkwell::OptimizationLevel;
12 |     use llvm_sys;
13 | 
14 |     /// Convenience type alias for the `sum` function.
15 |     ///
16 |     /// Calling this is innately `unsafe` because there's no guarantee it doesn't
17 |     /// do `unsafe` operations internally.
18 |     type SumFunc = unsafe extern "C" fn(u64, u64, u64) -> u64;
19 | 
20 |     fn jit_compile_sum(
21 |         context: &Context,
22 |         module: &Module,
23 |         builder: &Builder,
24 |         execution_engine: &ExecutionEngine,
25 |     ) -> Option<JitFunction<SumFunc>> {
26 |         let i64_type = context.i64_type();
27 |         let fn_type = i64_type.fn_type(&[i64_type.into(), i64_type.into(), i64_type.into()], false);
28 | 
29 |         let function = module.add_function("sum", fn_type, None);
30 |         let basic_block = context.append_basic_block(&function, "entry");
31 | 
32 |         builder.position_at_end(&basic_block);
33 | 
34 |         let x = function.get_nth_param(0)?.into_int_value();
35 |         let y = function.get_nth_param(1)?.into_int_value();
36 |         let z = function.get_nth_param(2)?.into_int_value();
37 | 
38 |         let sum = builder.build_int_add(x, y, "sum");
39 |         let sum = builder.build_int_add(sum, z, "sum");
40 | 
41 |         builder.build_return(Some(&sum));
42 | 
43 |         unsafe { execution_engine.get_function("sum").ok() }
44 |     }
45 | 
46 |     #[derive(Debug, PartialEq, Eq)]
47 |     pub struct MyModule {
48 |         pub(crate) non_global_context: Option<Context>, // REVIEW: Could we just set context to the global context?
49 |         data_layout: std::cell::RefCell<Option<inkwell::data_layout::DataLayout>>,
50 |         pub(crate) module: std::cell::Cell<llvm_sys::prelude::LLVMModuleRef>,
51 |         pub(crate) owned_by_ee:
52 |             std::cell::RefCell<Option<inkwell::execution_engine::ExecutionEngine>>,
53 |     }
54 | 
55 |     #[test]
56 |     fn inkwell_working() {
57 |         println!("Testing LLVM codegen...");
58 | 
59 |         let context = Context::create();
60 |         let module = context.create_module("sum");
61 |         let builder = context.create_builder();
62 |         let execution_engine = module
63 |             .create_jit_execution_engine(OptimizationLevel::Aggressive)
64 |             .unwrap();
65 | 
66 |         let sum = jit_compile_sum(&context, &module, &builder, &execution_engine)
67 |             .ok_or("Unable to JIT compile `sum`")
68 |             .unwrap();
69 | 
70 |         let x = 1u64;
71 |         let y = 2u64;
72 |         let z = 3u64;
73 |         let result : u64;
74 | 
75 |         unsafe { result = sum.call(x, y, z); }
76 | 
77 |         println!("{} + {} + {} = {}", x, y, z, result);
78 | 
79 |         let mymod: MyModule = unsafe { std::mem::transmute(module) };
80 | 
81 |         println!("Emitted Assembly:");
82 |         unsafe {
83 |             llvm_sys::core::LLVMDumpModule(mymod.module.get());
84 |         }
85 | 
86 |         assert_eq!(result, x + y + z);
87 | 
88 |     }
89 | 
90 | }
91 | 


--------------------------------------------------------------------------------
/src/core/group.rs:
--------------------------------------------------------------------------------
  1 | use crate::core::manifold::Manifold;
  2 | 
  3 | use nalgebra::allocator::Allocator;
  4 | use nalgebra::{DefaultAllocator, DimName, OMatrix, OVector, Scalar};
  5 | use std::fmt::Debug;
  6 | use std::ops::Mul;
  7 | 
  8 | #[allow(non_snake_case)]
  9 | pub trait LieGroup<N>: Debug + Sized + Copy
 10 | where
 11 |     Self: Mul<Self, Output = Self>,
 12 |     Self: for<'a> Mul<&'a Self, Output = Self>,
 13 |     for<'a> &'a Self: Mul<Self, Output = Self>,
 14 |     for<'a, 'b> &'a Self: Mul<&'b Self, Output = Self>,
 15 |     N: Scalar + num::Zero + num::One,
 16 |     Self: Manifold,
 17 | {
 18 |     type D: DimName;
 19 | 
 20 |     fn compose(&self, g: &Self) -> Self {
 21 |         self * g
 22 |     }
 23 | 
 24 |     fn between(&self, g: &Self) -> Self;
 25 | 
 26 |     fn adjoint_map(&self) -> OMatrix<N, Self::D, Self::D>
 27 |     where
 28 |         DefaultAllocator: Allocator<N, Self::D, Self::D>;
 29 | 
 30 |     // TODO(fan): H now does not work
 31 |     fn logmap(R: &Self, H: Option<&mut OMatrix<N, Self::D, Self::D>>) -> OVector<N, Self::D>
 32 |     where
 33 |         DefaultAllocator: Allocator<N, Self::D> + Allocator<N, Self::D, Self::D>;
 34 | 
 35 |     fn expmap(omega: &OVector<N, Self::D>) -> Self
 36 |     where
 37 |         DefaultAllocator: Allocator<N, Self::D>;
 38 | 
 39 |     fn expmap_with_derivative(
 40 |         omega: &OVector<N, Self::D>,
 41 |         H: Option<&mut OMatrix<N, Self::D, Self::D>>,
 42 |     ) -> Self
 43 |     where
 44 |         DefaultAllocator: Allocator<N, Self::D> + Allocator<N, Self::D, Self::D>;
 45 | }
 46 | 
 47 | #[cfg(test)]
 48 | mod tests {
 49 |     use crate::geometry::so3::*;
 50 |     use finitediff::FiniteDiff;
 51 |     use nalgebra::{Matrix3, Vector3};
 52 |     use std::f64::consts::PI;
 53 | 
 54 |     #[test]
 55 |     fn compose_works() {
 56 |         let r1 = SO3::<f64>::from_axis_angle(&Vector3::x_axis(), PI);
 57 |         let r2 = SO3::<f64>::from_axis_angle(&Vector3::x_axis(), -PI);
 58 | 
 59 |         assert_relative_eq!(
 60 |             (r1.compose(&r2).matrix() - SO3::identity().matrix()).norm(),
 61 |             0.0
 62 |         );
 63 |     }
 64 | 
 65 |     #[test]
 66 |     fn between_works() {
 67 |         let r1 = SO3::<f64>::from_axis_angle(&Vector3::x_axis(), 1. * PI);
 68 |         let r2 = SO3::<f64>::from_axis_angle(&Vector3::x_axis(), -1. * PI);
 69 | 
 70 |         assert_relative_eq!(
 71 |             (r1.between(&r2).matrix() - SO3::identity().matrix()).norm(),
 72 |             0.0,
 73 |             epsilon = 1e-10
 74 |         );
 75 |     }
 76 | 
 77 |     #[test]
 78 |     fn adjoint_map_works() {
 79 |         let r1 = SO3::<f64>::from_axis_angle(&Vector3::x_axis(), 1. * PI);
 80 |         let r2 = SO3::<f64>::from_axis_angle(&Vector3::x_axis(), 3. * PI);
 81 | 
 82 |         assert_relative_eq!(
 83 |             (r1.adjoint_map() - r2.adjoint_map()).norm(),
 84 |             0.0,
 85 |             epsilon = 1e-10
 86 |         );
 87 |     }
 88 | 
 89 |     // Left trivialized Derivative of exp(w) wrpt w:
 90 |     // How does exp(w) change when w changes?
 91 |     // We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
 92 |     // => y = log (exp(-w) * exp(w+dw))
 93 |     fn dexp_numeric(w: Vector3<f64>, dw: Vector3<f64>) -> Vector3<f64> {
 94 |         SO3::logmap(&(SO3::expmap(&-w) * SO3::expmap(&(w + dw))), None)
 95 |     }
 96 | 
 97 |     #[test]
 98 |     fn expmap_works_1() {
 99 |         let w = Vector3::new(0.1, 0.27, -0.2);
100 |         let mut actual_dexp = Matrix3::<f64>::identity();
101 | 
102 |         let f = |x: &Vec<f64>| -> Vec<f64> {
103 |             let arr: [f64; 3] = dexp_numeric(w, Vector3::new(x[0], x[1], x[2])).into();
104 |             arr.to_vec()
105 |         };
106 | 
107 |         let w_: [f64; 3] = w.into();
108 |         let expected_dexp_ = w_.to_vec().forward_jacobian(&f);
109 |         let expected_dexp: Matrix3<f64> =
110 |             Matrix3::from_iterator(expected_dexp_.iter().flatten().cloned());
111 | 
112 |         SO3::expmap_with_derivative(&w, Some(&mut actual_dexp));
113 | 
114 |         println!("Norm: {} vs {}", expected_dexp.norm(), actual_dexp.norm());
115 |         assert_relative_eq!((actual_dexp - expected_dexp).norm(), 0.0, epsilon = 0.02);
116 |     }
117 | 
118 |     #[test]
119 |     fn expmap_works_2() {
120 |         let w = Vector3::new(10., 20., 30.);
121 |         let mut actual_dexp = Matrix3::<f64>::identity();
122 | 
123 |         let f = |x: &Vec<f64>| -> Vec<f64> {
124 |             let arr: [f64; 3] = dexp_numeric(w, Vector3::new(x[0], x[1], x[2])).into();
125 |             arr.to_vec()
126 |         };
127 | 
128 |         let w_: [f64; 3] = w.into();
129 |         let expected_dexp_ = w_.to_vec().central_jacobian(&f);
130 |         let expected_dexp: Matrix3<f64> =
131 |             Matrix3::from_iterator(expected_dexp_.iter().flatten().cloned());
132 | 
133 |         SO3::expmap_with_derivative(&w, Some(&mut actual_dexp));
134 | 
135 |         assert_relative_eq!((actual_dexp - expected_dexp).norm(), 0.0, epsilon = 0.01);
136 |     }
137 | 
138 |     #[test]
139 |     fn expmap_logmap_invariant() {
140 |         let w = Vector3::new(1., 1.2, 1.3);
141 |         let mut dexp = Matrix3::<f64>::identity();
142 | 
143 |         let exp = SO3::expmap_with_derivative(&w, Some(&mut dexp));
144 | 
145 |         assert_relative_eq!((w - SO3::logmap(&exp, None)).norm(), 0.0, epsilon = 1e-10);
146 |     }
147 | }
148 | 


--------------------------------------------------------------------------------
/src/core/manifold.rs:
--------------------------------------------------------------------------------
1 | pub trait Manifold {
2 |     type TangentVector;
3 | 
4 |     fn local(origin: &Self, other: &Self) -> Self::TangentVector;
5 | 
6 |     fn retract(origin: &Self, v: &Self::TangentVector) -> Self;
7 | }
8 | 


--------------------------------------------------------------------------------
/src/core/matrix.rs:
--------------------------------------------------------------------------------
  1 | use nalgebra as na;
  2 | use nalgebra::base::{DMatrix, DMatrixSlice, DVector};
  3 | 
  4 | pub fn skew_symmetric<N: na::RealField + Copy>(
  5 |     wx: N,
  6 |     wy: N,
  7 |     wz: N,
  8 | ) -> na::OMatrix<N, na::U3, na::U3> {
  9 |     na::Matrix3::new(N::zero(), -wz, wy, wz, N::zero(), -wx, -wy, wx, N::zero())
 10 | }
 11 | 
 12 | pub fn skew_symmetric_v<N: na::RealField + Copy>(
 13 |     v: &na::Vector3<N>,
 14 | ) -> na::OMatrix<N, na::U3, na::U3> {
 15 |     na::Matrix3::new(
 16 |         N::zero(),
 17 |         -v.z,
 18 |         v.y,
 19 |         v.z,
 20 |         N::zero(),
 21 |         -v.x,
 22 |         -v.y,
 23 |         v.x,
 24 |         N::zero(),
 25 |     )
 26 | }
 27 | 
 28 | pub struct SymmetricBlockMatrix<T: na::RealField + core::fmt::Debug = f64> {
 29 |     matrix: na::OMatrix<T, na::Dynamic, na::Dynamic>,
 30 |     variable_col_offsets: Vec<usize>,
 31 | }
 32 | 
 33 | impl<T: na::RealField + core::fmt::Debug> SymmetricBlockMatrix<T> {
 34 |     pub fn new() -> SymmetricBlockMatrix<T> {
 35 |         SymmetricBlockMatrix {
 36 |             matrix: na::DMatrix::identity(0, 0),
 37 |             variable_col_offsets: Vec::new(),
 38 |         }
 39 |     }
 40 | 
 41 |     fn fill_offsets(&mut self, dims: &[usize], append_one_dim: bool) {
 42 |         self.variable_col_offsets
 43 |             .resize(dims.len() + 1 + append_one_dim as usize, 0);
 44 |         self.variable_col_offsets[0] = 0;
 45 | 
 46 |         let mut j: usize = 0;
 47 | 
 48 |         for dim in dims {
 49 |             self.variable_col_offsets[j + 1] = self.variable_col_offsets[j] + dim;
 50 |             j += 1;
 51 |         }
 52 | 
 53 |         if append_one_dim {
 54 |             self.variable_col_offsets[j + 1] = self.variable_col_offsets[j] + 1;
 55 |         }
 56 |     }
 57 | 
 58 |     pub fn from_dimensions(dims: &[usize]) -> SymmetricBlockMatrix<T> {
 59 |         let mut mat = SymmetricBlockMatrix {
 60 |             matrix: na::DMatrix::identity(0, 0),
 61 |             variable_col_offsets: Vec::new(),
 62 |         };
 63 | 
 64 |         mat.fill_offsets(dims, false);
 65 | 
 66 |         let d = *mat.variable_col_offsets.last().unwrap();
 67 | 
 68 |         mat.matrix = DMatrix::identity(d, d);
 69 | 
 70 |         mat
 71 |     }
 72 | 
 73 |     pub fn num_blocks(&self) -> usize {
 74 |         self.variable_col_offsets.len() - 1
 75 |     }
 76 | 
 77 |     pub fn offset(&self, block: usize) -> usize {
 78 |         self.variable_col_offsets[block]
 79 |     }
 80 | 
 81 |     pub fn calc_indices(
 82 |         &self,
 83 |         i: usize,
 84 |         j: usize,
 85 |         rows: usize,
 86 |         cols: usize,
 87 |     ) -> (usize, usize, usize, usize) {
 88 |         let di = self.offset(i);
 89 |         let dj = self.offset(j);
 90 |         let dr = self.offset(i + rows) - di;
 91 |         let dc = self.offset(j + cols) - dj;
 92 | 
 93 |         (di, dj, dr, dc)
 94 |     }
 95 | 
 96 |     pub fn block_(&self, i: usize, j: usize, rows: usize, cols: usize) -> DMatrixSlice<T> {
 97 |         let ind = self.calc_indices(i, j, rows, cols);
 98 | 
 99 |         self.matrix.slice((ind.0, ind.1), (ind.2, ind.3))
100 |     }
101 | 
102 |     pub fn diagonal(&self, j: usize) -> DVector<T> {
103 |         self.block_(j, j, 1, 1).diagonal()
104 |     }
105 | }
106 | 
107 | impl<T: na::RealField> core::fmt::Debug for SymmetricBlockMatrix<T> {
108 |     fn fmt(&self, f: &mut core::fmt::Formatter) -> core::fmt::Result {
109 |         write!(
110 |             f,
111 |             "SymmetricBlockMatrix:\n{:}\n{:?}",
112 |             self.matrix, self.variable_col_offsets
113 |         )
114 |     }
115 | }
116 | 
117 | impl<T: na::RealField> Default for SymmetricBlockMatrix<T> {
118 |     fn default() -> Self {
119 |         Self::new()
120 |     }
121 | }
122 | 
123 | #[cfg(test)]
124 | mod tests {
125 |     use super::*;
126 | 
127 |     #[test]
128 |     fn symmetric_block_matrix_new() {
129 |         let s = SymmetricBlockMatrix::<f64>::new();
130 |         println!("{:?}", s);
131 |     }
132 | 
133 |     #[test]
134 |     fn symmetric_block_matrix_from_dims() {
135 |         let v = vec![3, 3, 2, 1];
136 |         let s = SymmetricBlockMatrix::<f64>::from_dimensions(&v);
137 |         println!("{:?}", s);
138 |     }
139 | 
140 |     #[test]
141 |     fn symmetric_block_matrix_get_indices() {
142 |         let v = vec![3, 3, 2, 1];
143 |         let s = SymmetricBlockMatrix::<f64>::from_dimensions(&v);
144 | 
145 |         let ind = s.calc_indices(1, 2, 1, 1);
146 | 
147 |         assert_eq!(ind, (3, 6, 3, 2));
148 |     }
149 | 
150 |     #[test]
151 |     fn symmetric_block_matrix_block_operations() {
152 |         let v = vec![3, 3, 2, 1];
153 |         let s = SymmetricBlockMatrix::<f64>::from_dimensions(&v);
154 | 
155 |         println!("{:}", s.block_(1, 2, 1, 1));
156 |         println!("{:}", s.diagonal(1));
157 |     }
158 | }
159 | 


--------------------------------------------------------------------------------
/src/core/mod.rs:
--------------------------------------------------------------------------------
1 | pub mod group;
2 | pub mod manifold;
3 | pub mod matrix;
4 | pub mod vector;
5 | 


--------------------------------------------------------------------------------
/src/core/vector.rs:
--------------------------------------------------------------------------------
  1 | extern crate nalgebra as na;
  2 | 
  3 | use self::na::storage::Storage;
  4 | use self::na::{DimName, U1};
  5 | pub use na::{Vector, Vector1, Vector2, Vector3, Vector4, Vector5, Vector6};
  6 | 
  7 | pub fn linear_dependent<D, S>(vec1: &Vector<f64, D, S>, vec2: &Vector<f64, D, S>, tol: f64) -> bool
  8 | where
  9 |     D: DimName,
 10 |     S: Storage<f64, D, U1>,
 11 | {
 12 |     let mut flag = false;
 13 |     let mut scale = 1.0;
 14 | 
 15 |     for i in 0..vec1.len() {
 16 |         if ((vec1[i].abs() > tol) && (vec2[i].abs() < tol))
 17 |             || ((vec1[i].abs() < tol) && (vec2[i].abs() > tol))
 18 |         {
 19 |             return false;
 20 |         }
 21 | 
 22 |         if vec1[i] == 0.0 && vec2[i] == 0.0 {
 23 |             continue;
 24 |         }
 25 | 
 26 |         if !flag {
 27 |             scale = vec1[i] / vec2[i];
 28 |             flag = true;
 29 |         } else if (vec1[i] - vec2[i] * scale).abs() > tol {
 30 |             return false;
 31 |         }
 32 |     }
 33 | 
 34 |     flag
 35 | }
 36 | 
 37 | #[cfg(test)]
 38 | mod tests {
 39 |     use super::{Vector3, Vector6};
 40 |     use crate::core::vector::linear_dependent;
 41 | 
 42 |     #[test]
 43 |     fn vector_component_works() {
 44 |         assert_eq!(Vector3::x(), Vector3::new(1.0, 0.0, 0.0));
 45 |         assert_eq!(Vector3::y(), Vector3::new(0.0, 1.0, 0.0));
 46 |         assert_eq!(Vector3::z(), Vector3::new(0.0, 0.0, 1.0));
 47 | 
 48 |         assert_eq!(
 49 |             Vector6::w(),
 50 |             Vector6::new(0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
 51 |             "w()"
 52 |         );
 53 |         assert_eq!(
 54 |             Vector6::a(),
 55 |             Vector6::new(0.0, 0.0, 0.0, 0.0, 1.0, 0.0),
 56 |             "a()"
 57 |         );
 58 |         assert_eq!(
 59 |             Vector6::b(),
 60 |             Vector6::new(0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
 61 |             "b()"
 62 |         );
 63 |     }
 64 | 
 65 |     #[test]
 66 |     fn linear_dependent_works() {
 67 |         assert_eq!(
 68 |             linear_dependent(&Vector6::<f64>::x(), &Vector6::x(), 0.01),
 69 |             true
 70 |         );
 71 | 
 72 |         assert_eq!(
 73 |             linear_dependent(&Vector3::<f64>::x(), &Vector3::y(), 0.01),
 74 |             false
 75 |         );
 76 | 
 77 |         assert_eq!(
 78 |             linear_dependent(&(Vector3::<f64>::x() * 3.5), &Vector3::x(), 0.01),
 79 |             true
 80 |         );
 81 | 
 82 |         assert_eq!(
 83 |             linear_dependent(
 84 |                 &(Vector3::<f64>::x() * 3.5 + 0.01 * Vector3::y()),
 85 |                 &Vector3::x(),
 86 |                 0.001
 87 |             ),
 88 |             false
 89 |         );
 90 | 
 91 |         assert_eq!(
 92 |             linear_dependent(
 93 |                 &(Vector3::<f64>::x() * 3.5 + 0.001 * Vector3::y()),
 94 |                 &Vector3::x(),
 95 |                 0.001
 96 |             ),
 97 |             true
 98 |         );
 99 |     }
100 | }
101 | 


--------------------------------------------------------------------------------
/src/geometry/mod.rs:
--------------------------------------------------------------------------------
1 | pub mod se3;
2 | pub mod so3;
3 | 
4 | pub use se3::*;
5 | pub use so3::*;
6 | 


--------------------------------------------------------------------------------
/src/geometry/se3.rs:
--------------------------------------------------------------------------------
  1 | pub use crate::core::group::LieGroup;
  2 | pub use crate::core::manifold::Manifold;
  3 | use nalgebra::{Matrix6, OMatrix, Vector6, U6};
  4 | 
  5 | use nalgebra as na;
  6 | 
  7 | pub use na::Isometry3 as SE3;
  8 | 
  9 | #[allow(non_snake_case)]
 10 | impl LieGroup<f64> for SE3<f64> {
 11 |     type D = U6;
 12 | 
 13 |     fn between(&self, g: &Self) -> Self {
 14 |         self.inverse() * g
 15 |     }
 16 | 
 17 |     fn adjoint_map(&self) -> OMatrix<f64, U6, U6> {
 18 |         use crate::core::matrix::skew_symmetric;
 19 | 
 20 |         let mut res = OMatrix::<f64, U6, U6>::zeros();
 21 | 
 22 |         let R = self.rotation.to_rotation_matrix();
 23 | 
 24 |         res.fixed_slice_mut::<3, 3>(0, 0).copy_from(&R.matrix());
 25 |         res.fixed_slice_mut::<3, 3>(0, 3).copy_from(
 26 |             &(skew_symmetric(self.translation.x, self.translation.y, self.translation.z) * R),
 27 |         );
 28 |         res.fixed_slice_mut::<3, 3>(3, 3).copy_from(&R.matrix());
 29 | 
 30 |         res
 31 |     }
 32 | 
 33 |     fn logmap(P: &Self, optionalH: Option<&mut Matrix6<f64>>) -> Vector6<f64> {
 34 |         use crate::core::matrix::*;
 35 |         use crate::geometry::so3::*;
 36 | 
 37 |         if let Some(_H) = optionalH {
 38 |             unimplemented!("NOT IMPLEMENTED");
 39 |         }
 40 | 
 41 |         let w = SO3::logmap(&P.rotation.to_rotation_matrix(), None);
 42 |         let T = P.translation.vector;
 43 |         let t = w.norm();
 44 |         if t < 1e-10 {
 45 |             let mut log = Vector6::zeros();
 46 |             log.fixed_slice_mut::<3, 1>(0, 0).copy_from(&w);
 47 |             log.fixed_slice_mut::<3, 1>(3, 0).copy_from(&T);
 48 |             log
 49 |         } else {
 50 |             let W = skew_symmetric_v(&(w / t));
 51 |             // Formula from Agrawal06iros, equation (14)
 52 |             // simplified with Mathematica, and multiplying in T to avoid matrix math
 53 |             let Tan = (0.5 * t).tan();
 54 |             let WT = W * T;
 55 |             let u = T - (0.5 * t) * WT + (1. - t / (2. * Tan)) * (W * WT);
 56 |             let mut log = Vector6::zeros();
 57 |             log.fixed_slice_mut::<3, 1>(0, 0).copy_from(&w);
 58 |             log.fixed_slice_mut::<3, 1>(3, 0).copy_from(&u);
 59 |             log
 60 |         }
 61 |     }
 62 | 
 63 |     fn expmap(xi: &Vector6<f64>) -> Self {
 64 |         Self::expmap_with_derivative(xi, None)
 65 |     }
 66 | 
 67 |     /** Modified from Murray94book version (which assumes w and v normalized?) */
 68 |     #[inline]
 69 |     fn expmap_with_derivative(xi: &Vector6<f64>, optionalH: Option<&mut Matrix6<f64>>) -> Self {
 70 |         use crate::geometry::so3::*;
 71 |         use na::Vector3;
 72 | 
 73 |         if let Some(_H) = optionalH {
 74 |             unimplemented!("NOT IMPLEMENTED");
 75 |         }
 76 | 
 77 |         // get angular velocity omega and translational velocity v from twist xi
 78 |         let (omega, v) = (
 79 |             Vector3::new(xi[0], xi[1], xi[2]),
 80 |             Vector3::new(xi[3], xi[4], xi[5]),
 81 |         );
 82 | 
 83 |         let R = SO3::expmap(&omega);
 84 |         let theta2 = omega.dot(&omega);
 85 |         if theta2 > f64::EPSILON {
 86 |             let t_parallel = omega * omega.dot(&v); // translation parallel to axis
 87 |             let omega_cross_v = omega.cross(&v); // points towards axis
 88 |             let t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
 89 |             SE3::from_parts(t.into(), R.into())
 90 |         } else {
 91 |             SE3::from_parts(v.into(), R.into())
 92 |         }
 93 |     }
 94 | }
 95 | 
 96 | impl Manifold for SE3<f64> {
 97 |     type TangentVector = Vector6<f64>;
 98 | 
 99 |     fn local(origin: &Self, other: &Self) -> Self::TangentVector {
100 |         SE3::logmap(&origin.between(&other), None)
101 |     }
102 | 
103 |     fn retract(origin: &Self, v: &Self::TangentVector) -> Self {
104 |         origin * SE3::expmap(&v)
105 |     }
106 | }
107 | 
108 | #[cfg(test)]
109 | mod test {
110 |     use super::*;
111 | 
112 |     #[test]
113 |     fn test_between() {
114 |         use na::{Rotation3, Vector3};
115 |         let a = SE3::new(Vector3::new(0.1, 0.2, 0.3), Vector3::new(0.1, 0.2, 0.3));
116 |         let b = SE3::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.1, 0.2, 0.3));
117 | 
118 |         assert_relative_eq!(
119 |             (a.between(&b).rotation.to_rotation_matrix().matrix() - Rotation3::identity().matrix())
120 |                 .norm(),
121 |             0.0
122 |         );
123 |     }
124 | 
125 |     #[test]
126 |     fn test_adjoint_map() {
127 |         use na::Vector3;
128 |         let a = SE3::new(Vector3::new(0.1, 0.2, 0.3), Vector3::new(0.1, 0.2, 0.3));
129 | 
130 |         println!("{:}", a.adjoint_map());
131 |     }
132 | 
133 |     #[test]
134 |     fn expmap_logmap_invariant() {
135 |         let w = Vector6::new(1., 1.2, 1.3, 1., 1.4, 1.3);
136 | 
137 |         let exp = SE3::expmap_with_derivative(&w, None);
138 | 
139 |         assert_relative_eq!((w - SE3::logmap(&exp, None)).norm(), 0.0, epsilon = 1e-10);
140 |     }
141 | }
142 | 


--------------------------------------------------------------------------------
/src/geometry/so3.rs:
--------------------------------------------------------------------------------
  1 | pub use crate::core::group::LieGroup;
  2 | pub use crate::core::manifold::Manifold;
  3 | use nalgebra::{Matrix3, OMatrix, Vector3, U3};
  4 | 
  5 | pub use nalgebra::Rotation3 as SO3;
  6 | use std::f64::consts::PI;
  7 | 
  8 | #[allow(non_snake_case)]
  9 | impl LieGroup<f64> for SO3<f64> {
 10 |     type D = U3;
 11 | 
 12 |     fn between(&self, g: &Self) -> Self {
 13 |         self.inverse() * g
 14 |     }
 15 | 
 16 |     fn adjoint_map(&self) -> OMatrix<f64, U3, U3> {
 17 |         *self.matrix()
 18 |     }
 19 | 
 20 |     fn logmap(R: &Self, optionalH: Option<&mut Matrix3<f64>>) -> Vector3<f64> {
 21 |         let (R11, R12, R13) = (R[(0, 0)], R[(0, 1)], R[(0, 2)]);
 22 |         let (R21, R22, R23) = (R[(1, 0)], R[(1, 1)], R[(1, 2)]);
 23 |         let (R31, R32, R33) = (R[(2, 0)], R[(2, 1)], R[(2, 2)]);
 24 | 
 25 |         let tr = R.into_inner().trace();
 26 | 
 27 |         let omega: Vector3<f64>;
 28 | 
 29 |         // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
 30 |         // we do something special
 31 |         if tr + 1.0 < 1e-3 {
 32 |             if (R33 > R22) && (R33 > R11) {
 33 |                 // R33 is the largest diagonal, a=3, b=1, c=2
 34 |                 let W = R21 - R12;
 35 |                 let Q1 = 2.0 + 2.0 * R33;
 36 |                 let Q2 = R31 + R13;
 37 |                 let Q3 = R23 + R32;
 38 |                 let r = Q1.sqrt();
 39 |                 let one_over_r = 1.0 / r;
 40 |                 let norm = (Q1 * Q1 + Q2 * Q2 + Q3 * Q3 + W * W).sqrt();
 41 |                 let sgn_w = if W < 0.0 { -1.0 } else { 1.0 };
 42 |                 let mag = PI - (2.0 * sgn_w * W) / norm;
 43 |                 let scale = 0.5 * one_over_r * mag;
 44 |                 omega = sgn_w * scale * Vector3::new(Q2, Q3, Q1);
 45 |             } else if R22 > R11 {
 46 |                 // R22 is the largest diagonal, a=2, b=3, c=1
 47 |                 let W = R13 - R31;
 48 |                 let Q1 = 2.0 + 2.0 * R22;
 49 |                 let Q2 = R23 + R32;
 50 |                 let Q3 = R12 + R21;
 51 |                 let r = Q1.sqrt();
 52 |                 let one_over_r = 1.0 / r;
 53 |                 let norm = (Q1 * Q1 + Q2 * Q2 + Q3 * Q3 + W * W).sqrt();
 54 |                 let sgn_w = if W < 0.0 { -1.0 } else { 1.0 };
 55 |                 let mag = PI - (2.0 * sgn_w * W) / norm;
 56 |                 let scale = 0.5 * one_over_r * mag;
 57 |                 omega = sgn_w * scale * Vector3::new(Q3, Q1, Q2);
 58 |             } else {
 59 |                 // R11 is the largest diagonal, a=1, b=2, c=3
 60 |                 let W = R32 - R23;
 61 |                 let Q1 = 2.0 + 2.0 * R11;
 62 |                 let Q2 = R12 + R21;
 63 |                 let Q3 = R31 + R13;
 64 |                 let r = Q1.sqrt();
 65 |                 let one_over_r = 1.0 / r;
 66 |                 let norm = (Q1 * Q1 + Q2 * Q2 + Q3 * Q3 + W * W).sqrt();
 67 |                 let sgn_w = if W < 0.0 { -1.0 } else { 1.0 };
 68 |                 let mag = PI - (2.0 * sgn_w * W) / norm;
 69 |                 let scale = 0.5 * one_over_r * mag;
 70 |                 omega = sgn_w * scale * Vector3::new(Q1, Q2, Q3);
 71 |             }
 72 |         } else {
 73 |             let magnitude: f64;
 74 |             let tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal
 75 |             if tr_3 < -1e-6 {
 76 |                 // this is the normal case -1 < trace < 3
 77 |                 let theta = ((tr - 1.0) / 2.0).acos();
 78 |                 magnitude = theta / (2.0 * theta.sin());
 79 |             } else {
 80 |                 // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
 81 |                 // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
 82 |                 // see https://github.com/borglab/gtsam/issues/746 for details
 83 |                 magnitude = 0.5 - tr_3 / 12.0 + tr_3 * tr_3 / 60.0;
 84 |             }
 85 |             omega = magnitude * Vector3::new(R32 - R23, R13 - R31, R21 - R12);
 86 |         }
 87 | 
 88 |         if let Some(_H) = optionalH {
 89 |             unimplemented!("optionalH NOT IMPLEMENTED");
 90 |             // *H = LogmapDerivative(omega);
 91 |         }
 92 | 
 93 |         omega
 94 |     }
 95 | 
 96 |     fn expmap(omega: &Vector3<f64>) -> Self {
 97 |         Self::expmap_with_derivative(omega, None)
 98 |     }
 99 | 
100 |     #[inline]
101 |     fn expmap_with_derivative(omega: &Vector3<f64>, optionalH: Option<&mut Matrix3<f64>>) -> Self {
102 |         let theta2 = omega.dot(omega);
103 |         let nearZero = theta2 <= 1e-5;
104 |         let (wx, wy, wz) = (omega.x, omega.y, omega.z);
105 |         let W = Matrix3::new(0.0, -wz, wy, wz, 0.0, -wx, -wy, wx, 0.0);
106 | 
107 |         if !nearZero {
108 |             let theta = theta2.sqrt();
109 |             let sin_theta = theta.sin();
110 |             let s2 = (theta / 2.).sin();
111 |             let one_minus_cos = 2.0 * s2 * s2;
112 |             let K = W / theta;
113 |             let KK = K * K;
114 | 
115 |             if let Some(H) = optionalH {
116 |                 let a = one_minus_cos / theta;
117 |                 let b = 1.0 - sin_theta / theta;
118 |                 let dexp_ = Matrix3::identity() - a * K + b * KK;
119 | 
120 |                 *H = dexp_;
121 |             }
122 | 
123 |             SO3::from_matrix(&(Matrix3::identity() + sin_theta * K + one_minus_cos * KK))
124 |         } else {
125 |             if let Some(H) = optionalH {
126 |                 *H = Matrix3::identity() - 0.5 * W;
127 |             }
128 | 
129 |             SO3::from_matrix(&(Matrix3::identity() + W))
130 |         }
131 |     }
132 | }
133 | 
134 | impl Manifold for SO3<f64> {
135 |     type TangentVector = Vector3<f64>;
136 | 
137 |     fn local(origin: &Self, other: &Self) -> Self::TangentVector {
138 |         SO3::logmap(&origin.between(&other), None)
139 |     }
140 | 
141 |     fn retract(origin: &Self, v: &Self::TangentVector) -> Self {
142 |         origin * SO3::expmap(&v)
143 |     }
144 | }
145 | 
146 | #[cfg(test)]
147 | mod test {
148 |     use super::*;
149 | 
150 |     #[test]
151 |     fn test_manifold_local() {
152 |         let z = Vector3::new(0., 0., 0.2);
153 |         let v = SO3::new(Vector3::z() * 0.1);
154 |         let w = SO3::new(Vector3::z() * 0.3);
155 | 
156 |         assert_relative_eq!((z - SO3::local(&v, &w)).norm(), 0.0);
157 |     }
158 | 
159 |     #[test]
160 |     fn test_manifold_retract() {
161 |         let z = Vector3::new(0., 0., 0.1);
162 |         let v = SO3::new(Vector3::z() * 0.1);
163 |         let w = SO3::new(Vector3::z() * 0.2);
164 | 
165 |         assert_relative_eq!(
166 |             (w.matrix() - SO3::retract(&v, &z).matrix()).norm(),
167 |             0.0,
168 |             epsilon = 1e-10
169 |         );
170 |     }
171 | }
172 | 


--------------------------------------------------------------------------------
/src/inference/cluster_tree.rs:
--------------------------------------------------------------------------------
  1 | use crate::inference::FactorGraph;
  2 | use std::sync::Arc;
  3 | 
  4 | /// A cluster is just a collection of factors
  5 | #[derive(Debug)]
  6 | pub struct Cluster<Graph: FactorGraph> {
  7 |     frontals: Vec<u64>,
  8 |     children: Vec<Box<Self>>,
  9 |     factors: Graph,
 10 | }
 11 | 
 12 | /// A Cluster Tree is associated with a Factor graph and is defined as in Koller-Friedman:
 13 | /// Each node k represents a subset $`C_k \sub X`$, and the tree is family preserving, in that each factor
 14 | /// $`f_i`$ is associated with a single cluster and $`\text{scope}(f_i) \sub C_k`$.
 15 | #[derive(Debug)]
 16 | pub struct ClusterTree<Graph: FactorGraph> {
 17 |     roots: Vec<Box<Cluster<Graph>>>,
 18 | }
 19 | 
 20 | impl<Graph> Cluster<Graph>
 21 | where
 22 |     Graph: FactorGraph,
 23 | {
 24 |     fn new() -> Self {
 25 |         Cluster {
 26 |             frontals: vec![],
 27 |             children: vec![],
 28 |             factors: Graph::new(),
 29 |         }
 30 |     }
 31 | 
 32 |     fn from_single_key(key: u64, factors: &Vec<Arc<Graph::FactorType>>) -> Self {
 33 |         let mut fg = Graph::new();
 34 |         for f in factors {
 35 |             fg.insert_shared(f.clone());
 36 |         }
 37 |         Cluster {
 38 |             frontals: vec![key],
 39 |             children: vec![],
 40 |             factors: fg,
 41 |         }
 42 |     }
 43 | 
 44 |     fn merge(&mut self, other: &mut Self) {
 45 |         self.frontals.append(&mut other.frontals);
 46 |         self.factors.merge(&mut other.factors);
 47 |         self.children.append(&mut other.children);
 48 |     }
 49 | 
 50 |     /// Add a child cluster
 51 |     /// Apparently this operation taints the Cluster
 52 |     /// as the cluster frontals will be wrong
 53 |     fn add_child(&mut self, child: Box<Self>) {
 54 |         self.children.push(child);
 55 |     }
 56 | 
 57 |     /// Do the merge
 58 |     /// This fixes a tainted cluster
 59 |     fn merge_children(&mut self, merge: Vec<bool>) {
 60 |         let mut old_children = std::mem::replace(&mut self.children, Vec::new());
 61 | 
 62 |         let mut j = 0;
 63 |         for mut child in old_children {
 64 |             if merge[j] {
 65 |                 self.merge(&mut *child);
 66 |             } else {
 67 |                 self.add_child(child);
 68 |             }
 69 |             j += 1;
 70 |         }
 71 | 
 72 |         self.frontals.reverse();
 73 |     }
 74 | }
 75 | 
 76 | impl<Graph> ClusterTree<Graph>
 77 | where
 78 |     Graph: FactorGraph,
 79 | {
 80 |     fn new() -> Self {
 81 |         ClusterTree { roots: Vec::new() }
 82 |     }
 83 | }
 84 | 
 85 | #[cfg(test)]
 86 | mod tests {
 87 |     use crate::inference::cluster_tree::{Cluster, ClusterTree};
 88 |     use crate::inference::factor_graph::*;
 89 |     use std::default::Default;
 90 | 
 91 |     use crate::inference::factor_graph::tests::TestFactor;
 92 | 
 93 |     #[test]
 94 |     fn test_cluster_tree() {
 95 |         let mut cluster = Cluster::<SimpleFactorGraph<TestFactor>>::new();
 96 |         println!("{:?}", cluster);
 97 | 
 98 |         let mut cluster_tree = ClusterTree::<SimpleFactorGraph<TestFactor>>::new();
 99 |         println!("{:?}", cluster_tree);
100 | 
101 |         let mut factors = SimpleFactorGraph::new();
102 |         factors.insert(TestFactor {
103 |             inner: "cluster0".into(),
104 |             _keys: [0].into(),
105 |         });
106 |         let mut cluster_1 =
107 |             Cluster::<SimpleFactorGraph<TestFactor>>::from_single_key(0, &factors.factors);
108 |         println!("{:#?}", cluster_1)
109 |     }
110 | 
111 |     #[test]
112 |     fn test_merge_add() {
113 |         let mut factors_0 = SimpleFactorGraph::new();
114 |         factors_0.insert(TestFactor {
115 |             inner: "cluster0".into(),
116 |             _keys: [0].into(),
117 |         });
118 |         let mut cluster_0 =
119 |             Cluster::<SimpleFactorGraph<TestFactor>>::from_single_key(0, &mut factors_0.factors);
120 | 
121 |         let mut factors_1 = SimpleFactorGraph::new();
122 |         factors_1.insert(TestFactor {
123 |             inner: "cluster1".into(),
124 |             _keys: [1].into(),
125 |         });
126 |         let mut cluster_1 =
127 |             Cluster::<SimpleFactorGraph<TestFactor>>::from_single_key(1, &mut factors_1.factors);
128 | 
129 |         cluster_0.merge(&mut cluster_1);
130 | 
131 |         println!("Merged = {:#?}", cluster_0);
132 | 
133 |         let mut factors_2 = SimpleFactorGraph::new();
134 |         factors_2.insert(TestFactor {
135 |             inner: "cluster2".into(),
136 |             _keys: [2].into(),
137 |         });
138 | 
139 |         let mut cluster_2 =
140 |             Cluster::<SimpleFactorGraph<TestFactor>>::from_single_key(2, &mut factors_2.factors);
141 | 
142 |         cluster_0.add_child(Box::new(cluster_2));
143 | 
144 |         println!("Inserted = {:#?}", cluster_0);
145 | 
146 |         let mut factors_3 = SimpleFactorGraph::new();
147 |         factors_3.insert(TestFactor {
148 |             inner: "cluster3".into(),
149 |             _keys: [3].into(),
150 |         });
151 | 
152 |         let mut cluster_3 =
153 |             Cluster::<SimpleFactorGraph<TestFactor>>::from_single_key(3, &mut factors_3.factors);
154 | 
155 |         cluster_0.add_child(Box::new(cluster_3));
156 | 
157 |         cluster_0.merge_children([true, true].into());
158 | 
159 |         println!("merge_children = {:?}", cluster_0);
160 |         assert_eq!(cluster_0.frontals, vec![3, 2, 1, 0]);
161 |         assert_eq!(cluster_0.factors.size(), 4);
162 |     }
163 | }
164 | 


--------------------------------------------------------------------------------
/src/inference/conditional.rs:
--------------------------------------------------------------------------------
1 | pub trait Conditional<FactorType> {
2 |     fn num_frontals(&self) -> usize;
3 |     fn num_parents(&self) -> usize;
4 | 
5 |     fn frontals<'a>(&self) -> Box<dyn Iterator<Item = &'a u64> + 'a>;
6 |     fn parents<'a>(&self) -> Box<dyn Iterator<Item = &'a u64> + 'a>;
7 | }
8 | 


--------------------------------------------------------------------------------
/src/inference/expression/factor_graph.rs:
--------------------------------------------------------------------------------
 1 | use crate::inference::factor::*;
 2 | use crate::inference::factor_graph::*;
 3 | 
 4 | #[derive(Debug, Clone)]
 5 | pub struct ExpressionFactor {}
 6 | 
 7 | impl NonlinearFactor for ExpressionFactor {}
 8 | 
 9 | impl Factor for ExpressionFactor {
10 |     fn num_keys(&self) -> usize {
11 |         unimplemented!()
12 |     }
13 | 
14 |     fn key_at(&self, index: usize) -> Result<KeyType, std::io::Error> {
15 |         unimplemented!()
16 |     }
17 | }
18 | 
19 | impl SimpleFactorGraph<ExpressionFactor> {}
20 | 


--------------------------------------------------------------------------------
/src/inference/expression/mod.rs:
--------------------------------------------------------------------------------
1 | pub mod factor_graph;
2 | 


--------------------------------------------------------------------------------
/src/inference/factor.rs:
--------------------------------------------------------------------------------
 1 | extern crate alloc;
 2 | 
 3 | pub type KeyType = u64;
 4 | 
 5 | pub struct KeyIterator<'a> {
 6 |     owner: &'a dyn Factor,
 7 |     index: usize,
 8 | }
 9 | 
10 | impl<'a> Iterator for KeyIterator<'a> {
11 |     type Item = KeyType;
12 | 
13 |     fn next(&mut self) -> Option<Self::Item> {
14 |         self.owner.key_at(self.index - 1).ok()
15 |     }
16 | }
17 | 
18 | pub trait Factor: std::fmt::Debug {
19 |     fn num_keys(&self) -> usize;
20 |     fn key_at(&self, index: usize) -> Result<KeyType, std::io::Error>;
21 | }
22 | 
23 | pub trait NonlinearFactor: Factor {}
24 | 


--------------------------------------------------------------------------------
/src/inference/factor_graph.rs:
--------------------------------------------------------------------------------
  1 | extern crate alloc;
  2 | 
  3 | use crate::inference::Factor;
  4 | use alloc::sync::Arc;
  5 | use alloc::vec::Vec;
  6 | 
  7 | pub struct FactorIterator<'a, FactorGraphType>
  8 | where
  9 |     Self: Iterator,
 10 | {
 11 |     owner: &'a FactorGraphType,
 12 |     index: usize,
 13 | }
 14 | 
 15 | pub trait FactorGraph
 16 | where
 17 |     Self: Sized,
 18 | {
 19 |     type FactorType: ?Sized;
 20 | 
 21 |     fn new() -> Self;
 22 | 
 23 |     fn insert(&mut self, f: Self::FactorType)
 24 |     where
 25 |         Self::FactorType: Sized;
 26 | 
 27 |     fn merge(&mut self, other: &mut Self);
 28 | 
 29 |     fn insert_shared(&mut self, f: Arc<Self::FactorType>);
 30 | 
 31 |     fn iter(&self) -> Box<dyn Iterator<Item = &Self::FactorType> + '_>
 32 |     where
 33 |         Self::FactorType: Sized;
 34 | 
 35 |     fn size(&self) -> usize;
 36 | }
 37 | 
 38 | #[derive(Debug)]
 39 | pub struct SimpleFactorGraph<FactorType: ?Sized>
 40 | where
 41 |     FactorType: Factor,
 42 | {
 43 |     pub factors: Vec<Arc<FactorType>>,
 44 | }
 45 | 
 46 | impl<FACTOR: ?Sized> FactorGraph for SimpleFactorGraph<FACTOR>
 47 | where
 48 |     FACTOR: Factor,
 49 | {
 50 |     type FactorType = FACTOR;
 51 | 
 52 |     fn new() -> Self {
 53 |         Self { factors: vec![] }
 54 |     }
 55 | 
 56 |     fn insert(&mut self, f: Self::FactorType)
 57 |     where
 58 |         Self::FactorType: Sized,
 59 |     {
 60 |         self.factors.push(Arc::new(f));
 61 |     }
 62 | 
 63 |     fn merge(&mut self, other: &mut Self) {
 64 |         self.factors.append(&mut other.factors);
 65 |     }
 66 | 
 67 |     fn insert_shared(&mut self, f: Arc<Self::FactorType>) {
 68 |         self.factors.push(f);
 69 |     }
 70 | 
 71 |     fn iter(&self) -> Box<dyn Iterator<Item = &Self::FactorType> + '_>
 72 |     where
 73 |         Self::FactorType: Sized,
 74 |     {
 75 |         Box::new(FactorIterator {
 76 |             owner: self,
 77 |             index: 0,
 78 |         })
 79 |     }
 80 | 
 81 |     fn size(&self) -> usize {
 82 |         self.factors.len()
 83 |     }
 84 | }
 85 | 
 86 | impl<'a, FACTOR> Iterator for FactorIterator<'a, SimpleFactorGraph<FACTOR>>
 87 | where
 88 |     FACTOR: Factor,
 89 | {
 90 |     type Item = &'a FACTOR;
 91 | 
 92 |     fn next(&mut self) -> Option<Self::Item> {
 93 |         match self.owner.factors.get(self.index) {
 94 |             Some(a) => {
 95 |                 self.index += 1;
 96 |                 Some(&(*a))
 97 |             }
 98 |             None => None,
 99 |         }
100 |     }
101 | }
102 | 
103 | pub trait EliminateableFactorGraph<FactorType>: FactorGraph
104 | where
105 |     FactorType: Factor,
106 | {
107 | }
108 | 
109 | #[cfg(test)]
110 | pub mod tests {
111 |     use super::*;
112 |     use crate::inference::factor::KeyType;
113 |     use alloc::sync::Arc;
114 |     use std::io::ErrorKind;
115 |     use std::sync::Mutex;
116 |     use std::thread;
117 | 
118 |     #[derive(Debug)]
119 |     pub struct TestFactor {
120 |         pub inner: String,
121 |         pub _keys: Vec<u64>,
122 |     }
123 | 
124 |     impl Default for TestFactor {
125 |         fn default() -> TestFactor {
126 |             TestFactor {
127 |                 inner: "".into(),
128 |                 _keys: Vec::new(),
129 |             }
130 |         }
131 |     }
132 | 
133 |     impl Factor for TestFactor {
134 |         fn num_keys(&self) -> usize {
135 |             self._keys.len()
136 |         }
137 | 
138 |         fn key_at(&self, index: usize) -> Result<KeyType, std::io::Error> {
139 |             self._keys
140 |                 .get(index)
141 |                 .cloned()
142 |                 .ok_or(std::io::Error::new(ErrorKind::OutOfMemory, "Range"))
143 |         }
144 |     }
145 | 
146 |     impl SimpleFactorGraph<TestFactor> {
147 |         pub fn test(&self) -> String {
148 |             let mut res = String::new();
149 |             for i in self.factors.iter() {
150 |                 res += format!("Inside thread: {:?}\n", i).as_str();
151 |             }
152 |             res
153 |         }
154 |     }
155 | 
156 |     #[test]
157 |     fn test_factor_graph_threaded() {
158 |         let mut fg = SimpleFactorGraph::<TestFactor> {
159 |             factors: Vec::<Arc<TestFactor>>::new(),
160 |         };
161 | 
162 |         fg.factors.push(Arc::new(TestFactor {
163 |             inner: "TEST0".into(),
164 |             ..Default::default()
165 |         }));
166 |         fg.factors.push(Arc::new(TestFactor {
167 |             inner: "TEST1".into(),
168 |             ..Default::default()
169 |         }));
170 |         fg.factors.push(Arc::new(TestFactor {
171 |             inner: "TEST2".into(),
172 |             ..Default::default()
173 |         }));
174 |         fg.factors.push(Arc::new(TestFactor {
175 |             inner: "TEST3".into(),
176 |             ..Default::default()
177 |         }));
178 | 
179 |         // test iterator
180 |         for i in fg.iter() {
181 |             println!("{}", i.inner);
182 |         }
183 | 
184 |         let ptr = Arc::new(Mutex::new(fg));
185 | 
186 |         {
187 |             let ptr = ptr.clone();
188 |             let handle = thread::spawn(move || {
189 |                 return ptr.lock().unwrap().test();
190 |             });
191 |             println!("{}", handle.join().unwrap());
192 |         }
193 |     }
194 | 
195 |     #[test]
196 |     fn test_trait_object() {
197 |         let mut fg = SimpleFactorGraph::<dyn Factor> { factors: vec![] };
198 | 
199 |         fg.insert_shared(Arc::new(TestFactor {
200 |             inner: "TEST0".into(),
201 |             ..Default::default()
202 |         }));
203 |         fg.factors.push(Arc::new(TestFactor {
204 |             inner: "TEST1".into(),
205 |             ..Default::default()
206 |         }));
207 |         fg.factors.push(Arc::new(TestFactor {
208 |             inner: "TEST2".into(),
209 |             ..Default::default()
210 |         }));
211 |         fg.factors.push(Arc::new(TestFactor {
212 |             inner: "TEST3".into(),
213 |             ..Default::default()
214 |         }));
215 | 
216 |         println!("{:#?}", fg);
217 |     }
218 | }
219 | 


--------------------------------------------------------------------------------
/src/inference/junction_tree.rs:
--------------------------------------------------------------------------------
1 | 
2 | 


--------------------------------------------------------------------------------
/src/inference/mod.rs:
--------------------------------------------------------------------------------
 1 | pub mod cluster_tree;
 2 | pub mod conditional;
 3 | pub mod expression;
 4 | pub mod factor;
 5 | pub mod factor_graph;
 6 | pub mod junction_tree;
 7 | 
 8 | pub use conditional::Conditional;
 9 | pub use factor::Factor;
10 | pub use factor_graph::{EliminateableFactorGraph, FactorGraph};
11 | 


--------------------------------------------------------------------------------
/src/lib.rs:
--------------------------------------------------------------------------------
 1 | #[cfg(test)]
 2 | #[macro_use]
 3 | extern crate approx;
 4 | 
 5 | pub mod core;
 6 | pub mod geometry;
 7 | pub mod inference;
 8 | pub mod linear;
 9 | pub mod nonlinear;
10 | 


--------------------------------------------------------------------------------
/src/linear/gaussian.rs:
--------------------------------------------------------------------------------
 1 | use crate::inference::factor::{Factor, KeyType};
 2 | use crate::linear::gaussian_like::GaussianLikeFactor;
 3 | use nalgebra as na;
 4 | 
 5 | #[derive(Debug)]
 6 | pub struct GaussianFactor {}
 7 | 
 8 | impl Factor for GaussianFactor {
 9 |     fn num_keys(&self) -> usize {
10 |         todo!()
11 |     }
12 | 
13 |     fn key_at(&self, index: usize) -> Result<KeyType, std::io::Error> {
14 |         todo!()
15 |     }
16 | }
17 | 
18 | impl GaussianLikeFactor for GaussianFactor {
19 |     fn augmented_jacobian(&self) -> na::DMatrix<f64> {
20 |         unimplemented!()
21 |     }
22 | 
23 |     fn jacobian(
24 |         &self,
25 |     ) -> (
26 |         na::OMatrix<f64, na::Dynamic, na::Dynamic>,
27 |         na::OVector<f64, na::Dynamic>,
28 |     ) {
29 |         unimplemented!()
30 |     }
31 | 
32 |     fn augmented_information(&self) -> na::OMatrix<f64, na::Dynamic, na::Dynamic> {
33 |         unimplemented!()
34 |     }
35 | 
36 |     fn information(&self) -> na::OMatrix<f64, na::Dynamic, na::Dynamic> {
37 |         unimplemented!()
38 |     }
39 | 
40 |     fn hessian_diagonal(&self) -> Vec<(u64, na::OVector<f64, na::Dynamic>)> {
41 |         unimplemented!()
42 |     }
43 | 
44 |     fn hessian_block_diagonal(&self) -> Vec<(u64, na::OMatrix<f64, na::Dynamic, na::Dynamic>)> {
45 |         unimplemented!()
46 |     }
47 | }
48 | 


--------------------------------------------------------------------------------
/src/linear/gaussian_factor_graph.rs:
--------------------------------------------------------------------------------
 1 | //use crate::inference::{Factor, FactorGraph};
 2 | //use std::sync::Arc;
 3 | //use crate::linear::GaussianLikeFactor;
 4 | //
 5 | //#[derive(Debug)]
 6 | //pub struct GaussianFactorGraph
 7 | //{
 8 | //    factors: Vec<Arc<GaussianLikeFactorFactor>>,
 9 | //}
10 | //
11 | //impl FactorGraph<dyn GaussianLikeFactor> for GaussianFactorGraph {
12 | //
13 | //}
14 | 


--------------------------------------------------------------------------------
/src/linear/gaussian_like.rs:
--------------------------------------------------------------------------------
 1 | use crate::inference::Conditional;
 2 | use crate::inference::Factor;
 3 | 
 4 | use nalgebra as na;
 5 | 
 6 | pub trait GaussianLikeFactor: Factor {
 7 |     fn augmented_jacobian(&self) -> na::OMatrix<f64, na::Dynamic, na::Dynamic>;
 8 | 
 9 |     fn jacobian(
10 |         &self,
11 |     ) -> (
12 |         na::OMatrix<f64, na::Dynamic, na::Dynamic>,
13 |         na::OVector<f64, na::Dynamic>,
14 |     );
15 | 
16 |     fn augmented_information(&self) -> na::OMatrix<f64, na::Dynamic, na::Dynamic>;
17 | 
18 |     fn information(&self) -> na::OMatrix<f64, na::Dynamic, na::Dynamic>;
19 | 
20 |     fn hessian_diagonal(&self) -> Vec<(u64, na::OVector<f64, na::Dynamic>)>;
21 | 
22 |     fn hessian_block_diagonal(&self) -> Vec<(u64, na::OMatrix<f64, na::Dynamic, na::Dynamic>)>;
23 | }
24 | 
25 | pub trait GaussianConditional<F: GaussianLikeFactor>: Conditional<F> {}
26 | 


--------------------------------------------------------------------------------
/src/linear/jacobian.rs:
--------------------------------------------------------------------------------
 1 | use crate::inference::factor::{Factor, KeyType};
 2 | use crate::linear::gaussian_like::GaussianLikeFactor;
 3 | use nalgebra as na;
 4 | 
 5 | #[derive(Debug)]
 6 | pub struct JacobianFactor {}
 7 | 
 8 | impl Factor for JacobianFactor {
 9 |     fn num_keys(&self) -> usize {
10 |         todo!()
11 |     }
12 | 
13 |     fn key_at(&self, index: usize) -> Result<KeyType, std::io::Error> {
14 |         todo!()
15 |     }
16 | }
17 | 
18 | impl GaussianLikeFactor for JacobianFactor {
19 |     fn augmented_jacobian(&self) -> na::DMatrix<f64> {
20 |         unimplemented!()
21 |     }
22 | 
23 |     fn jacobian(
24 |         &self,
25 |     ) -> (
26 |         na::OMatrix<f64, na::Dynamic, na::Dynamic>,
27 |         na::OVector<f64, na::Dynamic>,
28 |     ) {
29 |         unimplemented!()
30 |     }
31 | 
32 |     fn augmented_information(&self) -> na::OMatrix<f64, na::Dynamic, na::Dynamic> {
33 |         unimplemented!()
34 |     }
35 | 
36 |     fn information(&self) -> na::OMatrix<f64, na::Dynamic, na::Dynamic> {
37 |         unimplemented!()
38 |     }
39 | 
40 |     fn hessian_diagonal(&self) -> Vec<(u64, na::OVector<f64, na::Dynamic>)> {
41 |         unimplemented!()
42 |     }
43 | 
44 |     fn hessian_block_diagonal(&self) -> Vec<(u64, na::OMatrix<f64, na::Dynamic, na::Dynamic>)> {
45 |         unimplemented!()
46 |     }
47 | }
48 | 


--------------------------------------------------------------------------------
/src/linear/mod.rs:
--------------------------------------------------------------------------------
1 | pub mod gaussian;
2 | pub mod gaussian_factor_graph;
3 | pub mod gaussian_like;
4 | pub mod jacobian;
5 | pub mod noise_model;
6 | 
7 | pub use gaussian_like::GaussianLikeFactor;
8 | 


--------------------------------------------------------------------------------
/src/linear/noise_model/diagonal.rs:
--------------------------------------------------------------------------------
  1 | use nalgebra::base::allocator::Allocator;
  2 | use nalgebra::base::default_allocator::DefaultAllocator;
  3 | use nalgebra::base::dimension::Dim;
  4 | use nalgebra::base::{DMatrix, DVector, OMatrix, OVector};
  5 | use nalgebra::RealField;
  6 | use std::fmt::Debug;
  7 | 
  8 | use super::*;
  9 | 
 10 | #[derive(Debug)]
 11 | pub struct Diagonal<D: Dim, T: RealField + Copy = f64>
 12 | where
 13 |     DefaultAllocator: Allocator<T, D>,
 14 | {
 15 |     dim: usize,
 16 |     sigmas_: OVector<T, D>,
 17 |     invsigmas_: OVector<T, D>,
 18 |     precisions_: OVector<T, D>,
 19 | }
 20 | 
 21 | #[allow(non_snake_case)]
 22 | impl<D: Dim, T: RealField + Copy> GaussianNoise<D, T> for Diagonal<D, T>
 23 | where
 24 |     DefaultAllocator: Allocator<T, D>,
 25 | {
 26 |     fn from_sqrtinfo(_R: &OMatrix<T, D, D>, _smart: bool) -> Self
 27 |     where
 28 |         DefaultAllocator: Allocator<T, D, D>,
 29 |     {
 30 |         unimplemented!() // TODO
 31 |     }
 32 | 
 33 |     fn from_information(_info: &OMatrix<T, D, D>, _smart: bool) -> Self
 34 |     where
 35 |         DefaultAllocator: Allocator<T, D, D>,
 36 |     {
 37 |         unimplemented!()
 38 |     }
 39 | 
 40 |     fn from_covariance(_cov: &OMatrix<T, D, D>, _smart: bool) -> Self
 41 |     where
 42 |         DefaultAllocator: Allocator<T, D, D>,
 43 |     {
 44 |         unimplemented!() // TODO
 45 |     }
 46 | 
 47 |     fn sqrt_info(&self) -> Option<&OMatrix<T, D, D>>
 48 |     where
 49 |         DefaultAllocator: Allocator<T, D, D>,
 50 |     {
 51 |         unimplemented!() // TODO
 52 |     }
 53 | 
 54 |     /**
 55 |      * Mahalanobis distance v'*R'*R*v = <R*v,R*v>
 56 |      */
 57 |     fn mahalanobis_dist(&self, v: &OVector<T, D>) -> T
 58 |     where
 59 |         DefaultAllocator: Allocator<T, D>,
 60 |     {
 61 |         let w = self.whiten(v);
 62 |         w.dot(&w)
 63 |     }
 64 | }
 65 | 
 66 | #[allow(non_snake_case)]
 67 | impl<D: Dim, T: RealField + Copy> NoiseModel<D, T> for Diagonal<D, T>
 68 | where
 69 |     DefaultAllocator: Allocator<T, D>,
 70 | {
 71 |     fn is_constrained(&self) -> bool {
 72 |         unimplemented!()
 73 |     }
 74 | 
 75 |     fn is_unit(&self) -> bool {
 76 |         unimplemented!()
 77 |     }
 78 | 
 79 |     fn dim(&self) -> usize {
 80 |         self.dim
 81 |     }
 82 | 
 83 |     fn sigmas(&self) -> DVector<T> {
 84 |         unimplemented!()
 85 |     }
 86 | 
 87 |     fn whiten(&self, _v: &OVector<T, D>) -> OVector<T, D>
 88 |     where
 89 |         DefaultAllocator: Allocator<T, D>,
 90 |     {
 91 |         unimplemented!()
 92 |     }
 93 | 
 94 |     fn whiten_mat(&self, _m: &OMatrix<T, D, D>) -> OMatrix<T, D, D>
 95 |     where
 96 |         DefaultAllocator: Allocator<T, D, D>,
 97 |     {
 98 |         unimplemented!()
 99 |     }
100 | 
101 |     fn unwhiten(&self, _v: &OVector<T, D>) -> OVector<T, D>
102 |     where
103 |         DefaultAllocator: Allocator<T, D>,
104 |     {
105 |         unimplemented!()
106 |     }
107 | 
108 |     fn distance(&self, _v: &OVector<T, D>) -> T
109 |     where
110 |         DefaultAllocator: Allocator<T, D>,
111 |     {
112 |         unimplemented!()
113 |     }
114 | 
115 |     fn whiten_system<_D: Dim>(&self, _A: &[DMatrix<T>], _b: &OVector<T, _D>)
116 |     where
117 |         DefaultAllocator: Allocator<T, _D>,
118 |     {
119 |         unimplemented!()
120 |     }
121 | }
122 | 


--------------------------------------------------------------------------------
/src/linear/noise_model/gaussian.rs:
--------------------------------------------------------------------------------
  1 | use nalgebra::base::allocator::Allocator;
  2 | use nalgebra::base::default_allocator::DefaultAllocator;
  3 | use nalgebra::base::dimension::Dim;
  4 | use nalgebra::base::{DMatrix, DVector, OMatrix, OVector};
  5 | use nalgebra::DimSub;
  6 | use nalgebra::RealField;
  7 | use std::fmt::Debug;
  8 | 
  9 | use super::*;
 10 | 
 11 | #[derive(Debug)]
 12 | pub struct Gaussian<D: Dim, T: RealField + Copy = f64>
 13 | where
 14 |     DefaultAllocator: Allocator<T, D, D>,
 15 | {
 16 |     dim: usize,
 17 |     sqrt_info: Option<OMatrix<T, D, D>>,
 18 | }
 19 | 
 20 | #[allow(non_snake_case)]
 21 | impl<D: Dim, T: RealField + Copy> GaussianNoise<D, T> for Gaussian<D, T>
 22 | where
 23 |     DefaultAllocator: Allocator<T, D, D>,
 24 | {
 25 |     fn from_sqrtinfo(R: &OMatrix<T, D, D>, smart: bool) -> Self
 26 |     where
 27 |         DefaultAllocator: Allocator<T, D, D>,
 28 |     {
 29 |         let (m, n) = (R.nrows(), R.ncols());
 30 |         assert_eq!(m, n, "Non-square Matrix");
 31 |         if smart {
 32 |             if let Some(_diagonal) = check_diagonal_upper(&R) {
 33 |                 unimplemented!();
 34 |             }
 35 |         }
 36 | 
 37 |         Gaussian {
 38 |             dim: R.nrows(),
 39 |             sqrt_info: Some(R.to_owned()),
 40 |         }
 41 |     }
 42 | 
 43 |     fn from_information(info: &OMatrix<T, D, D>, _smart: bool) -> Self
 44 |     where
 45 |         DefaultAllocator: Allocator<T, D, D>,
 46 |         D: DimSub<nalgebra::Dynamic>,
 47 |     {
 48 |         use nalgebra::Cholesky;
 49 | 
 50 |         let (m, n) = (info.nrows(), info.ncols());
 51 |         assert_eq!(m, n, "Non-square Matrix");
 52 | 
 53 |         let llt = Cholesky::new(info.clone()).unwrap();
 54 |         let R = llt.l_dirty();
 55 | 
 56 |         return Gaussian {
 57 |             dim: m,
 58 |             sqrt_info: Some(R.transpose()),
 59 |         };
 60 |     }
 61 | 
 62 |     fn from_covariance(cov: &OMatrix<T, D, D>, smart: bool) -> Self
 63 |     where
 64 |         DefaultAllocator: Allocator<T, D, D>,
 65 |         D: DimSub<nalgebra::Dynamic>,
 66 |     {
 67 |         let (m, n) = (cov.nrows(), cov.ncols());
 68 |         assert_eq!(m, n, "Non-square Matrix");
 69 |         if smart {
 70 |             if let Some(_diagonal) = check_diagonal_upper(cov) {
 71 |                 unimplemented!();
 72 |             }
 73 |         }
 74 | 
 75 |         // NOTE: if cov = L'*L, then the square root information R can be found by
 76 |         // QR, as L.inverse() = Q*R, with Q some rotation matrix. However, R has
 77 |         // annoying sign flips with respect to the simpler Information(inv(cov)),
 78 |         // hence we choose the simpler path here:
 79 |         let inv = cov.clone().try_inverse();
 80 |         Gaussian::from_information(&inv.unwrap(), false)
 81 |     }
 82 | 
 83 |     fn sqrt_info(&self) -> Option<&OMatrix<T, D, D>>
 84 |     where
 85 |         DefaultAllocator: Allocator<T, D, D>,
 86 |     {
 87 |         if let Some(s) = &self.sqrt_info {
 88 |             return Some(s);
 89 |         }
 90 |         None
 91 |     }
 92 | 
 93 |     /**
 94 |      * Mahalanobis distance v'*R'*R*v = <R*v,R*v>
 95 |      */
 96 |     fn mahalanobis_dist(&self, v: &OVector<T, D>) -> T
 97 |     where
 98 |         DefaultAllocator: Allocator<T, D>,
 99 |     {
100 |         let w = self.whiten(v);
101 |         w.dot(&w)
102 |     }
103 | }
104 | 
105 | #[allow(non_snake_case)]
106 | impl<D: Dim, T: RealField + Copy> NoiseModel<D, T> for Gaussian<D, T>
107 | where
108 |     DefaultAllocator: Allocator<T, D, D>,
109 | {
110 |     fn is_constrained(&self) -> bool {
111 |         unimplemented!()
112 |     }
113 | 
114 |     fn is_unit(&self) -> bool {
115 |         unimplemented!()
116 |     }
117 | 
118 |     fn dim(&self) -> usize {
119 |         self.dim
120 |     }
121 | 
122 |     fn sigmas(&self) -> DVector<T> {
123 |         unimplemented!()
124 |     }
125 | 
126 |     fn whiten(&self, v: &OVector<T, D>) -> OVector<T, D>
127 |     where
128 |         DefaultAllocator: Allocator<T, D, D> + Allocator<T, D>,
129 |     {
130 |         if let Some(R) = self.sqrt_info() {
131 |             R * v
132 |         } else {
133 |             panic!("SqrtInfo Undefined")
134 |         }
135 |     }
136 | 
137 |     fn whiten_mat(&self, m: &OMatrix<T, D, D>) -> OMatrix<T, D, D>
138 |     where
139 |         DefaultAllocator: Allocator<T, D, D>,
140 |     {
141 |         if let Some(R) = self.sqrt_info() {
142 |             R * m
143 |         } else {
144 |             panic!("SqrtInfo Undefined")
145 |         }
146 |     }
147 | 
148 |     fn unwhiten(&self, _v: &OVector<T, D>) -> OVector<T, D>
149 |     where
150 |         DefaultAllocator: Allocator<T, D, D> + Allocator<T, D>,
151 |     {
152 |         unimplemented!()
153 |     }
154 | 
155 |     fn distance(&self, v: &OVector<T, D>) -> T
156 |     where
157 |         DefaultAllocator: Allocator<T, D, D> + Allocator<T, D>,
158 |     {
159 |         self.mahalanobis_dist(v)
160 |     }
161 | 
162 |     fn whiten_system<_D: Dim>(&self, _A: &[DMatrix<T>], _b: &OVector<T, _D>)
163 |     where
164 |         DefaultAllocator: Allocator<T, D, D> + Allocator<T, _D>,
165 |     {
166 |         unimplemented!()
167 |     }
168 | }
169 | 
170 | #[cfg(test)]
171 | mod tests {
172 |     use super::*;
173 |     use nalgebra::Matrix4;
174 | 
175 |     #[test]
176 |     fn gaussian_model_construction() {
177 |         let si = DMatrix::<f64>::identity(4, 4);
178 |         let g = Gaussian::from_sqrtinfo(&si, false);
179 | 
180 |         let se = Matrix4::<f64>::identity();
181 |         let ge = Gaussian::from_information(&se, false);
182 | 
183 |         let lhs = g.sqrt_info().unwrap();
184 |         let rhs = ge.sqrt_info().unwrap();
185 |         assert_eq!((lhs - rhs).norm(), 0.0);
186 | 
187 |         println!("{:#?}", ge.sqrt_info());
188 |     }
189 | 
190 |     #[test]
191 |     fn sqrt_info_vs_cov_invariant() {
192 |         let _si = DMatrix::<f64>::identity(4, 4);
193 |         let _cm = DMatrix::<f64>::identity(4, 4);
194 |     }
195 | }
196 | 


--------------------------------------------------------------------------------
/src/linear/noise_model/isotropic.rs:
--------------------------------------------------------------------------------
  1 | use nalgebra::base::allocator::Allocator;
  2 | use nalgebra::base::default_allocator::DefaultAllocator;
  3 | use nalgebra::base::dimension::Dim;
  4 | use nalgebra::base::{DMatrix, DVector, OMatrix, OVector};
  5 | use nalgebra::RealField;
  6 | 
  7 | use super::*;
  8 | 
  9 | #[derive(Debug)]
 10 | pub struct Isotropic<D: Dim, T: RealField + Copy = f64> {
 11 |     dim: usize,
 12 |     sigma_: T,
 13 |     invsigma_: T,
 14 |     _phantom: std::marker::PhantomData<D>,
 15 | }
 16 | 
 17 | #[allow(non_snake_case)]
 18 | impl<D: Dim, T: RealField + Copy> GaussianNoise<D, T> for Isotropic<D, T>
 19 | where
 20 |     DefaultAllocator: Allocator<T, D>,
 21 | {
 22 |     fn from_sqrtinfo(_R: &OMatrix<T, D, D>, _smart: bool) -> Self
 23 |     where
 24 |         DefaultAllocator: Allocator<T, D, D>,
 25 |     {
 26 |         unimplemented!() // TODO
 27 |     }
 28 | 
 29 |     fn from_information(_info: &OMatrix<T, D, D>, _smart: bool) -> Self
 30 |     where
 31 |         DefaultAllocator: Allocator<T, D, D>,
 32 |     {
 33 |         unimplemented!()
 34 |     }
 35 | 
 36 |     fn from_covariance(_cov: &OMatrix<T, D, D>, _smart: bool) -> Self
 37 |     where
 38 |         DefaultAllocator: Allocator<T, D, D>,
 39 |     {
 40 |         unimplemented!() // TODO
 41 |     }
 42 | 
 43 |     fn sqrt_info(&self) -> Option<&OMatrix<T, D, D>>
 44 |     where
 45 |         DefaultAllocator: Allocator<T, D, D>,
 46 |     {
 47 |         unimplemented!() // TODO
 48 |     }
 49 | 
 50 |     /**
 51 |      * Mahalanobis distance v'*R'*R*v = <R*v,R*v>
 52 |      */
 53 |     fn mahalanobis_dist(&self, v: &OVector<T, D>) -> T
 54 |     where
 55 |         DefaultAllocator: Allocator<T, D>,
 56 |     {
 57 |         v.dot(v) * self.invsigma_ * self.invsigma_
 58 |     }
 59 | }
 60 | 
 61 | #[allow(non_snake_case)]
 62 | impl<D: Dim, T: RealField + Copy> NoiseModel<D, T> for Isotropic<D, T>
 63 | where
 64 |     DefaultAllocator: Allocator<T, D>,
 65 | {
 66 |     fn is_constrained(&self) -> bool {
 67 |         unimplemented!()
 68 |     }
 69 | 
 70 |     fn is_unit(&self) -> bool {
 71 |         unimplemented!()
 72 |     }
 73 | 
 74 |     fn dim(&self) -> usize {
 75 |         self.dim
 76 |     }
 77 | 
 78 |     fn sigmas(&self) -> DVector<T> {
 79 |         unimplemented!()
 80 |     }
 81 | 
 82 |     fn whiten(&self, v: &OVector<T, D>) -> OVector<T, D>
 83 |     where
 84 |         DefaultAllocator: Allocator<T, D>,
 85 |     {
 86 |         v * self.invsigma_
 87 |     }
 88 | 
 89 |     fn whiten_mat(&self, _m: &OMatrix<T, D, D>) -> OMatrix<T, D, D>
 90 |     where
 91 |         DefaultAllocator: Allocator<T, D, D>,
 92 |     {
 93 |         unimplemented!()
 94 |     }
 95 | 
 96 |     fn unwhiten(&self, _v: &OVector<T, D>) -> OVector<T, D>
 97 |     where
 98 |         DefaultAllocator: Allocator<T, D>,
 99 |     {
100 |         unimplemented!()
101 |     }
102 | 
103 |     fn distance(&self, _v: &OVector<T, D>) -> T
104 |     where
105 |         DefaultAllocator: Allocator<T, D>,
106 |     {
107 |         unimplemented!()
108 |     }
109 | 
110 |     fn whiten_system<_D: Dim>(&self, _A: &[DMatrix<T>], _b: &OVector<T, _D>)
111 |     where
112 |         DefaultAllocator: Allocator<T, _D>,
113 |     {
114 |         unimplemented!()
115 |     }
116 | }
117 | 


--------------------------------------------------------------------------------
/src/linear/noise_model/mod.rs:
--------------------------------------------------------------------------------
  1 | pub mod diagonal;
  2 | pub mod gaussian;
  3 | pub mod isotropic;
  4 | pub mod unit;
  5 | 
  6 | pub use diagonal::*;
  7 | pub use gaussian::*;
  8 | pub use isotropic::*;
  9 | pub use unit::*;
 10 | 
 11 | use nalgebra::base::allocator::Allocator;
 12 | use nalgebra::base::default_allocator::DefaultAllocator;
 13 | use nalgebra::base::dimension::Dim;
 14 | use nalgebra::base::{DMatrix, DVector, OMatrix, OVector};
 15 | use nalgebra::RealField;
 16 | use std::fmt::Debug;
 17 | 
 18 | #[allow(non_snake_case)]
 19 | pub trait NoiseModel<D: Dim, T: RealField + Copy = f64>: Debug {
 20 |     fn is_constrained(&self) -> bool;
 21 | 
 22 |     fn is_unit(&self) -> bool;
 23 | 
 24 |     fn dim(&self) -> usize;
 25 | 
 26 |     fn sigmas(&self) -> DVector<T>;
 27 | 
 28 |     fn whiten(&self, v: &OVector<T, D>) -> OVector<T, D>
 29 |     where
 30 |         DefaultAllocator: Allocator<T, D>;
 31 | 
 32 |     fn whiten_mat(&self, m: &OMatrix<T, D, D>) -> OMatrix<T, D, D>
 33 |     where
 34 |         DefaultAllocator: Allocator<T, D, D>;
 35 | 
 36 |     fn unwhiten(&self, v: &OVector<T, D>) -> OVector<T, D>
 37 |     where
 38 |         DefaultAllocator: Allocator<T, D>;
 39 | 
 40 |     fn distance(&self, v: &OVector<T, D>) -> T
 41 |     where
 42 |         DefaultAllocator: Allocator<T, D>;
 43 | 
 44 |     fn whiten_system<_D: Dim>(&self, A: &[DMatrix<T>], b: &OVector<T, _D>)
 45 |     where
 46 |         DefaultAllocator: Allocator<T, _D>;
 47 | }
 48 | 
 49 | #[allow(non_snake_case)]
 50 | pub trait GaussianNoise<D: Dim, T: RealField + Copy = f64>: NoiseModel<D, T> {
 51 |     fn from_sqrtinfo(R: &OMatrix<T, D, D>, smart: bool) -> Self
 52 |     where
 53 |         DefaultAllocator: Allocator<T, D, D>;
 54 | 
 55 |     fn from_information(info: &OMatrix<T, D, D>, smart: bool) -> Self
 56 |     where
 57 |         DefaultAllocator: Allocator<T, D, D>,
 58 |         D: nalgebra::DimSub<nalgebra::Dynamic>;
 59 | 
 60 |     fn from_covariance(cov: &OMatrix<T, D, D>, smart: bool) -> Self
 61 |     where
 62 |         DefaultAllocator: Allocator<T, D, D>,
 63 |         D: nalgebra::DimSub<nalgebra::Dynamic>;
 64 | 
 65 |     fn sqrt_info(&self) -> Option<&OMatrix<T, D, D>>
 66 |     where
 67 |         DefaultAllocator: Allocator<T, D, D>;
 68 | 
 69 |     /// Mahalanobis distance v'*R'*R*v = <R*v,R*v>
 70 |     fn mahalanobis_dist(&self, v: &OVector<T, D>) -> T
 71 |     where
 72 |         DefaultAllocator: Allocator<T, D>;
 73 | }
 74 | 
 75 | /// Check *above the diagonal* for non-zero entries and return the diagonal if true
 76 | fn check_diagonal_upper<D: Dim, T: nalgebra::RealField + Copy>(
 77 |     mat: &OMatrix<T, D, D>,
 78 | ) -> Option<DVector<T>>
 79 | where
 80 |     DefaultAllocator: Allocator<T, D, D>,
 81 | {
 82 |     let (m, n) = (mat.nrows(), mat.ncols());
 83 |     let mut full = false;
 84 |     for i in 0..m {
 85 |         if !full {
 86 |             for j in (i + 1)..n {
 87 |                 if mat[(i, j)].abs() > T::default_epsilon() {
 88 |                     full = true;
 89 |                     break;
 90 |                 }
 91 |             }
 92 |         }
 93 |     }
 94 | 
 95 |     if full {
 96 |         None
 97 |     } else {
 98 |         let mut diag = DVector::zeros(n);
 99 |         for i in 0..n {
100 |             diag[i] = mat[(i, i)]
101 |         }
102 |         Some(diag)
103 |     }
104 | }
105 | 
106 | #[cfg(test)]
107 | mod tests {
108 |     use super::*;
109 |     use nalgebra::base::Matrix4;
110 | 
111 |     #[test]
112 |     fn check_upper_diagonal() {
113 |         let mat = Matrix4::<f64>::identity();
114 |         assert_eq!(check_diagonal_upper(&mat).is_some(), true);
115 | 
116 |         let eps = std::f64::EPSILON;
117 | 
118 |         let mat1 = DMatrix::from_row_slice(
119 |             4,
120 |             3, // dim
121 |             &[
122 |                 1.0, 0.0, eps, //
123 |                 eps, 0.0, 0.0, //
124 |                 0.0, 0.0, 0.0, //
125 |                 0.0, 2.0, 0.0,
126 |             ],
127 |         );
128 | 
129 |         assert_eq!(check_diagonal_upper(&mat1).is_some(), true);
130 | 
131 |         let eps2 = 2.0 * std::f64::EPSILON;
132 |         let mat2 = DMatrix::from_row_slice(
133 |             4,
134 |             3, // dim
135 |             &[
136 |                 1.0, 0.0, eps2, //
137 |                 0.0, 1.0, 0.0, //
138 |                 0.0, 0.0, 0.0, //
139 |                 0.0, 0.0, 0.0,
140 |             ],
141 |         );
142 | 
143 |         println!("{:}", mat2);
144 |         assert_eq!(check_diagonal_upper(&mat2).is_some(), false);
145 |     }
146 | }
147 | 


--------------------------------------------------------------------------------
/src/linear/noise_model/unit.rs:
--------------------------------------------------------------------------------
  1 | use nalgebra::base::allocator::Allocator;
  2 | use nalgebra::base::default_allocator::DefaultAllocator;
  3 | use nalgebra::base::dimension::Dim;
  4 | use nalgebra::base::{DMatrix, DVector, OVector};
  5 | use nalgebra::RealField;
  6 | use std::fmt::Debug;
  7 | 
  8 | use super::*;
  9 | 
 10 | #[derive(Debug)]
 11 | pub struct Unit<D: Dim, T: RealField + Copy = f64>
 12 | where
 13 |     DefaultAllocator: Allocator<T, D>,
 14 | {
 15 |     dim: usize,
 16 |     sigmas_: OVector<T, D>,
 17 |     invsigmas_: OVector<T, D>,
 18 |     precisions_: OVector<T, D>,
 19 | }
 20 | 
 21 | #[allow(non_snake_case)]
 22 | impl<D: Dim, T: RealField + Copy> GaussianNoise<D, T> for Unit<D, T>
 23 | where
 24 |     DefaultAllocator: Allocator<T, D>,
 25 | {
 26 |     fn from_sqrtinfo(_R: &OMatrix<T, D, D>, _smart: bool) -> Self
 27 |     where
 28 |         DefaultAllocator: Allocator<T, D, D>,
 29 |     {
 30 |         unimplemented!() // TODO
 31 |     }
 32 | 
 33 |     fn from_information(_info: &OMatrix<T, D, D>, _smart: bool) -> Self
 34 |     where
 35 |         DefaultAllocator: Allocator<T, D, D>,
 36 |     {
 37 |         unimplemented!()
 38 |     }
 39 | 
 40 |     fn from_covariance(_cov: &OMatrix<T, D, D>, _smart: bool) -> Self
 41 |     where
 42 |         DefaultAllocator: Allocator<T, D, D>,
 43 |     {
 44 |         unimplemented!() // TODO
 45 |     }
 46 | 
 47 |     fn sqrt_info(&self) -> Option<&OMatrix<T, D, D>>
 48 |     where
 49 |         DefaultAllocator: Allocator<T, D, D>,
 50 |     {
 51 |         unimplemented!() // TODO
 52 |     }
 53 | 
 54 |     /**
 55 |      * Mahalanobis distance v'*R'*R*v = <R*v,R*v>
 56 |      */
 57 |     fn mahalanobis_dist(&self, v: &OVector<T, D>) -> T
 58 |     where
 59 |         DefaultAllocator: Allocator<T, D>,
 60 |     {
 61 |         let w = self.whiten(v);
 62 |         w.dot(&w)
 63 |     }
 64 | }
 65 | 
 66 | #[allow(non_snake_case)]
 67 | impl<D: Dim, T: RealField + Copy> NoiseModel<D, T> for Unit<D, T>
 68 | where
 69 |     DefaultAllocator: Allocator<T, D>,
 70 | {
 71 |     fn is_constrained(&self) -> bool {
 72 |         unimplemented!()
 73 |     }
 74 | 
 75 |     fn is_unit(&self) -> bool {
 76 |         unimplemented!()
 77 |     }
 78 | 
 79 |     fn dim(&self) -> usize {
 80 |         self.dim
 81 |     }
 82 | 
 83 |     fn sigmas(&self) -> DVector<T> {
 84 |         unimplemented!()
 85 |     }
 86 | 
 87 |     fn whiten(&self, _v: &OVector<T, D>) -> OVector<T, D>
 88 |     where
 89 |         DefaultAllocator: Allocator<T, D>,
 90 |     {
 91 |         unimplemented!()
 92 |     }
 93 | 
 94 |     fn whiten_mat(&self, _m: &OMatrix<T, D, D>) -> OMatrix<T, D, D>
 95 |     where
 96 |         DefaultAllocator: Allocator<T, D, D>,
 97 |     {
 98 |         unimplemented!()
 99 |     }
100 | 
101 |     fn unwhiten(&self, _v: &OVector<T, D>) -> OVector<T, D>
102 |     where
103 |         DefaultAllocator: Allocator<T, D>,
104 |     {
105 |         unimplemented!()
106 |     }
107 | 
108 |     fn distance(&self, _v: &OVector<T, D>) -> T
109 |     where
110 |         DefaultAllocator: Allocator<T, D>,
111 |     {
112 |         unimplemented!()
113 |     }
114 | 
115 |     fn whiten_system<_D: Dim>(&self, _A: &[DMatrix<T>], _b: &OVector<T, _D>)
116 |     where
117 |         DefaultAllocator: Allocator<T, _D>,
118 |     {
119 |         unimplemented!()
120 |     }
121 | }
122 | 


--------------------------------------------------------------------------------
/src/nonlinear/mod.rs:
--------------------------------------------------------------------------------
1 | pub mod nonlinear_factor;
2 | 


--------------------------------------------------------------------------------
/src/nonlinear/nonlinear_factor.rs:
--------------------------------------------------------------------------------
1 | 
2 | 


--------------------------------------------------------------------------------