├── .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 "] 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> { 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> { 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, // REVIEW: Could we just set context to the global context? 49 | data_layout: std::cell::RefCell>, 50 | pub(crate) module: std::cell::Cell, 51 | pub(crate) owned_by_ee: 52 | std::cell::RefCell>, 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: Debug + Sized + Copy 10 | where 11 | Self: Mul, 12 | Self: for<'a> Mul<&'a Self, Output = Self>, 13 | for<'a> &'a Self: Mul, 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 27 | where 28 | DefaultAllocator: Allocator; 29 | 30 | // TODO(fan): H now does not work 31 | fn logmap(R: &Self, H: Option<&mut OMatrix>) -> OVector 32 | where 33 | DefaultAllocator: Allocator + Allocator; 34 | 35 | fn expmap(omega: &OVector) -> Self 36 | where 37 | DefaultAllocator: Allocator; 38 | 39 | fn expmap_with_derivative( 40 | omega: &OVector, 41 | H: Option<&mut OMatrix>, 42 | ) -> Self 43 | where 44 | DefaultAllocator: Allocator + Allocator; 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::::from_axis_angle(&Vector3::x_axis(), PI); 57 | let r2 = SO3::::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::::from_axis_angle(&Vector3::x_axis(), 1. * PI); 68 | let r2 = SO3::::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::::from_axis_angle(&Vector3::x_axis(), 1. * PI); 80 | let r2 = SO3::::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, dw: Vector3) -> Vector3 { 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::::identity(); 101 | 102 | let f = |x: &Vec| -> Vec { 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 = 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::::identity(); 122 | 123 | let f = |x: &Vec| -> Vec { 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 = 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::::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( 5 | wx: N, 6 | wy: N, 7 | wz: N, 8 | ) -> na::OMatrix { 9 | na::Matrix3::new(N::zero(), -wz, wy, wz, N::zero(), -wx, -wy, wx, N::zero()) 10 | } 11 | 12 | pub fn skew_symmetric_v( 13 | v: &na::Vector3, 14 | ) -> na::OMatrix { 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 { 29 | matrix: na::OMatrix, 30 | variable_col_offsets: Vec, 31 | } 32 | 33 | impl SymmetricBlockMatrix { 34 | pub fn new() -> SymmetricBlockMatrix { 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 { 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 { 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 { 103 | self.block_(j, j, 1, 1).diagonal() 104 | } 105 | } 106 | 107 | impl core::fmt::Debug for SymmetricBlockMatrix { 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 Default for SymmetricBlockMatrix { 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::::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::::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::::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::::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(vec1: &Vector, vec2: &Vector, tol: f64) -> bool 8 | where 9 | D: DimName, 10 | S: Storage, 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::::x(), &Vector6::x(), 0.01), 69 | true 70 | ); 71 | 72 | assert_eq!( 73 | linear_dependent(&Vector3::::x(), &Vector3::y(), 0.01), 74 | false 75 | ); 76 | 77 | assert_eq!( 78 | linear_dependent(&(Vector3::::x() * 3.5), &Vector3::x(), 0.01), 79 | true 80 | ); 81 | 82 | assert_eq!( 83 | linear_dependent( 84 | &(Vector3::::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::::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 for SE3 { 11 | type D = U6; 12 | 13 | fn between(&self, g: &Self) -> Self { 14 | self.inverse() * g 15 | } 16 | 17 | fn adjoint_map(&self) -> OMatrix { 18 | use crate::core::matrix::skew_symmetric; 19 | 20 | let mut res = OMatrix::::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>) -> Vector6 { 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) -> 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, optionalH: Option<&mut Matrix6>) -> 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 { 97 | type TangentVector = Vector6; 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 for SO3 { 10 | type D = U3; 11 | 12 | fn between(&self, g: &Self) -> Self { 13 | self.inverse() * g 14 | } 15 | 16 | fn adjoint_map(&self) -> OMatrix { 17 | *self.matrix() 18 | } 19 | 20 | fn logmap(R: &Self, optionalH: Option<&mut Matrix3>) -> Vector3 { 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; 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) -> Self { 97 | Self::expmap_with_derivative(omega, None) 98 | } 99 | 100 | #[inline] 101 | fn expmap_with_derivative(omega: &Vector3, optionalH: Option<&mut Matrix3>) -> 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 { 135 | type TangentVector = Vector3; 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 { 7 | frontals: Vec, 8 | children: Vec>, 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 { 17 | roots: Vec>>, 18 | } 19 | 20 | impl Cluster 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>) -> 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) { 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) { 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 ClusterTree 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::>::new(); 96 | println!("{:?}", cluster); 97 | 98 | let mut cluster_tree = ClusterTree::>::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::>::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::>::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::>::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::>::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::>::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 { 2 | fn num_frontals(&self) -> usize; 3 | fn num_parents(&self) -> usize; 4 | 5 | fn frontals<'a>(&self) -> Box + 'a>; 6 | fn parents<'a>(&self) -> Box + '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 { 15 | unimplemented!() 16 | } 17 | } 18 | 19 | impl SimpleFactorGraph {} 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 { 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; 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); 30 | 31 | fn iter(&self) -> Box + '_> 32 | where 33 | Self::FactorType: Sized; 34 | 35 | fn size(&self) -> usize; 36 | } 37 | 38 | #[derive(Debug)] 39 | pub struct SimpleFactorGraph 40 | where 41 | FactorType: Factor, 42 | { 43 | pub factors: Vec>, 44 | } 45 | 46 | impl FactorGraph for SimpleFactorGraph 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) { 68 | self.factors.push(f); 69 | } 70 | 71 | fn iter(&self) -> Box + '_> 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> 87 | where 88 | FACTOR: Factor, 89 | { 90 | type Item = &'a FACTOR; 91 | 92 | fn next(&mut self) -> Option { 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: 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, 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 { 139 | self._keys 140 | .get(index) 141 | .cloned() 142 | .ok_or(std::io::Error::new(ErrorKind::OutOfMemory, "Range")) 143 | } 144 | } 145 | 146 | impl SimpleFactorGraph { 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:: { 159 | factors: Vec::>::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:: { 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 { 14 | todo!() 15 | } 16 | } 17 | 18 | impl GaussianLikeFactor for GaussianFactor { 19 | fn augmented_jacobian(&self) -> na::DMatrix { 20 | unimplemented!() 21 | } 22 | 23 | fn jacobian( 24 | &self, 25 | ) -> ( 26 | na::OMatrix, 27 | na::OVector, 28 | ) { 29 | unimplemented!() 30 | } 31 | 32 | fn augmented_information(&self) -> na::OMatrix { 33 | unimplemented!() 34 | } 35 | 36 | fn information(&self) -> na::OMatrix { 37 | unimplemented!() 38 | } 39 | 40 | fn hessian_diagonal(&self) -> Vec<(u64, na::OVector)> { 41 | unimplemented!() 42 | } 43 | 44 | fn hessian_block_diagonal(&self) -> Vec<(u64, na::OMatrix)> { 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>, 9 | //} 10 | // 11 | //impl FactorGraph 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; 8 | 9 | fn jacobian( 10 | &self, 11 | ) -> ( 12 | na::OMatrix, 13 | na::OVector, 14 | ); 15 | 16 | fn augmented_information(&self) -> na::OMatrix; 17 | 18 | fn information(&self) -> na::OMatrix; 19 | 20 | fn hessian_diagonal(&self) -> Vec<(u64, na::OVector)>; 21 | 22 | fn hessian_block_diagonal(&self) -> Vec<(u64, na::OMatrix)>; 23 | } 24 | 25 | pub trait GaussianConditional: Conditional {} 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 { 14 | todo!() 15 | } 16 | } 17 | 18 | impl GaussianLikeFactor for JacobianFactor { 19 | fn augmented_jacobian(&self) -> na::DMatrix { 20 | unimplemented!() 21 | } 22 | 23 | fn jacobian( 24 | &self, 25 | ) -> ( 26 | na::OMatrix, 27 | na::OVector, 28 | ) { 29 | unimplemented!() 30 | } 31 | 32 | fn augmented_information(&self) -> na::OMatrix { 33 | unimplemented!() 34 | } 35 | 36 | fn information(&self) -> na::OMatrix { 37 | unimplemented!() 38 | } 39 | 40 | fn hessian_diagonal(&self) -> Vec<(u64, na::OVector)> { 41 | unimplemented!() 42 | } 43 | 44 | fn hessian_block_diagonal(&self) -> Vec<(u64, na::OMatrix)> { 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 12 | where 13 | DefaultAllocator: Allocator, 14 | { 15 | dim: usize, 16 | sigmas_: OVector, 17 | invsigmas_: OVector, 18 | precisions_: OVector, 19 | } 20 | 21 | #[allow(non_snake_case)] 22 | impl GaussianNoise for Diagonal 23 | where 24 | DefaultAllocator: Allocator, 25 | { 26 | fn from_sqrtinfo(_R: &OMatrix, _smart: bool) -> Self 27 | where 28 | DefaultAllocator: Allocator, 29 | { 30 | unimplemented!() // TODO 31 | } 32 | 33 | fn from_information(_info: &OMatrix, _smart: bool) -> Self 34 | where 35 | DefaultAllocator: Allocator, 36 | { 37 | unimplemented!() 38 | } 39 | 40 | fn from_covariance(_cov: &OMatrix, _smart: bool) -> Self 41 | where 42 | DefaultAllocator: Allocator, 43 | { 44 | unimplemented!() // TODO 45 | } 46 | 47 | fn sqrt_info(&self) -> Option<&OMatrix> 48 | where 49 | DefaultAllocator: Allocator, 50 | { 51 | unimplemented!() // TODO 52 | } 53 | 54 | /** 55 | * Mahalanobis distance v'*R'*R*v = 56 | */ 57 | fn mahalanobis_dist(&self, v: &OVector) -> T 58 | where 59 | DefaultAllocator: Allocator, 60 | { 61 | let w = self.whiten(v); 62 | w.dot(&w) 63 | } 64 | } 65 | 66 | #[allow(non_snake_case)] 67 | impl NoiseModel for Diagonal 68 | where 69 | DefaultAllocator: Allocator, 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 { 84 | unimplemented!() 85 | } 86 | 87 | fn whiten(&self, _v: &OVector) -> OVector 88 | where 89 | DefaultAllocator: Allocator, 90 | { 91 | unimplemented!() 92 | } 93 | 94 | fn whiten_mat(&self, _m: &OMatrix) -> OMatrix 95 | where 96 | DefaultAllocator: Allocator, 97 | { 98 | unimplemented!() 99 | } 100 | 101 | fn unwhiten(&self, _v: &OVector) -> OVector 102 | where 103 | DefaultAllocator: Allocator, 104 | { 105 | unimplemented!() 106 | } 107 | 108 | fn distance(&self, _v: &OVector) -> T 109 | where 110 | DefaultAllocator: Allocator, 111 | { 112 | unimplemented!() 113 | } 114 | 115 | fn whiten_system<_D: Dim>(&self, _A: &[DMatrix], _b: &OVector) 116 | where 117 | DefaultAllocator: Allocator, 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 13 | where 14 | DefaultAllocator: Allocator, 15 | { 16 | dim: usize, 17 | sqrt_info: Option>, 18 | } 19 | 20 | #[allow(non_snake_case)] 21 | impl GaussianNoise for Gaussian 22 | where 23 | DefaultAllocator: Allocator, 24 | { 25 | fn from_sqrtinfo(R: &OMatrix, smart: bool) -> Self 26 | where 27 | DefaultAllocator: Allocator, 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, _smart: bool) -> Self 44 | where 45 | DefaultAllocator: Allocator, 46 | D: DimSub, 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, smart: bool) -> Self 63 | where 64 | DefaultAllocator: Allocator, 65 | D: DimSub, 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> 84 | where 85 | DefaultAllocator: Allocator, 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 = 95 | */ 96 | fn mahalanobis_dist(&self, v: &OVector) -> T 97 | where 98 | DefaultAllocator: Allocator, 99 | { 100 | let w = self.whiten(v); 101 | w.dot(&w) 102 | } 103 | } 104 | 105 | #[allow(non_snake_case)] 106 | impl NoiseModel for Gaussian 107 | where 108 | DefaultAllocator: Allocator, 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 { 123 | unimplemented!() 124 | } 125 | 126 | fn whiten(&self, v: &OVector) -> OVector 127 | where 128 | DefaultAllocator: Allocator + Allocator, 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) -> OMatrix 138 | where 139 | DefaultAllocator: Allocator, 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) -> OVector 149 | where 150 | DefaultAllocator: Allocator + Allocator, 151 | { 152 | unimplemented!() 153 | } 154 | 155 | fn distance(&self, v: &OVector) -> T 156 | where 157 | DefaultAllocator: Allocator + Allocator, 158 | { 159 | self.mahalanobis_dist(v) 160 | } 161 | 162 | fn whiten_system<_D: Dim>(&self, _A: &[DMatrix], _b: &OVector) 163 | where 164 | DefaultAllocator: Allocator + Allocator, 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::::identity(4, 4); 178 | let g = Gaussian::from_sqrtinfo(&si, false); 179 | 180 | let se = Matrix4::::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::::identity(4, 4); 193 | let _cm = DMatrix::::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 { 11 | dim: usize, 12 | sigma_: T, 13 | invsigma_: T, 14 | _phantom: std::marker::PhantomData, 15 | } 16 | 17 | #[allow(non_snake_case)] 18 | impl GaussianNoise for Isotropic 19 | where 20 | DefaultAllocator: Allocator, 21 | { 22 | fn from_sqrtinfo(_R: &OMatrix, _smart: bool) -> Self 23 | where 24 | DefaultAllocator: Allocator, 25 | { 26 | unimplemented!() // TODO 27 | } 28 | 29 | fn from_information(_info: &OMatrix, _smart: bool) -> Self 30 | where 31 | DefaultAllocator: Allocator, 32 | { 33 | unimplemented!() 34 | } 35 | 36 | fn from_covariance(_cov: &OMatrix, _smart: bool) -> Self 37 | where 38 | DefaultAllocator: Allocator, 39 | { 40 | unimplemented!() // TODO 41 | } 42 | 43 | fn sqrt_info(&self) -> Option<&OMatrix> 44 | where 45 | DefaultAllocator: Allocator, 46 | { 47 | unimplemented!() // TODO 48 | } 49 | 50 | /** 51 | * Mahalanobis distance v'*R'*R*v = 52 | */ 53 | fn mahalanobis_dist(&self, v: &OVector) -> T 54 | where 55 | DefaultAllocator: Allocator, 56 | { 57 | v.dot(v) * self.invsigma_ * self.invsigma_ 58 | } 59 | } 60 | 61 | #[allow(non_snake_case)] 62 | impl NoiseModel for Isotropic 63 | where 64 | DefaultAllocator: Allocator, 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 { 79 | unimplemented!() 80 | } 81 | 82 | fn whiten(&self, v: &OVector) -> OVector 83 | where 84 | DefaultAllocator: Allocator, 85 | { 86 | v * self.invsigma_ 87 | } 88 | 89 | fn whiten_mat(&self, _m: &OMatrix) -> OMatrix 90 | where 91 | DefaultAllocator: Allocator, 92 | { 93 | unimplemented!() 94 | } 95 | 96 | fn unwhiten(&self, _v: &OVector) -> OVector 97 | where 98 | DefaultAllocator: Allocator, 99 | { 100 | unimplemented!() 101 | } 102 | 103 | fn distance(&self, _v: &OVector) -> T 104 | where 105 | DefaultAllocator: Allocator, 106 | { 107 | unimplemented!() 108 | } 109 | 110 | fn whiten_system<_D: Dim>(&self, _A: &[DMatrix], _b: &OVector) 111 | where 112 | DefaultAllocator: Allocator, 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: 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; 27 | 28 | fn whiten(&self, v: &OVector) -> OVector 29 | where 30 | DefaultAllocator: Allocator; 31 | 32 | fn whiten_mat(&self, m: &OMatrix) -> OMatrix 33 | where 34 | DefaultAllocator: Allocator; 35 | 36 | fn unwhiten(&self, v: &OVector) -> OVector 37 | where 38 | DefaultAllocator: Allocator; 39 | 40 | fn distance(&self, v: &OVector) -> T 41 | where 42 | DefaultAllocator: Allocator; 43 | 44 | fn whiten_system<_D: Dim>(&self, A: &[DMatrix], b: &OVector) 45 | where 46 | DefaultAllocator: Allocator; 47 | } 48 | 49 | #[allow(non_snake_case)] 50 | pub trait GaussianNoise: NoiseModel { 51 | fn from_sqrtinfo(R: &OMatrix, smart: bool) -> Self 52 | where 53 | DefaultAllocator: Allocator; 54 | 55 | fn from_information(info: &OMatrix, smart: bool) -> Self 56 | where 57 | DefaultAllocator: Allocator, 58 | D: nalgebra::DimSub; 59 | 60 | fn from_covariance(cov: &OMatrix, smart: bool) -> Self 61 | where 62 | DefaultAllocator: Allocator, 63 | D: nalgebra::DimSub; 64 | 65 | fn sqrt_info(&self) -> Option<&OMatrix> 66 | where 67 | DefaultAllocator: Allocator; 68 | 69 | /// Mahalanobis distance v'*R'*R*v = 70 | fn mahalanobis_dist(&self, v: &OVector) -> T 71 | where 72 | DefaultAllocator: Allocator; 73 | } 74 | 75 | /// Check *above the diagonal* for non-zero entries and return the diagonal if true 76 | fn check_diagonal_upper( 77 | mat: &OMatrix, 78 | ) -> Option> 79 | where 80 | DefaultAllocator: Allocator, 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::::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 12 | where 13 | DefaultAllocator: Allocator, 14 | { 15 | dim: usize, 16 | sigmas_: OVector, 17 | invsigmas_: OVector, 18 | precisions_: OVector, 19 | } 20 | 21 | #[allow(non_snake_case)] 22 | impl GaussianNoise for Unit 23 | where 24 | DefaultAllocator: Allocator, 25 | { 26 | fn from_sqrtinfo(_R: &OMatrix, _smart: bool) -> Self 27 | where 28 | DefaultAllocator: Allocator, 29 | { 30 | unimplemented!() // TODO 31 | } 32 | 33 | fn from_information(_info: &OMatrix, _smart: bool) -> Self 34 | where 35 | DefaultAllocator: Allocator, 36 | { 37 | unimplemented!() 38 | } 39 | 40 | fn from_covariance(_cov: &OMatrix, _smart: bool) -> Self 41 | where 42 | DefaultAllocator: Allocator, 43 | { 44 | unimplemented!() // TODO 45 | } 46 | 47 | fn sqrt_info(&self) -> Option<&OMatrix> 48 | where 49 | DefaultAllocator: Allocator, 50 | { 51 | unimplemented!() // TODO 52 | } 53 | 54 | /** 55 | * Mahalanobis distance v'*R'*R*v = 56 | */ 57 | fn mahalanobis_dist(&self, v: &OVector) -> T 58 | where 59 | DefaultAllocator: Allocator, 60 | { 61 | let w = self.whiten(v); 62 | w.dot(&w) 63 | } 64 | } 65 | 66 | #[allow(non_snake_case)] 67 | impl NoiseModel for Unit 68 | where 69 | DefaultAllocator: Allocator, 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 { 84 | unimplemented!() 85 | } 86 | 87 | fn whiten(&self, _v: &OVector) -> OVector 88 | where 89 | DefaultAllocator: Allocator, 90 | { 91 | unimplemented!() 92 | } 93 | 94 | fn whiten_mat(&self, _m: &OMatrix) -> OMatrix 95 | where 96 | DefaultAllocator: Allocator, 97 | { 98 | unimplemented!() 99 | } 100 | 101 | fn unwhiten(&self, _v: &OVector) -> OVector 102 | where 103 | DefaultAllocator: Allocator, 104 | { 105 | unimplemented!() 106 | } 107 | 108 | fn distance(&self, _v: &OVector) -> T 109 | where 110 | DefaultAllocator: Allocator, 111 | { 112 | unimplemented!() 113 | } 114 | 115 | fn whiten_system<_D: Dim>(&self, _A: &[DMatrix], _b: &OVector) 116 | where 117 | DefaultAllocator: Allocator, 118 | { 119 | unimplemented!() 120 | } 121 | } 122 | -------------------------------------------------------------------------------- /src/nonlinear/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod nonlinear_factor; 2 | -------------------------------------------------------------------------------- /src/nonlinear/nonlinear_factor.rs: -------------------------------------------------------------------------------- 1 | 2 | --------------------------------------------------------------------------------