From 920ae6e5a763b24284f46fdfb8ed0c02089607d5 Mon Sep 17 00:00:00 2001 From: Jens Date: Thu, 1 Feb 2024 13:26:49 +0100 Subject: [PATCH] feat: variable and factor traits --- .vscode/settings.json | 7 + Cargo.lock | 250 ++++++++++++++++++++++++++ gbp-rs/Cargo.toml | 4 + gbp-rs/examples/1d_line_fitting.rs | 3 - gbp-rs/src/factorgraph/factor.rs | 78 +++++--- gbp-rs/src/factorgraph/factorgraph.rs | 52 +++++- gbp-rs/src/factorgraph/mod.rs | 4 +- gbp-rs/src/factorgraph/variable.rs | 28 +-- gbp-rs/src/lib.rs | 4 +- gbpplanner-rs/src/shapes.rs | 4 +- hx.log | 3 + shell.nix | 15 -- 12 files changed, 387 insertions(+), 65 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 hx.log delete mode 100644 shell.nix diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..55ea4df5 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,7 @@ +{ + "rust-analyzer.linkedProjects": [ + "./gbp-rs/Cargo.toml", + "./gbp-rs/Cargo.toml", + "./gbp-rs/Cargo.toml" + ] +} \ No newline at end of file diff --git a/Cargo.lock b/Cargo.lock index 6703e272..5cb7c23d 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1258,6 +1258,15 @@ version = "0.1.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0d8c1fef690941d3e7788d328517591fecc684c084084702d6ff1641e993699a" +[[package]] +name = "block-buffer" +version = "0.10.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3078c7629b62d3f0439517fa394996acacc5cbc91c5a20d8c658e77abd503a71" +dependencies = [ + "generic-array", +] + [[package]] name = "block-sys" version = "0.1.0-beta.1" @@ -1382,6 +1391,17 @@ version = "0.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fd16c4719339c4530435d38e511904438d07cce7950afa3718a84ac36c10e89e" +[[package]] +name = "charming" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f4c6b6990238a64b4ae139e7085ce2a11815cb67a0c066a3333ce40f3a329be3" +dependencies = [ + "handlebars", + "serde", + "serde_json", +] + [[package]] name = "clang-sys" version = "1.7.0" @@ -1555,6 +1575,15 @@ dependencies = [ "const_soft_float", ] +[[package]] +name = "convert_case" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec182b0ca2f35d8fc196cf3404988fd8b8c739a4d270ff118a398feb0cbec1ca" +dependencies = [ + "unicode-segmentation", +] + [[package]] name = "core-foundation" version = "0.9.4" @@ -1640,6 +1669,15 @@ dependencies = [ "windows 0.46.0", ] +[[package]] +name = "cpufeatures" +version = "0.2.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53fe5e26ff1b7aef8bca9c6080520cfb8d9333c7568e1829cef191a9723e5504" +dependencies = [ + "libc", +] + [[package]] name = "crc32fast" version = "1.3.2" @@ -1683,6 +1721,16 @@ version = "0.8.19" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "248e3bacc7dc6baa3b21e405ee045c3047101a49145e7e9eca583ab4c2ca5345" +[[package]] +name = "crypto-common" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1bfb12502f3fc46cca1bb51ac28df9d618d813cdc3d2f25b9fe775a34af26bb3" +dependencies = [ + "generic-array", + "typenum", +] + [[package]] name = "d3d12" version = "0.7.0" @@ -1706,6 +1754,16 @@ version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7e962a19be5cfc3f3bf6dd8f61eb50107f356ad6270fbb3ed41476571db78be5" +[[package]] +name = "digest" +version = "0.10.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9ed9a281f7bc9b7576e61468ba615a66a5c8cfdff42420a70aa82701a3b1e292" +dependencies = [ + "block-buffer", + "crypto-common", +] + [[package]] name = "dispatch" version = "0.2.0" @@ -2104,7 +2162,11 @@ dependencies = [ name = "gbp-rs" version = "0.1.0" dependencies = [ + "charming", "nalgebra", + "nutype", + "rand", + "rand_distr", "rayon", "rstest", ] @@ -2123,6 +2185,16 @@ dependencies = [ "toml", ] +[[package]] +name = "generic-array" +version = "0.14.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a" +dependencies = [ + "typenum", + "version_check", +] + [[package]] name = "gethostname" version = "0.3.0" @@ -2329,6 +2401,20 @@ dependencies = [ "svg_fmt", ] +[[package]] +name = "handlebars" +version = "4.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "faa67bab9ff362228eb3d00bd024a4965d8231bbb7921167f0cfa66c6626b225" +dependencies = [ + "log", + "pest", + "pest_derive", + "serde", + "serde_json", + "thiserror", +] + [[package]] name = "hashbrown" version = "0.12.3" @@ -2581,6 +2667,27 @@ dependencies = [ "pkg-config", ] +[[package]] +name = "kinded" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ce4bdbb2f423660b19f0e9f7115182214732d8dd5f840cd0a3aee3e22562f34c" +dependencies = [ + "kinded_macros", +] + +[[package]] +name = "kinded_macros" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a13b4ddc5dcb32f45dac3d6f606da2a52fdb9964a18427e63cd5ef6c0d13288d" +dependencies = [ + "convert_case", + "proc-macro2", + "quote", + "syn 2.0.48", +] + [[package]] name = "ktx2" version = "0.3.0" @@ -2639,6 +2746,12 @@ dependencies = [ "windows-sys 0.48.0", ] +[[package]] +name = "libm" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" + [[package]] name = "libredox" version = "0.0.2" @@ -3023,6 +3136,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "39e3200413f237f41ab11ad6d161bc7239c84dcb631773ccd7de3dfe4b5c267c" dependencies = [ "autocfg", + "libm", ] [[package]] @@ -3067,6 +3181,28 @@ dependencies = [ "syn 2.0.48", ] +[[package]] +name = "nutype" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38fd38251d275b6a3cecbf4d2f8ce83b80609f0e33e2508a8bc3a7d94401d007" +dependencies = [ + "nutype_macros", +] + +[[package]] +name = "nutype_macros" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b9dc33172cb6abafee47925b6a188f2d3c3726cb5df12f39132a53df98501d2" +dependencies = [ + "cfg-if", + "kinded", + "proc-macro2", + "quote", + "syn 2.0.48", +] + [[package]] name = "objc" version = "0.2.7" @@ -3256,6 +3392,51 @@ version = "2.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e3148f5046208a5d56bcfc03053e3ca6334e51da8dfb19b6cdc8b306fae3283e" +[[package]] +name = "pest" +version = "2.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1f200d8d83c44a45b21764d1916299752ca035d15ecd46faca3e9a2a2bf6ad06" +dependencies = [ + "memchr", + "thiserror", + "ucd-trie", +] + +[[package]] +name = "pest_derive" +version = "2.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bcd6ab1236bbdb3a49027e920e693192ebfe8913f6d60e294de57463a493cfde" +dependencies = [ + "pest", + "pest_generator", +] + +[[package]] +name = "pest_generator" +version = "2.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2a31940305ffc96863a735bef7c7994a00b325a7138fdbc5bda0f1a0476d3275" +dependencies = [ + "pest", + "pest_meta", + "proc-macro2", + "quote", + "syn 2.0.48", +] + +[[package]] +name = "pest_meta" +version = "2.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7ff62f5259e53b78d1af898941cdcdccfae7385cf7d793a6e55de5d05bb4b7d" +dependencies = [ + "once_cell", + "pest", + "sha2", +] + [[package]] name = "petgraph" version = "0.6.4" @@ -3317,6 +3498,12 @@ dependencies = [ "unicode-xid", ] +[[package]] +name = "ppv-lite86" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" + [[package]] name = "pretty-type-name" version = "1.0.1" @@ -3363,6 +3550,46 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "17fd96390ed3feda12e1dfe2645ed587e0bea749e319333f104a33ff62f77a0b" +[[package]] +name = "rand" +version = "0.8.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" +dependencies = [ + "libc", + "rand_chacha", + "rand_core", +] + +[[package]] +name = "rand_chacha" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" +dependencies = [ + "ppv-lite86", + "rand_core", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +dependencies = [ + "getrandom", +] + +[[package]] +name = "rand_distr" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32cb0b9bc82b0a0876c2dd994a7e7a2683d3e7390ca40e6886785ef0c7e3ee31" +dependencies = [ + "num-traits", + "rand", +] + [[package]] name = "range-alloc" version = "0.1.3" @@ -3665,6 +3892,17 @@ dependencies = [ "serde", ] +[[package]] +name = "sha2" +version = "0.10.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "793db75ad2bcafc3ffa7c68b215fee268f537982cd901d132f89c6343f3a3dc8" +dependencies = [ + "cfg-if", + "cpufeatures", + "digest", +] + [[package]] name = "sharded-slab" version = "0.1.7" @@ -4123,6 +4361,12 @@ version = "1.17.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" +[[package]] +name = "ucd-trie" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed646292ffc8188ef8ea4d1e0e0150fb15a5c2e12ad9b8fc191ae7a8a7f3c4b9" + [[package]] name = "unicode-bidi" version = "0.3.15" @@ -4144,6 +4388,12 @@ dependencies = [ "tinyvec", ] +[[package]] +name = "unicode-segmentation" +version = "1.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1dd624098567895118886609431a7c3b8f516e41d30e0643f03d94592a147e36" + [[package]] name = "unicode-width" version = "0.1.11" diff --git a/gbp-rs/Cargo.toml b/gbp-rs/Cargo.toml index d4dc1a88..c1736cbd 100644 --- a/gbp-rs/Cargo.toml +++ b/gbp-rs/Cargo.toml @@ -16,3 +16,7 @@ rayon = "1.8.1" rstest = "0.18.2" rand = "0.8.5" rand_distr = "0.4.3" + + +[dev-dependencies] +charming = "0.3.1" diff --git a/gbp-rs/examples/1d_line_fitting.rs b/gbp-rs/examples/1d_line_fitting.rs index 958c60c5..7f0dedd7 100644 --- a/gbp-rs/examples/1d_line_fitting.rs +++ b/gbp-rs/examples/1d_line_fitting.rs @@ -1,10 +1,7 @@ /// Reimplementation of the 1D line fitting example from https://gaussianbp.github.io/ /// jupyter notebook: https://colab.research.google.com/drive/1-nrE95X4UC9FBLR0-cTnsIP_XhA_PZKW?usp=sharing#scrollTo=kiAOHWV4uMGY - use gbp_rs::prelude::*; fn main() { - let mut fg = FactorGraph::new(); - } diff --git a/gbp-rs/src/factorgraph/factor.rs b/gbp-rs/src/factorgraph/factor.rs index 02ca6c5a..00c34d11 100644 --- a/gbp-rs/src/factorgraph/factor.rs +++ b/gbp-rs/src/factorgraph/factor.rs @@ -1,26 +1,64 @@ +use super::message::Message; +#[derive(Debug)] +pub enum MeasurementModel { + Linear, + NonLinear, +} +pub trait Factor { + fn id(&self) -> super::NodeId; + fn compute_messages(&mut self) -> Vec; + fn energy(&self) -> f64; + fn residual(&self) -> nalgebra::DVector; + fn adj_means(&self) -> nalgebra::DVector; + fn compute(&self) -> f64; + fn robustify_loss(&self); + fn measurement_model(&self) -> MeasurementModel; + fn linerisation_point(&self) -> nalgebra::DVector; +} -pub trait Factor {} +// [1,,3].iter(). -#[derive(Debug)] -struct DefaultFactor; -#[derive(Debug)] -struct DynamicFactor; -#[derive(Debug)] -struct InterRobotFactor; -#[derive(Debug)] -struct ObstacleFactor; +// #[derive(Debug)] +// struct DynamicFactor { +// id: usize, +// // messages: +// } -impl Factor for DefaultFactor {} -impl Factor for DynamicFactor {} -impl Factor for InterRobotFactor {} -impl Factor for ObstacleFactor {} +// impl Factor for DynamicFactor { +// // type ComputedValue = f64; -#[derive(Debug)] -enum FactorType { - Default(DefaultFactor), - Dynamic(DynamicFactor), - InterRobot(InterRobotFactor), - Ocstacle(ObstacleFactor), -} +// fn get_id(&self) -> usize { +// self.id +// // get_id_from_db() +// } + +// fn compute_messages(&self) -> Vec { +// // Implementation goes here +// Vec::new() +// } + +// fn compute(&self) -> Self::ComputedValue { +// // Implementation goes here +// } +// } +// let f = DynamicFactor {id : 2}; + +// #[derive(Debug)] +// struct InterRobotFactor; +// #[derive(Debug)] +// struct ObstacleFactor; + +// impl Factor for DefaultFactor {} +// impl Factor for DynamicFactor {} +// impl Factor for InterRobotFactor {} +// impl Factor for ObstacleFactor {} + +// #[derive(Debug)] +// enum FactorType { +// Default(DefaultFactor), +// Dynamic(DynamicFactor), +// InterRobot(InterRobotFactor), +// Ocstacle(ObstacleFactor), +// } diff --git a/gbp-rs/src/factorgraph/factorgraph.rs b/gbp-rs/src/factorgraph/factorgraph.rs index 10f646a9..65a75cca 100644 --- a/gbp-rs/src/factorgraph/factorgraph.rs +++ b/gbp-rs/src/factorgraph/factorgraph.rs @@ -1,29 +1,36 @@ - use crate::factorgraph::factor::Factor; use crate::factorgraph::variable::Variable; +pub struct GbpSettings { + pub beta: f64, +} /// A factor graph is a bipartite graph representing the factorization of a function. /// It is composed of two types of nodes: factors and variables. -/// -pub struct FactorGraph { - factors: Vec>, - variables: Vec, +#[derive(Debug)] +pub struct FactorGraph { + // pub struct FactorGraph { + // factors: Vec>, + factors: Vec, + variables: Vec, + gbp_settings: GbpSettings, } +// std::unique_ptr -impl FactorGraph { - pub fn new() -> Self { +impl FactorGraph { + pub fn new(gbp_settings: GbpSettings) -> Self { Self { factors: Vec::new(), variables: Vec::new(), + gbp_settings, } } - pub fn add_factor(&mut self, factor: Box) { + pub fn add_factor(&mut self, factor: F) { self.factors.push(factor); } - pub fn add_variable(&mut self, variable: Variable) { + pub fn add_variable(&mut self, variable: V) { self.variables.push(variable); } @@ -36,4 +43,31 @@ impl FactorGraph { // factor.update_belief(); // } } + + // linearize_all_factors + fn compute_factors(&mut self) { + for factor in self.factors.iter() { + factor.compute(); + } + } + + fn robustify_all_factors(&mut self) { + for factor in self.factors.iter() { + factor.robustify_loss(); + } + } + + fn jit_linearisation(&mut self) { + for factor in self.factors.iter() { + match factor.measurement_model() { + MeasurementModel::NonLinear => { + let adj_means = factor.adj_means(); + // factors.iters_since_relin += 1 + if ((adj_means - factor.linerisation_point()).norm() > self.gbp_settings.beta) { + factor.compute(); + } + } + } + } + } } diff --git a/gbp-rs/src/factorgraph/mod.rs b/gbp-rs/src/factorgraph/mod.rs index 41b7978c..6c09eb85 100644 --- a/gbp-rs/src/factorgraph/mod.rs +++ b/gbp-rs/src/factorgraph/mod.rs @@ -1,4 +1,6 @@ pub mod factor; -pub mod variable; pub mod factorgraph; pub mod message; +pub mod variable; + +type NodeId = usize; diff --git a/gbp-rs/src/factorgraph/variable.rs b/gbp-rs/src/factorgraph/variable.rs index 60ad707c..2cb529a0 100644 --- a/gbp-rs/src/factorgraph/variable.rs +++ b/gbp-rs/src/factorgraph/variable.rs @@ -1,21 +1,25 @@ -/// +/// // pub trait Variable {} // struct RobotId(usize); -type RobotId = usize; +// type RobotId = usize; // struct NodeId(usize); -type NodeId = usize; +// #[derive(Debug, Copy, Clone, Eq, PartialEq, PartialOrd)] +// pub struct Variable { +// node_id: NodeId, +// robot_id: RobotId, +// } -#[derive(Debug, Copy, Clone, Eq, PartialEq, PartialOrd)] -pub struct Variable { - node_id: NodeId, - robot_id: RobotId, -} +// impl Variable { +// fn new(node_id: NodeId, robot_id: RobotId) -> Self { +// Self { node_id, robot_id } +// } +// } -impl Variable { - fn new(node_id: NodeId, robot_id: RobotId) -> Self { - Self { node_id, robot_id } - } +pub trait Variable { + fn id(&self) -> super::NodeId; + fn update_belief(&mut self); + fn prior_energy(&self) -> f64; } diff --git a/gbp-rs/src/lib.rs b/gbp-rs/src/lib.rs index 0fa1a659..f48101a7 100644 --- a/gbp-rs/src/lib.rs +++ b/gbp-rs/src/lib.rs @@ -3,8 +3,6 @@ pub mod factorgraph; pub mod prelude { pub use crate::factorgraph::factor::Factor; pub use crate::factorgraph::factorgraph::FactorGraph; - pub use crate::factorgraph::variable::Variable; pub use crate::factorgraph::message::Message; + pub use crate::factorgraph::variable::Variable; } - - diff --git a/gbpplanner-rs/src/shapes.rs b/gbpplanner-rs/src/shapes.rs index 33bf97c7..3fbeb979 100644 --- a/gbpplanner-rs/src/shapes.rs +++ b/gbpplanner-rs/src/shapes.rs @@ -28,11 +28,11 @@ fn set_colors( mut query: Query<(&Transform, &Handle)>, mut materials: ResMut>, ) { - println!("Inside set_colors"); + info!("Inside set_colors"); for (i, (_, handle)) in query.iter_mut().enumerate() { // let material = materials.get_mut(handle).unwrap(); if let Some(material) = materials.get_mut(handle) { - println!("Setting color to {:?}", colors.0[i]); + info!("Setting color to {:?}", colors.0[i]); let color = colors.0[i]; material.color = color; } diff --git a/hx.log b/hx.log new file mode 100644 index 00000000..8feed474 --- /dev/null +++ b/hx.log @@ -0,0 +1,3 @@ +2024-02-01T13:21:37.825 helix_lsp::transport [ERROR] rust-analyzer err <- "error: 'rust-analyzer' is not installed for the toolchain 'stable-x86_64-unknown-linux-gnu'\n" +2024-02-01T13:21:37.826 helix_lsp [ERROR] failed to initialize language server: server closed the stream +2024-02-01T13:21:37.826 helix_lsp::transport [ERROR] rust-analyzer err: <- StreamClosed diff --git a/shell.nix b/shell.nix deleted file mode 100644 index ee8a26a8..00000000 --- a/shell.nix +++ /dev/null @@ -1,15 +0,0 @@ -{ pkgs ? import { } }: - - with pkgs; - -mkShell rec { - nativeBuildInputs = [ - pkg-config - ]; - buildInputs = [ - udev alsa-lib vulkan-loader - xorg.libX11 xorg.libXcursor xorg.libXi xorg.libXrandr # To use the x11 feature - libxkbcommon wayland # To use the wayland feature - ]; - LD_LIBRARY_PATH = lib.makeLibraryPath buildInputs; -}