diff --git a/Cargo.lock b/Cargo.lock index 35e4ae1a..e791954a 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2164,6 +2164,7 @@ version = "0.1.0" dependencies = [ "charming", "nalgebra", + "num", "nutype", "rand", "rand_distr", @@ -3089,6 +3090,31 @@ dependencies = [ "winapi", ] +[[package]] +name = "num" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b05180d69e3da0e530ba2a1dae5110317e49e3b7f3d41be227dc5f92e49ee7af" +dependencies = [ + "num-bigint", + "num-complex", + "num-integer", + "num-iter", + "num-rational", + "num-traits", +] + +[[package]] +name = "num-bigint" +version = "0.4.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "608e7659b5c3d7cba262d894801b9ec9d00de989e8a82bd4bef91d08da45cdc0" +dependencies = [ + "autocfg", + "num-integer", + "num-traits", +] + [[package]] name = "num-complex" version = "0.4.4" @@ -3119,6 +3145,17 @@ dependencies = [ "num-traits", ] +[[package]] +name = "num-iter" +version = "0.1.43" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7d03e6c028c5dc5cac6e2dec0efda81fc887605bb3d884578bb6d6bf7514e252" +dependencies = [ + "autocfg", + "num-integer", + "num-traits", +] + [[package]] name = "num-rational" version = "0.4.1" @@ -3126,6 +3163,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0638a1c9d0a3c0914158145bc76cff373a75a627e6ecbfb71cbe6f453a5a19b0" dependencies = [ "autocfg", + "num-bigint", "num-integer", "num-traits", ] diff --git a/gbp-rs/Cargo.toml b/gbp-rs/Cargo.toml index 846b07ce..80057c75 100644 --- a/gbp-rs/Cargo.toml +++ b/gbp-rs/Cargo.toml @@ -17,7 +17,7 @@ rstest = "0.18.2" rand = "0.8.5" rand_distr = "0.4.3" typed-builder = "0.18.1" - +num = "0.4.1" [dev-dependencies] charming = "0.3.1" diff --git a/gbp-rs/src/factorgraph/factor.rs b/gbp-rs/src/factorgraph/factor.rs index fc2d50e9..8d820b10 100644 --- a/gbp-rs/src/factorgraph/factor.rs +++ b/gbp-rs/src/factorgraph/factor.rs @@ -1,3 +1,5 @@ +use crate::gaussian::Gaussian; + use super::message::Message; #[derive(Debug)] @@ -15,6 +17,7 @@ pub trait Factor { fn robustify_loss(&self); fn measurement_model(&self) -> MeasurementModel; fn linerisation_point(&self) -> nalgebra::DVector; + fn get_gaussian(&self) -> &Gaussian; } // [1,,3].iter(). diff --git a/gbp-rs/src/factorgraph/factorgraph.rs b/gbp-rs/src/factorgraph/factorgraph.rs index 4e4e2127..c78f88dc 100644 --- a/gbp-rs/src/factorgraph/factorgraph.rs +++ b/gbp-rs/src/factorgraph/factorgraph.rs @@ -1,8 +1,11 @@ +use std::ops::AddAssign; + use crate::factorgraph::factor::Factor; use crate::factorgraph::variable::Variable; use super::factor::MeasurementModel; use super::{Dropout, UnitInterval}; +use crate::gaussian::Gaussian; use typed_builder::TypedBuilder; @@ -236,4 +239,137 @@ impl FactorGraph { .map(|variable_node| variable_node.dofs) .sum() } + + /// Get the joint distribution over all variables in the information form. + /// If non-linear factors exist, it is taken at the linearisation point. + fn joint_distribution(&self) -> Gaussian { + let dim = self.get_joint_dim(); + let mut joint = Gaussian::new(dim, None, None); + + // Priors + let mut var_ix = vec![0; self.variables.len()]; + let mut counter = 0; + + for variable_node in self.variables.iter() { + let variable = &variable_node.variable; + + var_ix[variable_node.id] = counter; + + joint + .information_vector + .rows_mut(counter, variable_node.dofs) + .add_assign( + &variable + .get_prior() + .information_vector + .rows(counter, variable_node.dofs), + ); + joint + .precision_matrix + .view_mut( + (counter, counter + variable_node.dofs), + (counter, counter + variable_node.dofs), + ) + .add_assign( + &variable + .get_prior() + .precision_matrix + .view((counter, variable_node.dofs), (counter, variable_node.dofs)), + ); + counter += variable_node.dofs; + } + + // Other factors + for factor_node in self.factors.iter() { + let mut fact_ix = 0; + for &adjacent_variable_node_id in factor_node.adjacent_variables.iter() { + let adjacent_variable_node = &self.variables[adjacent_variable_node_id]; + + // Diagonal contribution of factor + joint + .information_vector + .rows_mut( + var_ix[adjacent_variable_node_id], + adjacent_variable_node.dofs, + ) + .add_assign( + factor_node + .factor + .get_gaussian() + .information_vector + .rows(fact_ix, adjacent_variable_node.dofs), + ); + + // joint.lam[var_ix[vID]:var_ix[vID] + adj_var_node.dofs, var_ix[vID]:var_ix[vID] + adj_var_node.dofs] += \ + // factor.factor.lam[factor_ix:factor_ix + adj_var_node.dofs, factor_ix:factor_ix + adj_var_node.dofs] + joint + .precision_matrix + .view_mut( + ( + var_ix[adjacent_variable_node_id], + adjacent_variable_node.dofs, + ), + ( + var_ix[adjacent_variable_node_id], + adjacent_variable_node.dofs, + ), + ) + .add_assign(factor_node.factor.get_gaussian().precision_matrix.view( + (fact_ix, adjacent_variable_node.dofs), + (fact_ix, adjacent_variable_node.dofs), + )); + + let mut other_fact_ix = 0; + for &other_adjacent_variable_node_id in factor_node.adjacent_variables.iter() { + if &other_adjacent_variable_node_id > &adjacent_variable_node_id { + let other_adjacent_variable_node = + &self.variables[other_adjacent_variable_node_id]; + + // off diagonal contributions of factor + // joint.lam[var_ix[vID]:var_ix[vID] + adj_var_node.dofs, var_ix[other_vID]:var_ix[other_vID] + other_adj_var_node.dofs] += \ + // factor.factor.lam[factor_ix:factor_ix + adj_var_node.dofs, other_factor_ix:other_factor_ix + other_adj_var_node.dofs] + joint + .precision_matrix + .view_mut( + ( + var_ix[adjacent_variable_node_id], + adjacent_variable_node.dofs, + ), + ( + var_ix[adjacent_variable_node_id], + adjacent_variable_node.dofs, + ), + ) + .add_assign(factor_node.factor.get_gaussian().precision_matrix.view( + (fact_ix, adjacent_variable_node.dofs), + (other_fact_ix, adjacent_variable_node.dofs), + )); + // joint.lam[var_ix[other_vID]:var_ix[other_vID] + other_adj_var_node.dofs, var_ix[vID]:var_ix[vID] + adj_var_node.dofs] += \ + // factor.factor.lam[other_factor_ix:other_factor_ix + other_adj_var_node.dofs, factor_ix:factor_ix + adj_var_node.dofs] + joint + .precision_matrix + .view_mut( + ( + var_ix[other_adjacent_variable_node_id], + other_adjacent_variable_node.dofs, + ), + ( + var_ix[adjacent_variable_node_id], + adjacent_variable_node.dofs, + ), + ) + .add_assign(factor_node.factor.get_gaussian().precision_matrix.view( + (other_fact_ix, other_adjacent_variable_node.dofs), + (fact_ix, adjacent_variable_node.dofs), + )); + other_fact_ix += other_adjacent_variable_node.dofs; + } + + fact_ix += adjacent_variable_node.dofs; + } + } + } + + joint + } } diff --git a/gbp-rs/src/factorgraph/variable.rs b/gbp-rs/src/factorgraph/variable.rs index f7f37982..34195f2d 100644 --- a/gbp-rs/src/factorgraph/variable.rs +++ b/gbp-rs/src/factorgraph/variable.rs @@ -1,5 +1,4 @@ /// - // pub trait Variable {} // struct RobotId(usize); @@ -17,8 +16,12 @@ // Self { node_id, robot_id } // } // } +use crate::gaussian::Gaussian; pub trait Variable { fn update_belief(&mut self); fn prior_energy(&self) -> f64; + + fn get_belief(&self) -> &Gaussian; + fn get_prior(&self) -> &Gaussian; } diff --git a/gbp-rs/src/gaussian.rs b/gbp-rs/src/gaussian.rs new file mode 100644 index 00000000..e924ae61 --- /dev/null +++ b/gbp-rs/src/gaussian.rs @@ -0,0 +1,51 @@ +use nalgebra::{DMatrix, DVector}; +// use num::Float; + +// TODO: maybe there should be something to ensure the dimensions of the information vector and precision matrix match +#[derive(Debug)] +pub struct Gaussian { + size: usize, // dim + pub information_vector: DVector, // eta + pub precision_matrix: DMatrix, // lam +} + +impl Gaussian { + /// information_vector commonly used symbol: lowercase eta (η) + /// precision_matrix commmonly used symbol: uppercase lambda (Λ) + pub fn new( + size: usize, + information_vector: Option>, + precision_matrix: Option>, + ) -> Self { + let information_vector = information_vector.unwrap_or_else(|| DVector::zeros(size)); + // check if the precision_matrix has an inverse + let precision_matrix = precision_matrix.unwrap_or_else(|| DMatrix::zeros(size, size)); + + Gaussian { + size, + information_vector, + precision_matrix, + } + } + + pub fn mean(&self) -> DVector { + self.precision_matrix.try_inverse().unwrap() * &self.information_vector + } + + pub fn covariance(&self) -> DMatrix { + self.precision_matrix.try_inverse().unwrap() + } + + pub fn mean_and_coveriance(&self) -> (DVector, DMatrix) { + let covariance = self.covariance(); + let mean = &covariance * &self.information_vector; + + (mean, covariance) + } + + pub fn set_with_covariance_form(&mut self, mean: DVector, covariance: DMatrix) { + // check for invertibility of covariance matrix input + self.precision_matrix = covariance.try_inverse().unwrap(); + self.information_vector = &self.precision_matrix * mean; + } +} diff --git a/gbp-rs/src/lib.rs b/gbp-rs/src/lib.rs index f48101a7..a62d16dd 100644 --- a/gbp-rs/src/lib.rs +++ b/gbp-rs/src/lib.rs @@ -1,8 +1,10 @@ pub mod factorgraph; +pub mod gaussian; pub mod prelude { pub use crate::factorgraph::factor::Factor; pub use crate::factorgraph::factorgraph::FactorGraph; pub use crate::factorgraph::message::Message; pub use crate::factorgraph::variable::Variable; + pub use crate::gaussian; }