sophus_{autodiff,geo,lie} docs

.github/workflows/format.yml

@@ -13,6 +13,7 @@ jobs:
       steps:
         - name: Checkout workspace
           uses: actions/checkout@v3
+        - uses: extractions/setup-just@v1
         - name: Install pre-commit and install
           run: |
             pip install pre-commit
@@ -26,4 +27,4 @@ jobs:
           run: |
             rustup default nightly
             rustup component add rustfmt
-            cargo fmt
+            just format

Cargo.toml

@@ -53,6 +53,7 @@ nalgebra = { version = "0.33.2", features = ["rand", "bytemuck"] }
 ndarray = { version = "0.15", features = ["approx-0_5"] }
 num-traits = "0.2"
 rand = "0.8"
+rustc_version = "0.4"
 snafu = "0.8.5"
 typenum = { version = "1.17", features = ["const-generics"] }
 winit = { version = "0.30.5", features = ["android-native-activity"] }

README.md

@@ -11,16 +11,108 @@ sophus-rs is a Rust library for 2d and 3d geometry for Computer Vision and Robot
 It is a spin-off of the [Sophus](https://github.com/strasdat/Sophus) C++ library which
 focuses on *Lie groups* (e.g. rotations and transformations in 2d and 3d).
 
-In addition to Lie groups, sophus-rs also includes other geometric/maths concepts such unit vector,
-splines, image classes, camera models as well as a other utilities such as a non-linear least
-squares optimization.
+In addition to Lie groups, sophus-rs also includes other geometric/maths concepts.
 
-## Status
+## Automatic differentiation
 
-This library is in an early development stage - hence API is highly unstable. It is likely that
-existing features will be removed or changed in the future.
+sophus-rs provides an automatic differentiation using dual numbers such as
+[autodiff::dual::DualScalar] and [autodiff::dual::DualVector].
+
+```
+use sophus::prelude::*;
+use sophus::autodiff::dual::{DualScalar, DualVector};
+use sophus::autodiff::linalg::VecF64;
+use sophus::autodiff::maps::VectorValuedVectorMap;
+
+//       [[ x ]]   [[ x / z ]]
+//  proj [[ y ]] = [[       ]]
+//       [[ z ]]   [[ y / z ]]
+fn proj_fn<S: IsSingleScalar<DM, DN>, const DM: usize, const DN: usize>(
+    v: S::Vector<3>,
+) -> S::Vector<2> {
+    let x = v.elem(0);
+    let y = v.elem(1);
+    let z = v.elem(2);
+    S::Vector::<2>::from_array([x / z, y / z])
+}
+
+let a = VecF64::<3>::new(1.0, 2.0, 3.0);
+// Finite difference Jacobian
+let finite_diff = VectorValuedVectorMap::<f64, 1>::sym_diff_quotient_jacobian(
+    proj_fn::<f64, 0, 0>,
+    a,
+    0.0001,
+);
+// Automatic differentiation Jacobian
+let auto_diff = proj_fn::<DualScalar<3, 1>, 3, 1>(DualVector::var(a)).jacobian();
+
+approx::assert_abs_diff_eq!(finite_diff, auto_diff, epsilon = 0.0001);
+```
+
+Note that proj_fn is a function that takes a 3D vector and returns a 2D vector. The Jacobian of
+proj_fn is 2x3 matrix. When a (three dimensional) dual vector is passed to proj_fn, then
+a 2d dual vector is returned. Since we are expecting a 2x3 Jacobian, each element of the 2d dual
+vector must represent 3x1 Jacobian. This is why we use DualScalar<3, 1> as the scalar type.
+
+## Lie Groups
+
+sophus-rs provides a number of Lie groups, including:
+
+ * The group of 2D rotations, [lie::Rotation2], also known as the special orthogonal group SO(2),
+ * the group of 3D rotations, [lie::Rotation3], also known as the special orthogonal group SO(3),
+ * the group of 2d isometries, [lie::Isometry2], also known as the Euclidean group SE(2), and
+ * the group of 3d isometries, [lie::Isometry3], also known as the Euclidean group SE(3).
+
+
+```
+use sophus::autodiff::linalg::VecF64;
+use sophus::lie::{Rotation3F64, Isometry3F64};
+use std::f64::consts::FRAC_PI_4;
+
+// Create a rotation around the z-axis by 45 degrees.
+let world_from_foo_rotation = Rotation3F64::rot_z(FRAC_PI_4);
+
+// Create a translation in 3D.
+let foo_in_world = VecF64::<3>::new(1.0, 2.0, 3.0);
+
+// Combine them into an SE(3) transform.
+let world_from_foo_isometry
+    = Isometry3F64::from_translation_and_rotation(foo_in_world, world_from_foo_rotation);
+
+// Apply world_from_foo_isometry to a 3D point in the foo reference frame.
+let point_in_foo = VecF64::<3>::new(10.0, 0.0, 0.0);
+let point_in_world = world_from_foo_isometry.transform(&point_in_foo);
+
+// Manually compute the expected transformation:
+//  - rotate (10, 0, 0) around z by 45°
+//  - then translate by (1, 2, 3)
+let angle = FRAC_PI_4;
+let cos = angle.cos();
+let sin = angle.sin();
+let expected_point_in_world = VecF64::<3>::new(1.0 + 10.0 * cos, 2.0 + 10.0 * sin, 3.0);
+
+approx::assert_abs_diff_eq!(point_in_world[0], expected_point_in_world[0], epsilon = 1e-9);
+approx::assert_abs_diff_eq!(point_in_world[1], expected_point_in_world[1], epsilon = 1e-9);
+approx::assert_abs_diff_eq!(point_in_world[2], expected_point_in_world[2], epsilon = 1e-9);
+
+// Map isometry to 6-dimensional tangent space.
+let omega = world_from_foo_isometry.log();
+// Map tangent space element back to the manifold.
+let roundtrip_world_from_foo_isometry = Isometry3F64::exp(&omega);
+approx::assert_abs_diff_eq!(roundtrip_world_from_foo_isometry.matrix(),
+                            world_from_foo_isometry.matrix(),
+                            epsilon = 1e-9);
+
+// Compose with another isometry.
+let world_from_bar_isometry = Isometry3F64::rot_y(std::f64::consts::FRAC_PI_6);
+let bar_from_foo_isometry = world_from_bar_isometry.inverse() * world_from_foo_isometry;
+```
+
+## And more...
+
+such unit vector, splines, image classes, camera models, non-linear least squares optimization and
+some visualization tools. Check out the [documentation](https://docs.rs/sophus) for more information.
 
-However, the intend is to stride for correctness, facilitated by a comprehensive test suite.
 
 ## Building
 
@@ -31,12 +123,42 @@ sophus-rs builds on stable.
 sophus = "0.12.0"
 ```
 
+```toml
+[dependencies]
+sophus = { version = "0.12.0", features = ["simd"] }
+```
+
 To allow for batch types, such as BatchScalarF64, the 'simd' feature is required. This feature
 depends on [`portable-simd`](https://doc.rust-lang.org/std/simd/index.html), which is currently
 only available on [nightly](https://doc.rust-lang.org/book/appendix-07-nightly-rust.html). There
 are no plans to rely on any other nightly features.
 
-```toml
-[dependencies]
-sophus = { version = "0.12.0", features = ["simd"] }
-```
+## Crate Structure and Usage
+
+sophus-rs is an **umbrella crate** that provides a single entry point to multiple
+sub-crates (modules) under the `sophus::` namespace. For example, the automatic differentiation
+sub-crate can be accessed via `use sophus::autodiff`, and the lie group sub-crate via
+`use sophus::lie`, etc.
+
+- If you want all of sophus’s functionalities at once (geometry, AD, manifolds, etc.),
+  simply add `sophus` in your `Cargo.toml`, and then in your Rust code:
+
+  ```rust
+  use sophus::prelude::*;
+  use sophus::autodiff::dual::DualScalar;
+  // ...
+  ```
+
+- If you only need the autodiff functionalities in isolation, you can also depend on the
+  standalone crate underlying `sophus_autodiff`.
+
+   ```rust
+  use sophus_autodiff::prelude::*;
+  use sophus_autodiff::dual::DualScalar;
+  // ...
+  ```
+
+## Status
+
+This library is in an early development stage - hence API is highly unstable. It is likely that
+existing features will be removed or changed in the future.

crates/sophus/Cargo.toml

@@ -37,6 +37,9 @@ thingbuf.workspace = true
 tokio = { version = "1", features = ["full"] }
 wgpu.workspace = true
 
+[build-dependencies]
+rustc_version.workspace = true
+
 
 [features]
 simd = [

crates/sophus/build.rs

@@ -0,0 +1,10 @@
+fn main() {
+    println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition
+
+    let is_nightly =
+        rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly;
+
+    if is_nightly {
+        println!("cargo:rustc-cfg=nightly");
+    }
+}

crates/sophus/src/lib.rs

@@ -1,6 +1,11 @@
 #![cfg_attr(feature = "simd", feature(portable_simd))]
 #![allow(clippy::needless_range_loop)]
 #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))]
+#![cfg_attr(nightly, feature(doc_auto_cfg))]
+
+#[doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))]
+#[cfg(doctest)]
+pub struct ReadmeDoctests;
 
 #[doc(inline)]
 pub use sophus_autodiff as autodiff;
@@ -27,12 +32,22 @@ pub use sophus_timeseries as timeseries;
 #[doc(inline)]
 pub use sophus_viewer as viewer;
 
+/// Examples for the `sophus` umbrella crate. Note that this is not a comprehensive list of
+/// examples, and most usage examples are found in the individual sub-crates. In particular, the
+/// unit tests in each sub-crate are a good source of examples.
 pub mod examples;
 pub use eframe;
 pub use nalgebra;
 pub use ndarray;
 pub use thingbuf;
 
+/// sophus prelude.
+///
+/// It is recommended to import this prelude when working with `sophus` types:
+///
+/// ```
+/// use sophus_autodiff::prelude::*;
+/// ```
 pub mod prelude {
     pub use crate::{
         autodiff::prelude::*,

crates/sophus_autodiff/Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 description = "sophus - geometry for robotics and computer vision"
 name = "sophus_autodiff"
-readme = "../../README.md"
+readme = "../README.md"
 
 edition.workspace = true
 include.workspace = true
@@ -21,6 +21,9 @@ typenum.workspace = true
 
 sleef = {version = "0.3", optional = true}
 
+[build-dependencies]
+rustc_version.workspace = true
+
 [features]
 simd = ["sleef"]
 std = []

crates/sophus_autodiff/README.md

@@ -0,0 +1,100 @@
+This crate provides traits, structs, and helper functions to enable automatic
+differentiation (AD) through the use of dual numbers. In particular, it defines
+*dual scalar*, *dual vector*, and *dual matrix* types, along with their SIMD
+"batch" counterparts, for forward-mode AD in both scalar and vector/matrix contexts.
+
+## Overview
+
+The primary interfaces are defined by traits in [crate::linalg]:
+
+| **Trait**                  | **Real types**       | **Dual types**            | **Batch real types**           | **Batch dual types**              |
+|----------------------------|----------------------|---------------------------|---------------------------------|-----------------------------------|
+| [linalg::IsScalar]         | [f64]               | [dual::DualScalar]        | [linalg::BatchScalarF64]       | [dual::DualBatchScalar]           |
+| [linalg::IsVector]         | [linalg::VecF64]    | [dual::DualVector]        | [linalg::BatchVecF64]          | [dual::DualBatchVector]           |
+| [linalg::IsMatrix]         | [linalg::MatF64]    | [dual::DualMatrix]        | [linalg::BatchMatF64]          | [dual::DualBatchMatrix]           |
+
+- **Real types** are your basic floating-point types ([f64], etc.).
+- **Dual types** extend these scalars, vectors, or matrices to store partial
+  derivatives (the “infinitesimal” part) in addition to the real value.
+- **Batch real types** and **batch dual types** leverage Rust’s `portable_simd` to
+  process multiple derivatives or computations in parallel.
+
+**Note:** Batch types such as [linalg::BatchScalarF64], [linalg::BatchVecF64], and
+[linalg::BatchMatF64] require enabling the `"simd"` feature in your `Cargo.toml`.
+
+## Numerical differentiation
+In addition to forward-mode AD via dual numbers, this module also contains functionality
+to compute numerical derivatives (finite differences) of your functions in various shapes:
+
+ * **Curves,** functions which take a scalar input:
+    - f: ℝ -> ℝ, [maps::ScalarValuedCurve]
+    - f: ℝ -> ℝʳ, [maps::VectorValuedCurve]
+    - f: ℝ -> ℝʳˣᶜ, [maps::MatrixValuedCurve]
+ * **Scalar-valued maps**, functions which return a scalar:
+    - f: ℝᵐ -> ℝ, [maps::ScalarValuedVectorMap]
+    - f: ℝᵐ x ℝⁿ -> ℝ, [maps::ScalarValuedMatrixMap]
+ * **Vector-valued maps**, functions which return a vector:
+    - f: ℝᵐ -> ℝᵖ, [maps::VectorValuedVectorMap]
+    - f: ℝᵐˣⁿ -> ℝᵖ, [maps::VectorValuedMatrixMap]
+ * **Matrix-valued maps**, functions which return a matrix:
+    - f: ℝᵐ -> ℝʳˣᶜ, [maps::MatrixValuedVectorMap]
+    - f: ℝᵐˣⁿ -> ℝʳˣᶜ, [maps::MatrixValuedMatrixMap]
+
+You’ll find the associated functions for finite-difference computations in the
+[maps submodule][crate::maps].
+
+
+## Example: Differentiating a Vector Function
+
+```rust
+use sophus_autodiff::prelude::*;
+use sophus_autodiff::dual::{DualScalar, DualVector};
+use sophus_autodiff::maps::VectorValuedVectorMap;
+use sophus_autodiff::linalg::VecF64;
+
+// Suppose we have a function f: ℝ³ → ℝ²
+//
+//    [[ x ]]   [[ x / z ]]
+//  f [[ y ]] = [[       ]]
+//    [[ z ]]   [[ y / z ]]
+fn proj_fn<S: IsSingleScalar<DM, DN>, const DM: usize, const DN: usize>(
+    v: S::Vector<3>,
+) -> S::Vector<2> {
+    let x = v.elem(0);
+    let y = v.elem(1);
+    let z = v.elem(2);
+    S::Vector::<2>::from_array([x / z, y / z])
+}
+
+let input = VecF64::<3>::new(1.0, 2.0, 3.0);
+
+// (1) Finite difference approximation:
+let fd_jacobian = VectorValuedVectorMap::<f64, 1>::sym_diff_quotient_jacobian(
+    proj_fn::<f64, 0, 0>,
+    input,
+    1e-5,
+);
+
+// (2) Forward-mode autodiff using dual numbers:
+let auto_jacobian = proj_fn::<DualScalar<3, 1>, 3, 1>(DualVector::var(input)).jacobian();
+
+// Compare the two results:
+approx::assert_abs_diff_eq!(fd_jacobian, auto_jacobian, epsilon = 1e-5);
+```
+
+By using the [dual::DualScalar] (or [dual::DualVector], etc.) approach, we get the Jacobian
+via forward-mode autodiff. Alternatively, we can use finite differences for
+functions that might not be easily expressed in closed form but remain
+differentiable in practice.
+
+## See Also
+
+- [**linalg** module][crate::linalg] for linear algebra types and operations.
+- [**manifold** module][crate::manifold] for manifold operations and tangent spaces.
+- [**params** module][crate::params] for parameter-related traits.
+- [**points** module][crate::points] for point types.
+
+## Feature Flags
+
+- `"simd"`: Enables batch types like [dual::DualBatchScalar], [linalg::BatchVecF64], etc.
+  which require Rust’s nightly `portable_simd` feature.

crates/sophus_autodiff/build.rs

@@ -0,0 +1,10 @@
+fn main() {
+    println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition
+
+    let is_nightly =
+        rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly;
+
+    if is_nightly {
+        println!("cargo:rustc-cfg=nightly");
+    }
+}