smooth - GitLite

KO | EN

gitlite — search

#typescript #llm #go #golang #machine-learning #linux #docker #macos #java #windows #awesome #cli #security #ios

smooth

(★ 214)

Lie theory for robotics

File Explorer

Download Latest Version (.zip)

- - build_and_test.yml
  - docs.yaml
- ConfigTemplate.cmake.in
- version.hpp.in
- bspline.cpp
- CMakeLists.txt
- nls.cpp
- odeint.cpp
- odeint_diff.cpp
- plot_tools.hpp
- README.md
- snippets.cpp
- spline_fit.cpp
- spline_reparameterize.cpp
- - bundle.hpp
  - c1.hpp
  - derivatives.hpp
  - diff.hpp
  - galilei.hpp
  - lie_group_base.hpp
  - lie_groups.hpp
  - lie_sparse.hpp
  - manifolds.hpp
  - optim.hpp
  - se2.hpp
  - se3.hpp
  - se_k_3.hpp
  - so2.hpp
  - so3.hpp
  - wrt.hpp
- bspline.png
- ode.png
- adapted.hpp
- CMakeLists.txt
- nlreg_data.hpp
- test_adapted.cpp
- test_bundle.cpp
- test_c1.cpp
- test_ceres.cpp
- test_cspline.cpp
- test_d2rexp.cpp
- test_diff.cpp
- test_diff_analytic.cpp
- test_galilei.cpp
- test_hessian.cpp
- test_jacobians.cpp
- test_lie_api.cpp
- test_lie_dynamic.cpp
- test_manifold_any.cpp
- test_manifold_sub.cpp
- test_manifold_variant.cpp
- test_manifold_vector.cpp
- test_nlreg.cpp
- test_nls.cpp
- test_odeint.cpp
- test_optim.cpp
- test_polynomial.cpp
- test_ros.cpp
- test_se2.cpp
- test_se3.cpp
- test_se_k_3.cpp
- test_so2.cpp
- test_so3.cpp
- test_sparse.cpp
- test_spline.cpp
- test_spline_dubins.cpp
- test_spline_fit.cpp
- test_spline_reparameterize.cpp
- test_utils.cpp
.clang-format
.cmake-format.yaml
.codecov.yml
.gitignore
CMakeLists.txt
CMakePresets.json
Doxyfile
LICENSE
package.xml
README.md

# Installation Guide

1. Get the code

git clone https://github.com/pettni/smooth

Downloads the entire project code from GitHub to your computer.

cd smooth

Moves into the project folder you just downloaded.

2. CMake

Medium Recommended

Prerequisites

Git Needed to download the project code from GitHub.
CMake The tool used to generate build configuration.
C/C++ 컴파일러 Windows needs Visual Studio (Community edition, free), macOS needs Xcode Command Line Tools, Linux needs the gcc/g++ package.

cmake .. -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF

Analyzes the source code and generates build configuration files (must be run inside the build folder).

colcon build --cmake-args -DCMAKE_CXX_COMPILER=/usr/bin/g++-10

Type this command into your terminal and run it.

✅ Check that an executable was created inside the build folder, then run it directly (e.g. ./build/app_name).

Pulled directly from this repo's README.

// repository documentation

Was this content helpful?

★ 0 (0 ratings)

# Lie Theory for Robotics

[![CI Build and Test][ci-shield]][ci-link]
[![Code coverage][cov-shield]][cov-link]
[![License][license-shield]][license-link]

<img src="media/ode.png" width="300">  <img src="media/bspline.png" width="300">

* Requirements: C++20, Eigen 3.4
* [Documentation][doc-link]
* Compatible with: autodiff, boost::numeric::odeint, Ceres, ROS
* Written in an extensible functional programming style

