Merge pull request #29 from rpl-cmu/prep-release

Release v0.2.0

Author: contagon

Cargo.lock

@@ -1,6 +1,6 @@
 [package]
 name = "factrs"
-version = "0.1.0"
+version = "0.2.0"
 edition = "2021"
 license = "MIT"
 description = "Factor graph optimization for robotics"
@@ -32,8 +32,9 @@ foldhash = "0.1.4"
 paste = "1.0.15"
 downcast-rs = "2.0.1"
 log = "0.4.22"
-factrs-proc = { version = "0.1.0", path = "./factrs-proc" }
 pad-adapter = "0.1.1"
+dyn-clone = "1.0.17"
+factrs-proc = { version = "0.2.0", path = "./factrs-proc" }
 
 # numerical 
 faer = { version = "0.20.2", default-features = false, features = [
@@ -43,19 +44,18 @@ faer = { version = "0.20.2", default-features = false, features = [
 ] }
 faer-ext = { version = "0.4.1", features = ["nalgebra"] }
 nalgebra = { version = "0.33.2", features = ["compare"] }
+simba = { version = "0.9.0", default-features = false }
 num-dual = "0.11.0"
 matrixcompare = { version = "0.3.0" }
 
 # serialization
 serde = { version = "1.0.217", optional = true }
-typetag = { version = "0.2.18", optional = true, path = "./factrs-typetag" }
+factrs-typetag = { version = "0.2.0", optional = true, path = "./factrs-typetag" }
 
 # rerun support
 rerun = { version = "0.21.0", optional = true, default-features = false, features = [
     "sdk",
 ] }
-simba = { version = "0.9.0", default-features = false }
-dyn-clone = "1.0.17"
 
 
 [features]
@@ -74,7 +74,7 @@ rayon = ["faer/rayon"]
 # Add support for serialization
 serde = [
     "dep:serde",
-    "dep:typetag",
+    "dep:factrs-typetag",
     "factrs-proc/serde",
     "nalgebra/serde-serialize",
 ]

Cargo.toml

@@ -1,11 +1,11 @@
 [package]
 name = "factrs-bench"
-version = "0.1.0"
+version = "0.2.0"
 edition = "2021"
 
 [dependencies]
 nalgebra = { version = "0.33", features = ["compare"] }
-factrs = { version = "0.1.0", path = ".." }
+factrs = { version = "0.2.0", path = ".." }
 tiny-solver = { git = "https://github.com/contagon/tiny-solver-rs", branch = "rayon" }
 
 [dev-dependencies]

factrs-bench/Cargo.toml

@@ -1,17 +1,24 @@
 [package]
 name = "factrs-proc"
-version = "0.1.0"
+version = "0.2.0"
 edition = "2021"
+license = "MIT"
+description = "Proc-macros for factrs"
+authors = ["Easton Potokar", "Taylor Pool"]
+repository = "https://github.com/rpl-cmu/factrs"
+keywords = ["nonlinear", "optimization", "robotics", "estimation", "SLAM"]
+categories = ["science::robotics", "mathematics"]
+rust-version = "1.83"
 
 [lib]
 name = "factrs_proc"
 path = "src/lib.rs"
 proc-macro = true
 
 [dependencies]
-proc-macro2 = "1.0.89"
-quote = "1.0.37"
-syn = { version = "2.0.87", features = ["full"] }
+proc-macro2 = "1.0.93"
+quote = "1.0.38"
+syn = { version = "2.0.96", features = ["full"] }
 
 [features]
 serde = []

factrs-proc/Cargo.toml

@@ -58,7 +58,7 @@ pub struct DefaultSymbolHandler;
 
 impl DefaultSymbolHandler {
     pub fn sym_to_key(chr: char, idx: u32) -> Key {
-        Key((chr as u64) << IDX_SIZE | idx as u64 & IDX_MASK)
+        Key(((chr as u64) << IDX_SIZE) | (idx as u64 & IDX_MASK))
     }
 
     pub fn key_to_sym(k: Key) -> (char, u32) {

factrs-typetag

@@ -160,15 +160,21 @@ impl<T: Numeric> Variable for SO3<T> {
 
     fn log(&self) -> VectorX<T> {
         let xi = vectorx![self.xyzw.x, self.xyzw.y, self.xyzw.z];
-        // Abs value in case we had a negative quaternion
-        let w = self.xyzw.w.abs();
+        let w = self.xyzw.w;
 
-        let norm_v = xi.norm();
-        if norm_v < T::from(1e-3) {
-            xi * T::from(2.0)
+        let norm_v2 = xi.norm_squared();
+        let scale = if norm_v2 < T::from(1e-6) {
+            // Here we don't have to worry about the sign as it'll cancel out
+            T::from(2.0) / w - T::from(2.0 / 3.0) * norm_v2 / (w * w * w)
         } else {
-            xi * norm_v.atan2(w) * T::from(2.0) / norm_v
-        }
+            // flip both xi and w sign here (to reduce multiplications)
+            #[rustfmt::skip]
+            let sign = if w.is_sign_positive() { T::one() } else { T::from(-1.0) };
+            let norm_v = norm_v2.sqrt();
+            sign * norm_v.atan2(sign * w) * T::from(2.0) / norm_v
+        };
+
+        xi * scale
     }
 
     fn cast<TT: Numeric + SupersetOf<Self::T>>(&self) -> Self::Alias<TT> {

src/containers/symbol.rs

@@ -32,8 +32,8 @@ impl DoubleCharHandler {
         debug_assert!(chr1.is_ascii());
         debug_assert!(chr2.is_ascii());
 
-        Key((chr1 as u64) << IDX_SIZE << CHR_SIZE
-            | (chr2 as u64) << IDX_SIZE
+        Key(((chr1 as u64) << IDX_SIZE << CHR_SIZE)
+            | ((chr2 as u64) << IDX_SIZE)
             | (idx as u64) & IDX_MASK)
     }