Fix so3 log (#40)

* fix so3 log

* read g2o

Author: powei-lin

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "tiny-solver"
-version = "0.14.1"
+version = "0.14.2"
 edition = "2021"
 authors = ["Powei Lin <poweilin1994@gmail.com>"]
 readme = "README.md"
@@ -43,7 +43,9 @@ python = ["num-dual/python", "numpy", "pyo3"]
 [dev-dependencies]
 env_logger = "0.11.6"
 itertools = "0.14.0"
+nalgebra = { version = "0.33.2", features = ["rand"] }
 plotters = "0.3.6"
+rand = "0.8.5"
 
 [profile.dev.package.faer]
 opt-level = 3

README.md

@@ -9,6 +9,10 @@ Inspired by [ceres-solver](https://github.com/ceres-solver/ceres-solver), [tiny-
 
 This is a general optimizer written in Rust, including bindings for Python. If you're familiar with ceres-solver or factor-graph optimizers, you'll find it very easy to use.
 
+#### Other great rust optimizers
+* [factrs](https://github.com/rpl-cmu/factrs)
+* [sophus-rs](https://github.com/sophus-vision/sophus-rs)
+
 ## Installation
 ### rust
 ```sh

src/manifold/so3.rs

@@ -66,16 +66,30 @@ impl<T: na::RealField> SO3<T> {
     }
 
     pub fn log(&self) -> na::DVector<T> {
-        let xi = na::dvector![self.qx.clone(), self.qy.clone(), self.qz.clone()];
-        // Abs value in case we had a negative quaternion
-        let w = self.qw.clone().abs();
+        const EPS: f64 = 1e-6;
+        let ivec = na::dvector![self.qx.clone(), self.qy.clone(), self.qz.clone()];
 
-        let norm_v = xi.norm();
-        if norm_v < T::from_f64(1e-3).unwrap() {
-            xi * T::from_f64(2.0).unwrap()
-        } else {
-            xi * norm_v.clone().atan2(w) * T::from_f64(2.0).unwrap() / norm_v
-        }
+        let squared_n = ivec.norm_squared();
+        let w = self.qw.clone();
+
+        let near_zero = squared_n.le(&T::from_f64(EPS * EPS).unwrap());
+
+        let w_sq = w.clone() * w.clone();
+        let t0 = T::from_f64(2.0).unwrap() / w.clone()
+            - T::from_f64(2.0 / 3.0).unwrap() * squared_n.clone() / (w_sq * w.clone());
+
+        let n = squared_n.sqrt();
+
+        let sign = T::from_f64(-1.0)
+            .unwrap()
+            .select(w.le(&T::zero()), T::one());
+        let atan_nbyw = sign.clone() * n.clone().atan2(sign * w);
+
+        let t = T::from_f64(2.0).unwrap() * atan_nbyw / n;
+
+        let two_atan_nbyd_by_n = t0.select(near_zero, t);
+
+        ivec * two_atan_nbyd_by_n
     }
 
     pub fn hat(xi: na::VectorView3<T>) -> na::Matrix3<T> {
@@ -137,7 +151,7 @@ impl<T: na::RealField> SO3<T> {
         let qz = w0.clone() * z1.clone() + x0.clone() * y1.clone() - y0.clone() * x1.clone()
             + z0.clone() * w1.clone();
         let qw = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1;
-
+        // println!("q {}", self.to_vec());
         SO3 { qx, qy, qz, qw }
     }
 }

tests/test_manifold.rs

@@ -0,0 +1,26 @@
+#[cfg(test)]
+mod tests {
+    use nalgebra as na;
+    use std::f64::consts::PI;
+
+    use tiny_solver::manifold::so3::SO3;
+
+    fn equal_to_na(so3: &SO3<f64>) -> bool {
+        let q = so3.to_vec();
+        let na_so3 =
+            na::UnitQuaternion::from_quaternion(na::Quaternion::new(q[3], q[0], q[1], q[2]));
+        let diff = so3.log() - na_so3.scaled_axis();
+        diff.norm() < 1e-6
+    }
+
+    #[test]
+    fn test_so3() {
+        for _ in 0..10000 {
+            let mut rvec = na::DVector::new_random(3);
+            rvec /= rvec.norm();
+            rvec *= rand::random::<f64>() * PI;
+            let r = SO3::exp(rvec.as_view());
+            assert!(equal_to_na(&r));
+        }
+    }
+}