In robotics it is often convenient to work in non-Euclidean manifolds.
[Lie groups](https://en.wikipedia.org/wiki/Lie_group) are a class of manifolds that are
easy to work with due to their symmetries, and that are also good models for many robotic
systems. This header-only C++20 library facilitates leveraging Lie theory in
robotics software, by enabling:

* Algebraic manipulation
 * Automatic differentiation
 * Interpolation (right figure shows a B-spline of order 5 on smooth::SO3, see `examples/bspline.cpp`)
 * Numerical integration (left figure shows the solution of an ODE on ![](https://latex.codecogs.com/png.latex?\mathbb{SO}(3)\times\mathbb{R}^3), see `examples/odeint.cpp`)
 * Optimization

The following common Lie groups are implemented:
 * smooth::SO2: two-dimensional rotations with complex number ![](https://latex.codecogs.com/png.latex?\mathbb{C}(1)) memory representation
 * smooth::SO3: three-dimensional rotations with quaternion ![](https://latex.codecogs.com/png.latex?\mathbb{S}^3) memory representation
 * smooth::SE2: two-dimensional rigid motions
 * smooth::SE3: three-dimensional rigid motions
 * smooth::C1: complex numbers (excluding zero) under multiplication
 * smooth::Galilei: the [Galilean group](https://en.wikipedia.org/wiki/Galilean_transformation#Galilean_group). It includes SE\_2(3) as a special case.
 * smooth::SE_K_3: generalization of SE3 with multiple translations
 * A smooth::Bundle type to treat Lie group products ![](https://latex.codecogs.com/png.latex?G&space;=&space;G_1&space;\times&space;\ldots&space;\times&space;G_n) as a single Lie group. The Bundle type also supports regular Eigen vectors as ![](https://latex.codecogs.com/png.latex?\mathbb{R}^n\cong\mathbb{T}(n)) components
 * Lie group interfaces for Eigen vectors and builtin scalars

# Getting started

## Download and Build

Clone the repository and install it
```bash
git clone https://github.com/pettni/smooth.git
cd smooth
mkdir build && cd build

# Specify a C++20-compatible compiler if your default does not support C++20.
# Build tests and/or examples as desired.
cmake .. -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF
make -j8
sudo make install
```

Alternatively, if using ROS or ROS2 just clone `smooth` into a
catkin/[colcon](https://colcon.readthedocs.io/en/released/) workspace source folder and build the
workspace with a compiler that supports C++20. Example with colcon:
```bash
colcon build --cmake-args -DCMAKE_CXX_COMPILER=/usr/bin/g++-10
```

## Use with cmake

To utilize `smooth` in your own project, include something along these lines in your `CMakeLists.txt`
```cmake
find_package(smooth)

add_executable(my_executable main.cpp)
target_link_libraries(my_executable smooth::smooth)
```

## Explore the API

Check out the [Documentation][doc-link] and the [`examples`](https://github.com/pettni/smooth/tree/master/examples).

# Using the library

## Algebraic Manipulations

```cpp
 // Also works with other types: SO2d, SE2d, SE3d, Bundle<SO3d, T3d> etc...

using Tangent = typename smooth::SO3d::Tangent;

// construct a random group element and a random tangent element
 smooth::SO3d g = smooth::SO3d::Random();
 Tangent a = Tangent::Random();

// lie group exponential
 auto exp_a = smooth::SO3d::exp(a);

// lie group logarithm
 auto g_log = g.log();

// lie algebra hat and vee maps
 auto a_hat = smooth::SO3d::hat(a);
 auto a_hat_vee = smooth::SO3d::vee(a_hat);

// group adjoint
 auto Ad_g = g.Ad();

// lie algebra adjoint
 auto ad_a = smooth::SO3d::ad(a);

// derivatives of the exponential map
 auto dr_exp_v = smooth::SO3d::dr_exp(a);   // right derivative
 auto dl_exp_v = smooth::SO3d::dl_exp(a);   // left derivative
 auto dr_expinv_v = smooth::SO3d::dr_expinv(a);   // inverse of right derivative
 auto dl_expinv_v = smooth::SO3d::dl_expinv(a);   // inverse of left derivative

// group action
 Eigen::Vector3d v = Eigen::Vector3d::Random();
 auto v_transformed = g * v;

// memory mapping using Eigen::Map
 std::array<double, smooth::SO3d::RepSize> mem;
 Eigen::Map<const smooth::SO3d> m_g(mem.data());
 ```

## Concepts and Types

These [C++20 concepts](https://en.cppreference.com/w/cpp/concepts) are implemented in `concepts.hpp`.

* `Manifold`: type for which `rplus` (geodesic addition) and `rminus` (geodesic subtraction) are defined. Examples:
  * All `LieGroup` types
  * `std::vector<Manifold>` is a Manifold defined in `manifold_vector.hpp`---it facilitates e.g. optimization and differentiation w.r.t. a dynamic number of `Manifold`s
  * `std::variant<Manifold ...>` is a Manifold defined in `manifold_variant.hpp`. Using `std::vector<std::variant<Manifold...>>` can be convenient when optimizing over variables with different parameterizations.

* `LieGroup`: type for which Lie group operations (`exp`, `log`, `Ad`, etc...) are defined. Examples:
  * All `NativeLieGroup` types
  * Fixed-size Eigen vectors (e.g. `Eigen::Vector3d`)
  * Dynamic-size Eigen vectors (e.g. `Eigen::VectorXd`)
  * Built-in scalars (e.g. `double`)

* `NativeLieGroup`: type that implements the Lie group operations as class methods. Examples:
  * `smooth::SO3<float>`
  * `smooth::C1<double>`
  * `smooth::Bundle<NativeLieGroup | Eigen::Matrix<Scalar, N, 1> ...>`

Both `Manifold` and `LieGroup` are defined via external type traits (`traits::man` and `traits::lie`) that can be specialized in order to define `Manifold` or `LieGroup` interfaces for third-party types.

## Algorithms

### Tangent space differentiation

Available for `Manifold` types, see diff.hpp.

Supported techniques (see smooth::diff::Type):
* Numerical derivatives (default)
* Automatic differentiation using [`autodiff`](https://autodiff.github.io) (must #include <smooth/compat/autodiff.hpp>)
* Automatic differentiation using [Ceres 2.x](http://ceres-solver.org/) (must #include <smooth/compat/ceres.hpp>)

Example: calculate ![](https://latex.codecogs.com/png.latex?\mathrm{d}^r&space;(\log(g_1&space;\circ&space;g_2))_{g_i}) for i=1, 2

```cpp
#include <smooth/diff.hpp>
#include <smooth/so3.hpp>

auto f = []<typename T>(const smooth::SO3<T> & v1, const smooth::SO3<T> & v2) {
  return (v1 * v2).log();
};

smooth::SO3d g1 = smooth::SO3d::Random();
smooth::SO3d g2 = smooth::SO3d::Random();

// differentiate f at (g1, g2) w.r.t. first argument
auto [fval1, J1] = smooth::diff::dr<1>(f, smooth::wrt(g1, g2), std::index_sequence<0>{});

// differentiate f at (g1, g2) w.r.t. second argument
auto [fval2, J2] = smooth::diff::dr<1>(f, smooth::wrt(g1, g2), std::index_sequence<1>{});

// differentiate f at (g1, g2) w.r.t. both arguments
auto [fval, J] = smooth::diff::dr<1>(f, smooth::wrt(g1, g2));

// Now J == [J1, J2]
```

### Non-linear least squares optimization

Available for `Manifold` types, see optim.hpp.

The minimize() function implements a Levenberg-Marquardt trust-region procedure to find
a local minimum.  All derivatives and computations are done in the tangent space as opposed
to e.g. Ceres which uses derivatives w.r.t. the parameterization.

A sparse solver is implemented, but it is currently only available when analytical
derivatives are provided.

Example: Calculate ![](https://latex.codecogs.com/svg.image?\mathrm{argmin}_{g_1}&space;\|\log(g_1&space;\circ&space;g_2&space;)&space;\||)

```cpp
#include <smooth/optim.hpp>
#include <smooth/so3.hpp>

smooth::SO3d g1       = smooth::SO3d::Random();
const smooth::SO3d g2 = smooth::SO3d::Random();

// function defining residual
auto f = [&g2]<typename T>(const smooth::SO3<T> & v1) {
  return (v1 * g2.template cast<T>()).log();
};

// minimize || f ||^2 w.r.t. g1 (g1 is modified in-place)
smooth::minimize(f, smooth::wrt(g1));

// Now g1 == g2.inverse()
```

### Piecewise polynomial curve evaluation and fitting

Available for `LieGroup` types, see spline/spline.hpp.

These splines are piecewise defined via
[Bernstein polynomials](https://en.wikipedia.org/wiki/Bernstein_polynomial) and pass through
the control points. See examples/spline_fit.cpp for usage.

### B-spline evaluation and fitting

Available for `LieGroup` types, see spline/bspline.hpp.

The B-spline basis functions have local support, A B-spline generally does not pass through its control points.
See examples/spline_fit.cpp and examples/bspline.cpp for usage.

# Compatibility

Utility headers for interfacing with adjacent software are included.

* compat/autodiff.hpp: Use the [autodiff](https://autodiff.github.io/) library as a back-end for automatic differentiation
* compat/ceres.hpp: Local parameterization for [Ceres](http://ceres-solver.org/) on-manifold optimization, and use the Ceres automatic differentiation as a back-end
* compat/odeint.hpp: Numerical integration using [`boost::odeint`](https://www.boost.org/doc/libs/1_76_0/libs/numeric/odeint/doc/html/index.html)
* compat/ros.hpp: Memory mapping of [ROS/ROS2](https://www.ros.org/) message types

# Related Projects

* [smooth_feedback](https://pettni.github.io/smooth_feedback) utilizes `smooth` for control and estimation on Lie groups.

Two projects that have served as inspiration for `smooth` are [`manif`][manif-link]---which
also has an accompanying [paper][manif-paper-link] that is a great practical introduction to
Lie theory---and [`Sophus`][sophus-link]. Certain design decisions are different in
`smooth`: derivatives are with respect to tangent elements as in `manif`, but the tangent
types are Eigen vectors like in `Sophus`. This library also includes the Bundle type which
facilitates control and estimation tasks, as well as utilities such as differentiation,
optimization, and splines. Finally `smooth` is written in C++20 and leverages modern
features such as [concepts](https://en.cppreference.com/w/cpp/language/constraints) and
[ranges](https://en.cppreference.com/w/cpp/ranges).

[doc-link]: https://pettni.github.io/smooth

[ci-shield]: https://img.shields.io/github/actions/workflow/status/pettni/smooth/build_and_test.yml?style=flat-square
[ci-link]: https://github.com/pettni/lie/actions/workflows/build_and_test.yml

[cov-shield]: https://img.shields.io/codecov/c/gh/pettni/smooth/master?style=flat-square
[cov-link]: https://codecov.io/gh/pettni/smooth

[license-shield]: https://img.shields.io/github/license/pettni/smooth.svg?style=flat-square
[license-link]: https://github.com/pettni/smooth/blob/master/LICENSE

[manif-link]: https://github.com/artivis/manif/
[manif-paper-link]: https://arxiv.org/abs/1812.01537
[sophus-link]: https://github.com/strasdat/Sophus/