diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 987b0005..aef10dcd 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -19,21 +19,23 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-16.04, ubuntu-18.04, ubuntu-20.04] + os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04] build_type: [Release, Debug] - compiler: [{ - "cc": "gcc", - "cxx": "g++" - }, { - "cc": "clang", - "cxx": "clang++" - }] + compiler: [ + { + "cc": "gcc", + "cxx": "g++" + }, { + "cc": "clang", + "cxx": "clang++" + } + ] env: CC: ${{ matrix.compiler.cc }} CXX: ${{ matrix.compiler.cxx }} steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Setup run: | sudo apt update @@ -58,16 +60,10 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - # Xcode 10.3 & Xcode 12.2 - # removing macos-11.0 for now, see - #https://github.com/actions/virtual-environments/issues/841 - os: [macos-10.15] + os: [macos-12, macos-13, macos-14] steps: - name: Checkout - uses: actions/checkout@v2 - - name: Xcode - if: matrix.os == 'macOS-10.15' - run: sudo xcode-select -s /Applications/Xcode_10.3.app/Contents/Developer + uses: actions/checkout@v4 - name: Setup run: | brew install eigen @@ -79,7 +75,7 @@ jobs: echo "Eigen:" && brew info eigen - name: Configure CMake working-directory: ${{runner.workspace}}/build - run: cmake $GITHUB_WORKSPACE -DBUILD_EXAMPLES=ON -DBUILD_TESTING=ON + run: cmake $GITHUB_WORKSPACE -DBUILD_EXAMPLES=ON -DBUILD_TESTING=ON -DCMAKE_BUILD_TYPE=Release - name: Build working-directory: ${{runner.workspace}}/build run: make -j2 @@ -91,16 +87,18 @@ jobs: runs-on: ${{ matrix.combinations.os }} strategy: matrix: - combinations: [{ - "os": "windows-2016", - "cmake_generator": "Visual Studio 15 2017" - }, { - "os": "windows-2019", - "cmake_generator": "Visual Studio 16 2019" - }] + combinations: [ + { + "os": "windows-2019", + "cmake_generator": "Visual Studio 16 2019" + }, { + "os": "windows-2022", + "cmake_generator": "Visual Studio 17 2022" + } + ] steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Setup run: | vcpkg install eigen3:x64-windows @@ -126,10 +124,11 @@ jobs: needs: [build-ubuntu] runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - name: Setup run: | sudo apt update + sudo apt install -y libunwind-dev sudo apt install -y libceres-dev cppcheck mkdir ${{runner.workspace}}/build - name: Configure CMake @@ -147,10 +146,11 @@ jobs: runs-on: ubuntu-latest steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Setup run: | sudo apt update + sudo apt install -y libunwind-dev sudo apt install -y libceres-dev valgrind mkdir ${{runner.workspace}}/build - name: Configure CMake @@ -168,10 +168,11 @@ jobs: runs-on: ubuntu-latest steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Setup run: | sudo apt update + sudo apt install -y libunwind-dev sudo apt install -y libceres-dev mkdir ${{runner.workspace}}/build - name: Configure CMake @@ -189,13 +190,20 @@ jobs: ceres: needs: [build-ubuntu] - runs-on: ubuntu-latest + # runs-on: ubuntu-latest + runs-on: ${{ matrix.os }} + strategy: + fail-fast: false + # Test for both Ceres pre/post 2.2 + matrix: + os: [ubuntu-22.04, ubuntu-24.04] steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Setup run: | sudo apt update + sudo apt install -y libunwind-dev sudo apt install -y libceres-dev mkdir ${{runner.workspace}}/build - name: Configure CMake @@ -208,18 +216,55 @@ jobs: working-directory: ${{runner.workspace}}/build run: make test - pybind11: + autodiff: + needs: [build-ubuntu] + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Setup + run: | + sudo apt update + sudo apt install -y libeigen3-dev + mkdir ${{runner.workspace}}/build + # Install autodiff + - name: Checkout autodiff + uses: actions/checkout@v3 + with: + repository: autodiff/autodiff + ref: main + path: 'autodiff' + - name: Setup autodiff + run: mkdir ${{runner.workspace}}/build_autodiff + - name: Configure CMake autodiff + working-directory: ${{runner.workspace}}/build_autodiff + run: cmake $GITHUB_WORKSPACE/autodiff -DAUTODIFF_BUILD_TESTS=OFF -DAUTODIFF_BUILD_PYTHON=OFF -DAUTODIFF_BUILD_EXAMPLES=OFF -DAUTODIFF_BUILD_DOCS=OFF + - name: Install autodiff + working-directory: ${{runner.workspace}}/build_autodiff + run: sudo cmake --build . --target install + # Build/test manif autodiff + - name: Configure CMake + working-directory: ${{runner.workspace}}/build + run: cmake $GITHUB_WORKSPACE -DBUILD_TESTING=ON + - name: Build + working-directory: ${{runner.workspace}}/build + run: make -j2 + - name: Test + working-directory: ${{runner.workspace}}/build + run: make test + + pybind11-pip: needs: [build-ubuntu, build-mac] strategy: fail-fast: false matrix: platform: [macos-latest, ubuntu-latest] #windows-latest, - # python-version: [3.5, 3.6, 3.7, 3.8] - python-version: [3.6] + python-version: ['3.8', '3.9', '3.10', '3.12'] runs-on: ${{ matrix.platform }} steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 + - run: git fetch --prune --unshallow - name: Set up Python ${{ matrix.python-version }} uses: actions/setup-python@v2 with: @@ -235,27 +280,42 @@ jobs: - name: Setup run: | python -m pip install --upgrade pip - pip install pytest "pybind11[global]" - pip install -r requirements.txt + python -m pip install build - name: Build - run: pip install . + run: python -m pip install -v .[testing] - name: Test - run: pytest + run: python -m pytest - # arm64: - # needs: [build-ubuntu] - # runs-on: ubuntu-latest - # steps: - # - name: Checkout - # uses: actions/checkout@v2 - # - name: Setup - # run: mkdir ${{runner.workspace}}/build - # - name: Configure CMake - # working-directory: ${{runner.workspace}}/build - # run: cmake $GITHUB_WORKSPACE -DENABLE_COVERAGE=ON -DBUILD_TESTING=ON - # - name: Build - # working-directory: ${{runner.workspace}}/build - # run: make - # - name: Test - # working-directory: ${{runner.workspace}}/build - # run: make test + pybind11-cmake: + needs: [build-ubuntu, build-mac] + strategy: + matrix: + platform: [ubuntu-20.04, ubuntu-22.04, ubuntu-latest] + runs-on: ${{ matrix.platform }} + steps: + - name: Checkout + uses: actions/checkout@v4 + - run: git fetch --prune --unshallow + # - name: Set up Python ${{ matrix.python-version }} + # uses: actions/setup-python@v2 + # with: + # python-version: ${{ matrix.python-version }} + - name: Setup apt + run: | + sudo apt update + sudo apt install -y libeigen3-dev pybind11-dev python3-pytest python3-numpy + mkdir ${{runner.workspace}}/build + - name: Configure + working-directory: ${{runner.workspace}}/build + run: cmake $GITHUB_WORKSPACE -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=ON -DBUILD_PYTHON_BINDINGS=ON -DCMAKE_BUILD_TYPE=Release + - name: Build + working-directory: ${{runner.workspace}}/build + run: make -j2 + - name: Test + working-directory: ${{runner.workspace}}/build + run: make test + - name: Install + working-directory: ${{runner.workspace}}/build + run: sudo make install + - name: Test Import + run: python3 -c 'import manifpy' diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index fe32cd99..0febc476 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -2,6 +2,9 @@ name: documentation on: push: branches: devel + pull_request: + branches: + - devel workflow_dispatch: jobs: @@ -27,10 +30,9 @@ jobs: - name: Setup run: | python -m pip install --upgrade pip - pip install pytest "pybind11[global]" - pip install -r requirements.txt + python -m pip install build - name: Build - run: pip install . + run: python -m pip install -v . # build: # runs-on: ubuntu-20.04 @@ -58,8 +60,10 @@ jobs: - name: Fetch Python deps run: python -m pip install jinja2 Pygments docutils - name: Fetch m.css - working-directory: ${{runner.workspace}}/manif/docs - run: git clone git://github.com/mosra/m.css + uses: actions/checkout@v3 + with: + repository: mosra/m.css + path: docs/m.css - name: Build Python docs working-directory: ${{runner.workspace}}/manif/docs @@ -86,6 +90,10 @@ jobs: deploy: runs-on: ubuntu-20.04 needs: [build] + # todo: deploy if new tag/release + if: | + github.repository == 'artivis/manif' && + github.event_name == 'push' && github.ref == 'refs/heads/devel' steps: - name: Download artifacts uses: actions/download-artifact@v2 diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 00000000..c6c4ea1a --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,72 @@ +name: release +on: + push: + tags: + - '*' + pull_request: + branches: + - devel # master only when ready + - master + workflow_dispatch: + +jobs: + + build-sdist: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v2 + - run: git fetch --prune --unshallow + + - name: Install Python + uses: actions/setup-python@v2 + with: + python-version: 3.7 + + - name: Setup apt + run: | + sudo apt update + sudo apt install -y libeigen3-dev + + - name: Setup + run: | + python3 -m pip install --upgrade pip + pip3 install build + + - name: Build sdist + run: python3 -m build --sdist -o dist/ + + - name: Build wheel + run: python3 -m build --wheel -o dist/ + + - name: Upload artifacts + uses: actions/upload-artifact@v2 + with: + name: dist + path: dist/* + # path: | + # path/*.whl + # path/*.tar.gz + + upload_pypi: + needs: build-sdist + runs-on: ubuntu-latest + steps: + + - uses: actions/download-artifact@v2 + with: + name: dist + path: dist + + - name: Inspect dist folder + run: ls -lah dist/ + + # @todo: see https://github.com/diegoferigo/manif/pull/1#discussion_r668531581 + # - uses: pypa/gh-action-pypi-publish@master + # if: | + # github.repository == 'artivis/manif' && + # ((github.event_name == 'release' && github.event.action == 'published') || + # (github.event_name == 'push' && github.ref == 'refs/heads/main')) + # with: + # user: __token__ + # password: ${{ secrets.PYPI_TOKEN }} diff --git a/.gitignore b/.gitignore index 15efcd57..feb5f881 100644 --- a/.gitignore +++ b/.gitignore @@ -15,3 +15,4 @@ docs/m.css *.so *__pycache__ .pytest_cache +dist diff --git a/CMakeLists.txt b/CMakeLists.txt index a50fd0cf..bcc55a4a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,6 +61,15 @@ option(BUILD_PYTHON_BINDINGS "Build Python bindings with pybind11." OFF) option(BUILD_TESTING_PYTHON "Build Python tests only." OFF) if (BUILD_PYTHON_BINDINGS) + # In theory this is not required, as CMake selects the appropriate C++ standard + # However, as long as we support pybind11 < 2.6 we need to make sure that CMAKE_CXX_STANDARD + # is defined before find_package(pybind11 REQUIRED) to avoid that the CMake standard + # command line option is set by pybind11 via the PYBIND11_CPP_STANDARD variable, in a way that could + # interfere with CMake, see https://github.com/pybind/pybind11/blob/v2.4.3/tools/pybind11Tools.cmake#L21 + if(NOT DEFINED CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 11) + endif() + find_package(pybind11 REQUIRED) add_subdirectory(python) endif() @@ -101,6 +110,13 @@ else(TARGET Eigen3::Eigen) target_include_directories(${PROJECT_NAME} SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIRS}) endif(TARGET Eigen3::Eigen) +# Add tl-optional interface dependency if enabled +if(tl-optional_FOUND) + set(tl-optional_DEPENDENCY "find_dependency(tl-optional)") +else() + set(tl-optional_DEPENDENCY "") +endif() + if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") target_compile_options(${PROJECT_NAME} INTERFACE -ftemplate-depth=512) elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") @@ -152,6 +168,7 @@ write_basic_package_version_file( "${generated_dir}/${PROJECT_NAME}ConfigVersion.cmake" VERSION ${PROJECT_VERSION} COMPATIBILITY AnyNewerVersion + ARCH_INDEPENDENT ) # Config @@ -244,7 +261,11 @@ if(ENABLE_CPPCHECK) --suppress=unknownMacro:*test/common_tester.h --suppress=unknownMacro:*test/ceres/ceres_test_utils.h --suppress=constStatement:*examples* - --suppress=unreadVariable:*gtest_se2_autodiff.cpp:101 + --suppress=constStatement:*gtest_misc.cpp:35 + --suppress=constStatement:*gtest_misc.cpp:43 + --suppress=unreadVariable:*gtest_se2_ceres_autodiff.cpp:101 + --suppress=unreadVariable:*bundle_sam.cpp:94 + --suppress=unreadVariable:*bundle_sam.cpp:124 # Uncomment the line below to run cppcheck-html # --output-file=${CMAKE_BINARY_DIR}/cppcheck_results.xml ${CMAKE_SOURCE_DIR}/include diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 18eeb4ba..e85f37e3 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -42,8 +42,7 @@ Let's install all dependencies for development and testing, ```terminal apt install libeigen3-dev -python3 -m pip install "pybind11[global]" pytest -python3 -m pip install -r requirements +python3 -m pip install "pybind11[global]" pytest numpy ``` We can now build **manif**, its Python wrappers and all tests, diff --git a/README.md b/README.md index e34d9a0b..5a9cf91f 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,10 @@ At the moment, it provides the groups: introduced (to the best of knowledge) in this [paper][barrau15]. NOTE: The implementation here differs slightly from the developments in the [paper][barrau15]. +- SGal(3): The Special Galilean group (rotation, translation, velocity and time) in 3D space, described in these papers [[1][fourmy19]] & [[2][kelly24]]. +- Bundle<>: allows manipulating a manifold bundle as a single Lie group. + Referred to as a *composite manifold* in Section IV of the + [reference paper](http://arxiv.org/abs/1812.01537). Other Lie groups can and will be added, contributions are welcome. @@ -123,6 +127,16 @@ in Python, X_plus_w = X.plus(w, J_o_x, J_o_w) ``` +#### Note + +While Jacobians in **manif** are differentiated with respect to a +local perturbation on the tangent space, many non-linear solvers +(e.g. [Ceres][ceres]) expect functions to be differentiated with respect to the underlying representation vector of the group element +(e.g. with respect to quaternion vector for `SO3`). + +For this reason, **manif** is compliant with the auto-differentiation libraries +[`ceres::Jet`][ceres-jet], [`autodiff::Dual`][autodiff] & [`autodiff::Real`][autodiff]. + ## Documentation The documentation is available online at the accompanying [website][manif-doc]. @@ -157,6 +171,8 @@ Want to contribute? Great! Check out our [contribution guidelines](CONTRIBUTING. [jsola18]: http://arxiv.org/abs/1812.01537 [jsola18v]: http://arxiv.org/abs/1812.01537v4 [barrau15]: https://arxiv.org/pdf/1410.1465.pdf +[fourmy19]: https://hal.science/hal-02183498/document +[kelly24]: https://arxiv.org/abs/2312.07555 [deray20]: https://joss.theoj.org/papers/10.21105/joss.01371 [jsola-iri-lecture]: https://www.youtube.com/watch?v=nHOcoIyJj2o @@ -166,6 +182,7 @@ Want to contribute? Great! Check out our [contribution guidelines](CONTRIBUTING. [eigen]: http://eigen.tuxfamily.org [ceres]: http://ceres-solver.org/ [ceres-jet]: http://ceres-solver.org/automatic_derivatives.html#dual-numbers-jets +[autodiff]: https://autodiff.github.io/ [crtp]: https://en.wikipedia.org/wiki/Curiously_recurring_template_pattern [manif-repo]: https://github.com/artivis/manif.git diff --git a/cmake/manifConfig.cmake.in b/cmake/manifConfig.cmake.in index bbbc6987..863d5b57 100644 --- a/cmake/manifConfig.cmake.in +++ b/cmake/manifConfig.cmake.in @@ -2,6 +2,7 @@ include(CMakeFindDependencyMacro) @Eigen3_DEPENDENCY@ +@tl-optional_DEPENDENCY@ if(NOT TARGET @PROJECT_NAME@) include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") diff --git a/docs/pages/cpp/On-the-use-with-Ceres.md b/docs/pages/cpp/On-the-use-with-Ceres.md index 1830dfe5..e1a6eaff 100644 --- a/docs/pages/cpp/On-the-use-with-Ceres.md +++ b/docs/pages/cpp/On-the-use-with-Ceres.md @@ -94,8 +94,8 @@ ceres::Problem problem(problem_options); problem->AddParameterBlock(my_state.data(), 4); // Associate a LocalParameterization to the state vector -problem_->SetParameterization(my_state.data(), - new EigenQuaternionParameterization() ); +problem_->SetManifold(my_state.data(), + new EigenQuaternionParameterization() ); ``` The `LocalParameterization` class (and derived) performs the state update step @@ -179,7 +179,7 @@ In this example, we compute an average point from 4 points in `SE2`. // Tell ceres not to take ownership of the raw pointers ceres::Problem::Options problem_options; problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; -problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; +problem_options.manifold_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; ceres::Problem problem(problem_options); @@ -217,12 +217,12 @@ problem.AddResidualBlock( obj_3_pi_over_4.get(), // We use a second manif helper that creates a ceres local parameterization // for our optimized state block. -std::shared_ptr - auto_diff_local_parameterization = - manif::make_local_parametrization_autodiff(); +std::shared_ptr + auto_diff_manifold = + manif::make_manifold_autodiff(); -problem.SetParameterization( average_state.data(), - auto_diff_local_parameterization.get() ); +problem.SetManifold( average_state.data(), + auto_diff_manifold.get() ); // Run the solver! ceres::Solver::Options options; diff --git a/docs/pages/cpp/autodiff.md b/docs/pages/cpp/autodiff.md new file mode 100644 index 00000000..aa193352 --- /dev/null +++ b/docs/pages/cpp/autodiff.md @@ -0,0 +1,132 @@ +# Autodiff + +- [Autodiff](#autodiff) + - [Jacobians](#jacobians) + +The **manif** package differentiates Jacobians with respect to a +local perturbation on the tangent space. +These Jacobians map tangent spaces, as described in [this paper][jsola18]. + +However, many non-linear solvers +(e.g. [Ceres][ceres]) expect functions to be differentiated with respect to the underlying +representation vector of the group element +(e.g. with respect to quaternion vector for `SO3`). + +For this reason **manif** is compliant with the [`autodiff::dual`][autodiff] +auto-differentiation type. + +For reference of the size of the Jacobians returned when using [`autodiff::dual`][autodiff], +**manif** implements rotations in the following way: + +- SO(2) and SE(2): as a complex number with `real = cos(theta)` and `imag = sin(theta)` values. +- SO(3), SE(3) and SE_2(3): as a unit quaternion, using the underlying `Eigen::Quaternion` type. + +Therefore, the respective Jacobian sizes using [`autodiff::dual`][autodiff] are as follows: + +- ℝ(n) : size n +- SO(2) : size 2 +- SO(3) : size 4 +- SE(2) : size 4 +- SE(3) : size 7 +- SE_2(3): size 10 + +## Jacobians + +Considering, + +![x][latex2] a group element (e.g. S3), +![omega][latex3] the vector tangent to the group at ![x][latex4], +![f(x)][latex5] an error function, + +one is interested in expressing the Taylor series of the error function, + +![f(x(+)omega)][latex6] + +Therefore we have to compute the Jacobian + +![J_e_omega][latex7] + +the **Jacobian of** ![f(x)][latex8] **with respect to a perturbation on the tangent space**, +so that the state update happens on the manifold tangent space. + +In some optimization frameworks, the computation of this Jacobian is decoupled +in two folds as explained hereafter. + +Using the **autodiff** library, +a cost function can very easily designed as follows, + +```cpp +// functor to be evaluated +auto fun = [](const auto& measurement, const auto& state_i, const auto& state_j){ + return measurement - (state_j - state_i); +}; +``` + +where `state_i` & `state_j` belong to a group +and `measurement` belongs to the group's tangent. + +Evaluating the function and its Jacobians is then, + +```cpp +using namespace autodiff; +Eigen::MatrixXd J_e_xi = jacobian(fun, wrt(xi), at(meas_ij, xi, xj), e); +Eigen::MatrixXd J_e_xj = jacobian(fun, wrt(xj), at(meas_ij, xi, xj), e); +``` + +It produces Jacobians of the form, + +![J_e_x(+)omega][latex10] + +We thus then need to compute the Jacobian that will map to the tangent space - +often called local-parameterization. +A convenience function is provided in **manif** to do so as follow: + +```cpp +Eigen::MatrixXd J_xi_lp = autodiffLocalParameterizationJacobian(xi); +Eigen::MatrixXd J_xj_lp = autodiffLocalParameterizationJacobian(xj); +``` + +This function computes the ![x(+)omega][latex11] operation's +Jacobian evaluated for ![omega=0][latex13] thus providing the Jacobian, + +![J_x(+)w_w][latex14] + +Once both the cost function and local-parameterization's Jacobians are evaluated, +they can be compose as, + +![J_e_w = J_e_x(+)omega * J_x(+)w_w][latex15] + +Voila. + +The intermediate Jacobians (2-3) that some solver requires are **not** available in `manif` +since the library provides directly the final Jacobian `(1)`. + +However, **manif** is compliant with `autodiff::dual` +auto-differentiation type to compute (2-3). + +[//]: # (URLs) + +[jsola18]: http://arxiv.org/abs/1812.01537 + +[ceres]: http://ceres-solver.org/ +[ceres-costfunction]: http://ceres-solver.org/nnls_modeling.html#costfunction +[ceres-localparam]: http://ceres-solver.org/nnls_modeling.html#localparameterization +[ceres-jet]: http://ceres-solver.org/automatic_derivatives.html#dual-numbers-jets +[autodiff]: https://autodiff.github.io/ + +[latex1]: https://latex.codecogs.com/svg.latex?SO^3 +[latex2]: https://latex.codecogs.com/svg.latex?\bf&space;x +[latex3]: https://latex.codecogs.com/svg.latex?\omega +[latex4]: https://latex.codecogs.com/svg.latex?\bf&space;x +[latex5]: https://latex.codecogs.com/svg.latex?{\bf&space;e}=f({\bf&space;x}) +[latex6]: https://latex.codecogs.com/svg.latex?f({\bf&space;x\oplus\omega})\approx{\bf&space;e}+{\bf&space;J}_{\omega}^{e}~\omega&space;. +[latex7]: https://latex.codecogs.com/svg.latex?{\bf&space;J}_{\omega}^{e}=\frac{\delta{\bf&space;e}}{\delta{\bf&space;x}}=\frac{\delta&space;f({\bf&space;x})}{\delta{\bf&space;x}}=\lim_{\omega\to0}\frac{f({\bf&space;x}\oplus\omega)\ominus&space;f({\bf&space;x})}{\omega},&space;(1) +[latex8]: https://latex.codecogs.com/svg.latex?f({\bf&space;x}) +[latex9]: https://latex.codecogs.com/svg.latex?{\bf&space;e}=f({\bf&space;x}) +[latex10]: https://latex.codecogs.com/svg.latex?{\bf&space;J}_{{\bf&space;x}\oplus\omega}^{e}=\frac{\delta{\bf&space;e}}{\delta({\bf&space;x}\oplus\omega)}=\lim_{\mathbf&space;h\to0}\frac{&space;f({\bf&space;x}+\mathbf&space;h)-f({\bf&space;x})}{\mathbf&space;h}.&space;(2) +[latex11]: https://latex.codecogs.com/svg.latex?{\bf&space;x}\oplus\mathbf\omega +[latex12]: https://latex.codecogs.com/svg.latex?\mathbf\omega +[latex13]: https://latex.codecogs.com/svg.latex?\omega=0 +[latex14]: https://latex.codecogs.com/svg.latex?{\bf&space;J}_{\omega}^{{\bf&space;x}\oplus\omega}=\frac{\delta({\bf&space;x}\oplus\omega)}{\delta\omega}=\lim_{\delta\omega\to0}\frac{{\bf&space;x}\oplus(\omega+\delta\omega)-{\bf&space;x}\oplus\mathbf\omega}{\delta\omega}=\lim_{\delta\omega\to0}\frac{{\bf&space;x}\oplus\delta\omega-{\bf&space;x}}{\delta\omega}.&space;(3) +[latex15]: https://latex.codecogs.com/svg.latex?{\bf&space;J}_{\omega}^{e}={\bf&space;J}_{{\bf&space;x}\oplus\omega}^{e}\times{\bf&space;J}_{\omega}^{{\bf&space;x}\oplus\omega}.&space;(4) +[latex16]: https://latex.codecogs.com/svg.latex?{\bf&space;x}\oplus\mathbf\omega diff --git a/docs/pages/python/Quick-start.md b/docs/pages/python/Quick-start.md index d51d11e0..480a36a1 100644 --- a/docs/pages/python/Quick-start.md +++ b/docs/pages/python/Quick-start.md @@ -1,14 +1,29 @@ # Quick start - [Quick start](#quick-start) - - [Getting Pybind11](#getting-pybind11) - - [Installation](#installation) - - [Dependencies](#dependencies) + - [Installing manifpy](#installing-manifpy) + - [From conda](#from-conda) - [From source](#from-source) + - [Getting Pybind11](#getting-pybind11) + - [Getting the dependencies](#getting-the-dependencies) + - [Building](#building) + - [Testing](#testing) - [Use manifpy in your project](#use-manifpy-in-your-project) - [Tutorials and application demos](#tutorials-and-application-demos) -## Getting Pybind11 +## Installing manifpy + +### From conda + +`manifpy` can be installed from the [conda-forge][conda-manifpy], + +```bash +conda install -c conda-forge manifpy +``` + +### From source + +#### Getting Pybind11 The Python wrappers are generated using [pybind11][pybind11-rtd]. So first we need to install it, but we want it available directly in our environment root so that `CMake` can find it. @@ -30,9 +45,9 @@ cmake .. make install ``` -## Installation + -### Dependencies +#### Getting the dependencies - Eigen 3 : - Linux ( Ubuntu and similar ) @@ -49,13 +64,7 @@ make install - [lt::optional][optional-repo] : included in the `external` folder -Python bindings also depends on `numpy`. - -```bash -python3 -m pip install -r requirements -``` - -### From source +#### Building To generate `manif` Python bindings run, @@ -65,6 +74,20 @@ cd manif python3 -m pip install . ``` +#### Testing + +To run the tests you will also need `numpy`, + +```bash +python3 -m pip install numpy +``` + +To run the tests, simply hits: + +```bash +python3 -m pytest +``` + ## Use manifpy in your project ```python @@ -101,3 +124,4 @@ python3 se2_localization.py [pybind11-rtd]: https://pybind11.readthedocs.io/en/stable/index.html [optional-repo]: https://github.com/TartanLlama/optional +[conda-manifpy]: https://anaconda.org/conda-forge/manifpy diff --git a/docs/pages/python/index.rst b/docs/pages/python/index.rst index f1341957..96af4f4c 100644 --- a/docs/pages/python/index.rst +++ b/docs/pages/python/index.rst @@ -8,30 +8,6 @@ Quick start .. contents:: Table of Contents :depth: 3 - -Getting Pybind11 ----------------- - -The Python wrappers are generated using `pybind11 `_. So first we need to install it, -but we want it available directly in our environment root so that ``CMake`` can find it. -To do so we can use, - -.. code-block:: bash - - python3 -m pip install "pybind11[global]" - -Note that this is not recommended when using one's system Python, -as it will add files to ``/usr/local/include/pybind11`` and ``/usr/local/share/cmake/pybind11``. - -Another way is to use ``CMake`` to install it, - -.. code-block:: bash - - git clone https://github.com/pybind/pybind11.git - cd pybind11 && mkdir build && cd build - cmake .. - make install - Installation ------------ @@ -60,11 +36,6 @@ Dependencies * `lt::optional `_ : included in the ``external`` folder -Python bindings also depends on ``numpy``. - -.. code-block:: bash - - python3 -m pip install -r requirements From source ^^^^^^^^^^^ diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 4f0548d4..abcff5a9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -36,6 +36,18 @@ set(CXX_11_EXAMPLE_TARGETS se_2_3_localization ) +if(NOT MSVC) + add_executable(bundle_sam bundle_sam.cpp) + + set(CXX_11_EXAMPLE_TARGETS + + ${CXX_11_EXAMPLE_TARGETS} + + # Bundle + bundle_sam + ) +endif() + # Link to manif foreach(target ${CXX_11_EXAMPLE_TARGETS}) target_link_libraries(${target} ${PROJECT_NAME}) @@ -45,7 +57,7 @@ foreach(target ${CXX_11_EXAMPLE_TARGETS}) target_compile_options(${target} PRIVATE -Werror=all -Werror=extra - ) + ) endif() endforeach() diff --git a/examples/bundle_sam.cpp b/examples/bundle_sam.cpp new file mode 100644 index 00000000..06c63d41 --- /dev/null +++ b/examples/bundle_sam.cpp @@ -0,0 +1,386 @@ +/** + * \file bundle_sam.cpp + * + * Created on: Feb 28, 2021 + * \author: pettni + * + * Adopted from se2_sam.cpp, see that file for explanations. + */ + + +// manif +#include "manif/Rn.h" +#include "manif/SE2.h" +#include "manif/Bundle.h" + +// Std +#include +#include +#include +#include + +// Debug +#include +#include + +// std shortcuts and namespaces +using std::cout; +using std::endl; +using std::vector; +using std::map; +using std::list; +using std::pair; + +// Eigen namespace +using namespace Eigen; + +// manif namespace and shortcuts +using manif::SE2d; +using manif::SE2Tangentd; + +static constexpr int DoF = SE2d::DoF; +static constexpr int Dim = SE2d::Dim; + +// Define many data types (Tangent refers to the tangent of SE2) +typedef Array ArrayT; // tangent-size array +typedef Matrix VectorT; // tangent-size vector +typedef Matrix MatrixT; // tangent-size square matrix +typedef Matrix VectorB; // landmark-size vector +typedef Array ArrayY; // measurement-size array +typedef Matrix VectorY; // measurement-size vector +typedef Matrix MatrixY; // measurement-size square matrix +typedef Matrix MatrixYT; // measurement x tangent size matrix +typedef Matrix MatrixYB; // measurement x landmark size matrix + +// some experiment constants +static const int NUM_POSES = 3; +static const int NUM_LMKS = 5; +static const int NUM_FACTORS = 9; +static const int NUM_MEAS = NUM_POSES * DoF + NUM_FACTORS * Dim; +static const int MAX_ITER = 20; // for the solver + + +// bundle state type to optimize over +using BundleT = manif::Bundle; + +// Insert a relative pose factor from pose XI to pose XJ +// into the residual-jacobian pair (r, J) +template +void add_pose_factor( + const BundleT & X, + const SE2Tangentd & control, + const MatrixT & W, + Eigen::Ref> r, + Eigen::Ref> J) +{ + // index start position and length in the DoF of BundleT + const int BegI = std::get(manif::internal::traits::DoFIdx); + constexpr int LenI = BundleT::Element::DoF; + const int BegJ = std::get(manif::internal::traits::DoFIdx); + constexpr int LenJ = BundleT::Element::DoF; + + MatrixT J_d_xi, J_d_xj; // Jacobian of motion wrt poses i and j + + auto d = X.element().rminus(X.element(), J_d_xj, J_d_xi); + r = W * (d - control).coeffs(); + J.setZero(); + J.block<3, LenI>(0, BegI) = W * J_d_xi; + J.block<3, LenJ>(0, BegJ) = W * J_d_xj; +} + + +// Insert a landmark measurement factor of landmark LK +// from pose XI into the residual-jacobian pair (r, J) +template +void add_beacon_factor( + const BundleT & X, + const VectorY & measurement, + const MatrixY & S, + Eigen::Ref> r, + Eigen::Ref> J) +{ + // index start position and length in the DoF of BundleT + const int BegX = std::get(manif::internal::traits::DoFIdx); + constexpr int LenX = BundleT::Element::DoF; + const int BegLMK = std::get(manif::internal::traits::DoFIdx); + constexpr int LenLMK = BundleT::Element::DoF; + + MatrixT J_ix_x; // Jacobian of inverse pose wrt pose + MatrixYT J_e_ix; // Jacobian of measurement expectation wrt inverse pose + MatrixYT J_e_x; // Jacobian of measurement expectation wrt pose + MatrixYB J_e_b; // Jacobian of measurement expectation wrt lmk + + auto e = X.element().inverse(J_ix_x).act(X.element().coeffs(), J_e_ix, J_e_b); + J_e_x = J_e_ix * J_ix_x; + r = S * (e - measurement); + J.setZero(); + J.block(0, BegX) = S * J_e_x; + J.block(0, BegLMK) = S * J_e_b; +} + + +int main() +{ + std::srand((unsigned int) time(0)); + + // DEBUG INFO + cout << endl; + cout << "2D Smoothing and Mapping. 3 poses, 5 landmarks." << endl; + cout << "-----------------------------------------------" << endl; + cout << std::fixed << std::setprecision(3) << std::showpos; + + // START CONFIGURATION + // + // + + // Define the robot pose elements + SE2d X_simu, // pose of the simulated robot + Xi, // robot pose at time i + Xj; // robot pose at time j + vector poses, // estimator poses + poses_simu;// simulator poses + Xi.setIdentity(); + X_simu.setIdentity(); + + + // Define a control vector and its noise and covariance in the tangent of SE2 + SE2Tangentd u; // control signal, generic + SE2Tangentd u_nom; // nominal control signal + ArrayT u_sigmas; // control noise std specification + VectorT u_noise; // control noise + // MatrixT Q; // Covariance + MatrixT W; // sqrt Info + vector controls; // robot controls + + u_nom << 0.1, 0.0, 0.05; + u_sigmas << 0.01, 0.01, 0.01; + // Q = (u_sigmas * u_sigmas).matrix().asDiagonal(); + W = u_sigmas.inverse() .matrix().asDiagonal(); // this is Q^(-T/2) + + // Landmarks in R^2 and map + VectorB b; // Landmark, generic + vector landmarks(NUM_LMKS), landmarks_simu; + { + // Define five landmarks (beacons) in R^2 + VectorB b0, b1, b2, b3, b4; + b0 << 3.0, 0.0; + b1 << 2.0, -1.0; + b2 << 2.0, 1.0; + b3 << 3.0, -1.0; + b4 << 3.0, 1.0; + landmarks_simu.push_back(b0); + landmarks_simu.push_back(b1); + landmarks_simu.push_back(b2); + landmarks_simu.push_back(b3); + landmarks_simu.push_back(b4); + } // destroy b0...b4 + + // Define the beacon's measurements in R^2 + VectorY y, y_noise; + ArrayY y_sigmas; + // MatrixY R; // Covariance + MatrixY S; // sqrt Info + vector> measurements(NUM_POSES); // y = measurements[pose_id][lmk_id] + + y_sigmas << 0.001, 0.001; + // R = (y_sigmas * y_sigmas).matrix().asDiagonal(); + S = y_sigmas.inverse() .matrix().asDiagonal(); // this is R^(-T/2) + + // Problem-size variables + + /* + * The factor graph of the SAM problem looks like this: + * + * ------- b1 + * b3 / | + * | / b4 | + * | / / \| + * X0 ---- X1 ---- X2 + * | \ / \ / + * | b0 b2 + * * + * + * where: + * - Xi are poses + * - bk are landmarks or beacons + * - * is a pose prior to anchor the map and make the problem observable + * + * Define pairs of nodes for all the landmark measurements + * There are 3 pose nodes [0..2] and 5 landmarks [0..4]. + * A pair pose -- lmk means that the lmk was measured from the pose + * Each pair declares a factor in the factor graph + * We declare 9 pairs, or 9 factors, as follows: + */ + vector> pairs(NUM_POSES); + pairs[0].push_back(0); // 0-0 + pairs[0].push_back(1); // 0-1 + pairs[0].push_back(3); // 0-3 + pairs[1].push_back(0); // 1-0 + pairs[1].push_back(2); // 1-2 + pairs[1].push_back(4); // 1-4 + pairs[2].push_back(1); // 2-1 + pairs[2].push_back(2); // 2-2 + pairs[2].push_back(4); // 2-4 + + // + // + // END CONFIGURATION + + + //// Simulator ################################################################### + poses_simu. push_back(X_simu); + poses. push_back(Xi + SE2Tangentd::Random()); // use very noisy priors + + // temporal loop + for (int i = 0; i < NUM_POSES; ++i) + { + // make measurements + for (const auto& k : pairs[i]) + { + // simulate measurement + b = landmarks_simu[k]; // lmk coordinates in world frame + y_noise = y_sigmas * ArrayY::Random(); // measurement noise + y = X_simu.inverse().act(b); // landmark measurement, before adding noise + + // add noise and compute prior lmk from prior pose + measurements[i][k] = y + y_noise; // store noisy measurements + b = Xi.act(y + y_noise); // mapped landmark with noise + landmarks[k] = b + VectorB::Random(); // use very noisy priors + } + + // make motions + if (i < NUM_POSES - 1) // do not make the last motion since we're done after 3rd pose + { + // move simulator, without noise + X_simu = X_simu + u_nom; + + // move prior, with noise + u_noise = u_sigmas * ArrayT::Random(); + Xi = Xi + (u_nom + u_noise); + + // store + poses_simu. push_back(X_simu); + poses. push_back(Xi + SE2Tangentd::Random()); // use very noisy priors + controls. push_back(u_nom + u_noise); + } + } + + //// Estimator ################################################################# + + // Insert priors into bundle state + BundleT X(poses[0], poses[1], poses[2], + manif::R2d(landmarks[0]), manif::R2d(landmarks[1]), + manif::R2d(landmarks[2]), manif::R2d(landmarks[3]), + manif::R2d(landmarks[4])); + + cout << "prior" << std::showpos << endl; + cout << "pose :" << X.element<0>().translation().transpose() << " " << X.element<0>().angle() << endl; + cout << "pose :" << X.element<1>().translation().transpose() << " " << X.element<1>().angle() << endl; + cout << "pose :" << X.element<2>().translation().transpose() << " " << X.element<2>().angle() << endl; + + cout << "lmk :" << X.element<3>() << endl; + cout << "lmk :" << X.element<4>() << endl; + cout << "lmk :" << X.element<5>() << endl; + cout << "lmk :" << X.element<6>() << endl; + cout << "lmk :" << X.element<7>() << endl; + cout << "-----------------------------------------------" << endl; + + // iterate + // DEBUG INFO + cout << "iterations" << std::noshowpos << endl; + for (int iteration = 0; iteration < MAX_ITER; ++iteration) + { + Matrix J; // full Jacobian + Matrix r; // full residual + + int row = 0; // keep track of row in J and r + + // first residual: prior + r.segment(row) = X.element<0>().lminus(SE2d::Identity(), J.block(row, 0)).coeffs(); + row += DoF; + + // motion residuals + add_pose_factor<0, 1>(X, controls[0], W, r.segment(row), J.block(row, 0)); + row += DoF; + + add_pose_factor<1, 2>(X, controls[1], W, r.segment(row), J.block(row, 0)); + row += DoF; + + // measurement residuals + add_beacon_factor<0, 0>(X, measurements[0][0], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<0, 1>(X, measurements[0][1], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<0, 3>(X, measurements[0][3], S, r.segment(row), J.block(row, 0)); + row += Dim; + + add_beacon_factor<1, 0>(X, measurements[1][0], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<1, 2>(X, measurements[1][2], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<1, 4>(X, measurements[1][4], S, r.segment(row), J.block(row, 0)); + row += Dim; + + add_beacon_factor<2, 1>(X, measurements[2][1], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<2, 2>(X, measurements[2][2], S, r.segment(row), J.block(row, 0)); + row += Dim; + add_beacon_factor<2, 4>(X, measurements[2][4], S, r.segment(row), J.block(row, 0)); + row += Dim; + + // 4. Solve ----------------------------------------------------------------- + + // compute optimal step + // ATTENTION: This is an expensive step!! + // ATTENTION: Use QR factorization and column reordering for larger problems!! + const auto dX = (-(J.transpose() * J).inverse() * J.transpose() * r).eval(); + + // update estimate + X += BundleT::Tangent(dX); + + // DEBUG INFO + cout << "residual norm: " << std::scientific << r.norm() << ", step norm: " << dX.norm() << endl; + + // conditional exit + if (dX.norm() < 1e-6) break; + } + cout << "-----------------------------------------------" << endl; + + + //// Print results #################################################################### + + cout << std::fixed; + + // solved problem + cout << "posterior" << std::showpos << endl; + cout << "pose :" << X.element<0>().translation().transpose() << " " << X.element<0>().angle() << endl; + cout << "pose :" << X.element<1>().translation().transpose() << " " << X.element<1>().angle() << endl; + cout << "pose :" << X.element<2>().translation().transpose() << " " << X.element<2>().angle() << endl; + + cout << "lmk :" << X.element<3>() << endl; + cout << "lmk :" << X.element<4>() << endl; + cout << "lmk :" << X.element<5>() << endl; + cout << "lmk :" << X.element<6>() << endl; + cout << "lmk :" << X.element<7>() << endl; + + cout << "-----------------------------------------------" << endl; + + // ground truth + cout << "ground truth" << std::showpos << endl; + for (const auto& ps : poses_simu) + cout << "pose : " << ps.translation().transpose() << " " << ps.angle() << endl; + for (const auto& ls : landmarks_simu) + cout << "lmk : " << ls.transpose() << endl; + cout << "-----------------------------------------------" << endl; + + return 0; +} diff --git a/examples/se2_localization.py b/examples/se2_localization.py index 873688f8..7332bc92 100644 --- a/examples/se2_localization.py +++ b/examples/se2_localization.py @@ -202,7 +202,7 @@ def Jacobian(): X = X.plus(u_est, J_x, J_u) # X * exp(u), with Jacobians - P = J_x * P * J_x.transpose() + J_u * U * J_u.transpose() + P = J_x @ P @ J_x.transpose() + J_u @ U @ J_u.transpose() # Then we correct using the measurements of each lmk for i in range(NUMBER_OF_LMKS_TO_MEASURE): diff --git a/include/manif/Bundle.h b/include/manif/Bundle.h new file mode 100644 index 00000000..8cf7873c --- /dev/null +++ b/include/manif/Bundle.h @@ -0,0 +1,17 @@ +#ifndef _MANIF_BUNDLE_H_ +#define _MANIF_BUNDLE_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/utils.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/bundle/Bundle_properties.h" +#include "manif/impl/bundle/Bundle_base.h" +#include "manif/impl/bundle/Bundle_map.h" +#include "manif/impl/bundle/Bundle.h" +#include "manif/impl/bundle/BundleTangent_base.h" +#include "manif/impl/bundle/BundleTangent.h" +#include "manif/impl/bundle/BundleTangent_map.h" + +#endif // _MANIF_BUNDLE_H_ diff --git a/include/manif/SGal3.h b/include/manif/SGal3.h new file mode 100644 index 00000000..895e29ea --- /dev/null +++ b/include/manif/SGal3.h @@ -0,0 +1,16 @@ +#ifndef _MANIF_SGAL3_H_ +#define _MANIF_SGAL3_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/sgal3/SGal3_properties.h" +#include "manif/impl/sgal3/SGal3_base.h" +#include "manif/impl/sgal3/SGal3Tangent_base.h" +#include "manif/impl/sgal3/SGal3.h" +#include "manif/impl/sgal3/SGal3Tangent.h" +#include "manif/impl/sgal3/SGal3_map.h" +#include "manif/impl/sgal3/SGal3Tangent_map.h" + +#endif // _MANIF_SGAL3_H_ diff --git a/include/manif/autodiff/autodiff.h b/include/manif/autodiff/autodiff.h new file mode 100644 index 00000000..e04bb4e0 --- /dev/null +++ b/include/manif/autodiff/autodiff.h @@ -0,0 +1,54 @@ +#ifndef _MANIF_MANIF_AUTODIFF_AUTODIFF_H_ +#define _MANIF_MANIF_AUTODIFF_AUTODIFF_H_ + +#include "manif/autodiff/constants.h" +#include "manif/autodiff/local_parameterization.h" + +namespace manif::internal { +// @note: Unfortunately HigherOrderDual is a non-deducible context +// template +// struct is_ad> : std::integral_constant { }; + +using dual0thf = autodiff::HigherOrderDual<0, float>; +using dual1stf = autodiff::HigherOrderDual<1, float>; +using dual2ndf = autodiff::HigherOrderDual<2, float>; +using dual3rdf = autodiff::HigherOrderDual<3, float>; +using dual4thf = autodiff::HigherOrderDual<4, float>; + +template <> struct is_ad : std::integral_constant { }; +template <> struct is_ad : std::integral_constant { }; +template <> struct is_ad : std::integral_constant { }; +template <> struct is_ad : std::integral_constant { }; +template <> struct is_ad : std::integral_constant { }; + +template <> struct is_ad : std::integral_constant { }; +template <> struct is_ad : std::integral_constant { }; +template <> struct is_ad : std::integral_constant { }; +template <> struct is_ad : std::integral_constant { }; +template <> struct is_ad : std::integral_constant { }; + +template +struct is_ad> : std::integral_constant { }; +} // namespace manif + +namespace autodiff::detail { +/// @brief VectorTraits specialization for Derived of LieGroupBase +template +struct VectorTraits::value>> { + using ValueType = typename T::Scalar; + + template + using ReplaceValueType = typename T::template LieGroupTemplate; +}; + +/// @brief VectorTraits specialization for Derived of TangentBase +template +struct VectorTraits::value>> { + using ValueType = typename T::Scalar; + + template + using ReplaceValueType = typename T::template TangentTemplate; +}; +} // namespace autodiff::detail + +#endif // _MANIF_MANIF_AUTODIFF_AUTODIFF_H_ \ No newline at end of file diff --git a/include/manif/autodiff/constants.h b/include/manif/autodiff/constants.h new file mode 100644 index 00000000..7b5a44df --- /dev/null +++ b/include/manif/autodiff/constants.h @@ -0,0 +1,42 @@ +#ifndef _MANIF_MANIF_AUTODIFF_CONSTANTS_H_ +#define _MANIF_MANIF_AUTODIFF_CONSTANTS_H_ + +namespace { + // size_t without includes. + using size_type = decltype(alignof(char)); +} + +namespace autodiff { +namespace detail { + template struct Dual; + template class Real; +} // namespace detail +} // namespace autodiff + +namespace manif { + +/// @brief Specialize Constants traits for autodiff::Dual type +template +struct Constants> { + static const autodiff::detail::Dual eps; +}; + +template +const autodiff::detail::Dual +Constants>::eps = + autodiff::detail::Dual(Constants::eps); + +/// @brief Specialize Constants traits for autodiff::Real type +template +struct Constants> { + static const autodiff::detail::Real eps; +}; + +template +const autodiff::detail::Real +Constants>::eps = + autodiff::detail::Real(Constants::eps); + +} // namespace manif + +#endif // _MANIF_MANIF_AUTODIFF_CONSTANTS_H_ diff --git a/include/manif/autodiff/local_parameterization.h b/include/manif/autodiff/local_parameterization.h new file mode 100644 index 00000000..017d66bc --- /dev/null +++ b/include/manif/autodiff/local_parameterization.h @@ -0,0 +1,37 @@ +#ifndef _MANIF_MANIF_AUTODIFF_LOCAL_PARAMETRIZATION_H_ +#define _MANIF_MANIF_AUTODIFF_LOCAL_PARAMETRIZATION_H_ + +namespace manif { + +template +Eigen::Matrix +autodiffLocalParameterizationJacobian(const manif::LieGroupBase& _state) { + + using Scalar = typename Derived::Scalar; + using LieGroup = typename Derived::template LieGroupTemplate; + using Tangent = typename Derived::Tangent::template TangentTemplate; + + using Jac = Eigen::Matrix; + + LieGroup state = _state.template cast(); + Tangent delta = Tangent::Zero(); + + LieGroup state_plus_delta; + + auto f = [](const auto& s, const auto& t){ + return s + t; + }; + + Jac J_so_t = autodiff::jacobian( + f, autodiff::wrt(delta), autodiff::at(state, delta), state_plus_delta + ); + + MANIF_ASSERT(state.isApprox(state_plus_delta)); + MANIF_ASSERT(Derived::RepSize == J_so_t.rows()); + MANIF_ASSERT(Derived::DoF == J_so_t.cols()); + + return J_so_t; +} + +} // namespace manif +#endif // _MANIF_MANIF_AUTODIFF_LOCAL_PARAMETRIZATION_H_ \ No newline at end of file diff --git a/include/manif/ceres/ceres.h b/include/manif/ceres/ceres.h index 285ab5f2..843d025b 100644 --- a/include/manif/ceres/ceres.h +++ b/include/manif/ceres/ceres.h @@ -3,7 +3,11 @@ #include "manif/ceres/constants.h" #include "manif/ceres/constraint.h" +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 +#include "manif/ceres/manifold.h" +#else #include "manif/ceres/local_parametrization.h" +#endif #include "manif/ceres/objective.h" #include "manif/ceres/ceres_utils.h" @@ -18,41 +22,57 @@ struct is_ad> : std::integral_constant { }; #ifdef _MANIF_MANIF_SO2_H_ using CeresConstraintSO2 = CeresConstraintFunctor; +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 +using CeresManifoldSO2 = CeresManifoldFunctor; +#else using CeresLocalParameterizationSO2 = CeresLocalParameterizationFunctor; +#endif using CeresObjectiveSO2 = CeresObjectiveFunctor; #else using CeresConstraintSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; -using CeresLocalParameterizationSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; +using CeresManifoldSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; using CeresObjectiveSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; #endif #ifdef _MANIF_MANIF_SO3_H_ using CeresConstraintSO3 = CeresConstraintFunctor; +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 +using CeresManifoldSO3 = CeresManifoldFunctor; +#else using CeresLocalParameterizationSO3 = CeresLocalParameterizationFunctor; +#endif using CeresObjectiveSO3 = CeresObjectiveFunctor; #else using CeresConstraintSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; -using CeresLocalParameterizationSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; +using CeresManifoldSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; using CeresObjectiveSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; #endif #ifdef _MANIF_MANIF_SE2_H_ using CeresConstraintSE2 = CeresConstraintFunctor; +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 +using CeresManifoldSE2 = CeresManifoldFunctor; +#else using CeresLocalParameterizationSE2 = CeresLocalParameterizationFunctor; +#endif using CeresObjectiveSE2 = CeresObjectiveFunctor; #else using CeresConstraintSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; -using CeresLocalParameterizationSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; +using CeresManifoldSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; using CeresObjectiveSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; #endif #ifdef _MANIF_MANIF_SE3_H_ using CeresConstraintSE3 = CeresConstraintFunctor; +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 +using CeresManifoldSE3 = CeresManifoldFunctor; +#else using CeresLocalParameterizationSE3 = CeresLocalParameterizationFunctor; +#endif using CeresObjectiveSE3 = CeresObjectiveFunctor; #else using CeresConstraintSE3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; -using CeresLocalParameterizationSE3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; +using CeresManifoldSE3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; using CeresObjectiveSE3= internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS; #endif diff --git a/include/manif/ceres/ceres_utils.h b/include/manif/ceres/ceres_utils.h index 65d379d2..44a4e830 100644 --- a/include/manif/ceres/ceres_utils.h +++ b/include/manif/ceres/ceres_utils.h @@ -1,15 +1,39 @@ #ifndef _MANIF_MANIF_CERES_UTILS_H_ #define _MANIF_MANIF_CERES_UTILS_H_ +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 +#include "manif/ceres/manifold.h" +#else #include "manif/ceres/local_parametrization.h" +#endif #include "manif/ceres/objective.h" #include "manif/ceres/constraint.h" +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 +#include +#else #include +#endif #include namespace manif { +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 +/** + * @brief Helper function to create a Ceres Manifold parameterization wrapper. + * @see CeresManifoldFunctor + */ +template +std::shared_ptr< + ceres::AutoDiffManifold, + _LieGroup::RepSize, _LieGroup::DoF>> +make_manifold_autodiff() +{ + return std::make_shared< + ceres::AutoDiffManifold< + CeresManifoldFunctor<_LieGroup>, _LieGroup::RepSize, _LieGroup::DoF>>(); +} +#else /** * @brief Helper function to create a Ceres autodiff local parameterization wrapper. * @see CeresLocalParameterizationFunctor @@ -24,6 +48,7 @@ make_local_parameterization_autodiff() ceres::AutoDiffLocalParameterization< CeresLocalParameterizationFunctor<_LieGroup>, _LieGroup::RepSize, _LieGroup::DoF>>(); } +#endif /** * @brief Helper function to create a Ceres autodiff objective wrapper. diff --git a/include/manif/ceres/constants.h b/include/manif/ceres/constants.h index 4dc42da9..0353314b 100644 --- a/include/manif/ceres/constants.h +++ b/include/manif/ceres/constants.h @@ -4,6 +4,7 @@ #include "manif/constants.h" #include +#include namespace manif { @@ -17,7 +18,8 @@ struct Constants> template const ceres::Jet<_Scalar, N> -Constants>::eps = ceres::Jet<_Scalar, N>(1e-14); +Constants>::eps = + ceres::Jet<_Scalar, N>(Constants<_Scalar>::eps); } /* namespace manif */ diff --git a/include/manif/ceres/manifold.h b/include/manif/ceres/manifold.h new file mode 100644 index 00000000..43fc60c7 --- /dev/null +++ b/include/manif/ceres/manifold.h @@ -0,0 +1,61 @@ +#ifndef _MANIF_MANIF_CERES_MANIFOLD_H_ +#define _MANIF_MANIF_CERES_MANIFOLD_H_ + +#include + +namespace manif { + +/** + * @brief A wrapper for Ceres autodiff local parameterization. + */ +template +class CeresManifoldFunctor +{ + using LieGroup = _LieGroup; + using Tangent = typename _LieGroup::Tangent; + + template + using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>; + + template + using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>; + +public: + + CeresManifoldFunctor() = default; + virtual ~CeresManifoldFunctor() = default; + + template + bool Plus(const T* state_raw, + const T* delta_raw, + T* state_plus_delta_raw) const + { + const Eigen::Map> state(state_raw); + const Eigen::Map> delta(delta_raw); + + Eigen::Map> state_plus_delta(state_plus_delta_raw); + + state_plus_delta = state + delta; + + return true; + } + + template + bool Minus(const T* y_raw, + const T* x_raw, + T* y_minus_x_raw) const + { + const Eigen::Map> y(y_raw); + const Eigen::Map> x(x_raw); + + Eigen::Map> y_minus_x(y_minus_x_raw); + + y_minus_x = y - x; + + return true; + } +}; + +} /* namespace manif */ + +#endif /* _MANIF_MANIF_CERES_MANIFOLD_H_ */ diff --git a/include/manif/constants.h b/include/manif/constants.h index 6381a68c..b2dab76c 100644 --- a/include/manif/constants.h +++ b/include/manif/constants.h @@ -47,7 +47,7 @@ T constexpr csqrt(T x) template struct Constants { - static constexpr _Scalar eps = _Scalar(1e-14); + static constexpr _Scalar eps = std::numeric_limits<_Scalar>::epsilon()*_Scalar(100); static constexpr _Scalar eps_sqrt = internal::csqrt(eps); static constexpr _Scalar to_rad = _Scalar(MANIF_PI / 180.0); @@ -63,16 +63,6 @@ constexpr _Scalar Constants<_Scalar>::to_rad; template constexpr _Scalar Constants<_Scalar>::to_deg; -template <> -struct Constants -{ - static constexpr float eps = float(1e-6); - static constexpr float eps_sqrt = internal::csqrt(eps); - - static constexpr float to_rad = float(MANIF_PI / 180.0); - static constexpr float to_deg = float(180.0 / MANIF_PI); -}; - } /* namespace manif */ #endif /* _MANIF_MANIF_CONSTANTS_H_ */ diff --git a/include/manif/impl/bracket.h b/include/manif/impl/bracket.h new file mode 100644 index 00000000..ddc6b0f2 --- /dev/null +++ b/include/manif/impl/bracket.h @@ -0,0 +1,35 @@ +#ifndef _MANIF_MANIF_IMPL_BRACKET_H_ +#define _MANIF_MANIF_IMPL_BRACKET_H_ + +namespace manif { +namespace internal { + +template +struct BracketEvaluatorImpl { + template + static typename Derived::Tangent run(const TL& a, const TR& b) { + return a.smallAdj() * b; + } +}; + +template +struct BracketEvaluator : BracketEvaluatorImpl { + using Base = BracketEvaluatorImpl; + + BracketEvaluator(const Derived& xptr, const DerivedOther& xptr_o) + : xptr_(xptr), xptr_o_(xptr_o) {} + + typename Derived::Tangent run() { + return Base::run(xptr_, xptr_o_); + } + +protected: + + const Derived& xptr_; + const DerivedOther& xptr_o_; +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_IMPL_BRACKET_H_ diff --git a/include/manif/impl/bundle/Bundle.h b/include/manif/impl/bundle/Bundle.h new file mode 100644 index 00000000..a7c4ae22 --- /dev/null +++ b/include/manif/impl/bundle/Bundle.h @@ -0,0 +1,208 @@ +#ifndef _MANIF_MANIF_BUNDLE_H_ +#define _MANIF_MANIF_BUNDLE_H_ + +#include "manif/impl/bundle/Bundle_base.h" +#include "manif/impl/traits.h" + +namespace manif { + +// Forward declare for type traits specialization + +template class ... _T> struct Bundle; +template class ... _T> struct BundleTangent; + +namespace internal { + +//! Traits specialization +template class ... _T> +struct traits> +{ + // Bundle-specific traits + static constexpr std::size_t BundleSize = sizeof...(_T); + + using Elements = std::tuple<_T<_Scalar>...>; + + template + using Element = typename std::tuple_element<_N, Elements>::type; + + template + using MapElement = Eigen::Map>; + + template + using MapConstElement = Eigen::Map>; + + static constexpr std::array DimIdx = compute_indices<_T<_Scalar>::Dim ...>(); + static constexpr std::array DoFIdx = compute_indices<_T<_Scalar>::DoF ...>(); + static constexpr std::array RepSizeIdx = compute_indices<_T<_Scalar>::RepSize ...>(); + static constexpr std::array TraIdx = compute_indices<_T<_Scalar>::Transformation::RowsAtCompileTime ...>(); + + // Regular traits + using Scalar = _Scalar; + + using LieGroup = Bundle<_Scalar, _T ...>; + using Tangent = BundleTangent<_Scalar, _T ...>; + + using Base = BundleBase>; + + static constexpr int Dim = accumulate(int(_T<_Scalar>::Dim) ...); + static constexpr int DoF = accumulate(int(_T<_Scalar>::DoF) ...); + static constexpr int RepSize = accumulate(int(_T<_Scalar>::RepSize) ...); + + using DataType = Eigen::Matrix<_Scalar, RepSize, 1>; + using Jacobian = Eigen::Matrix<_Scalar, DoF, DoF>; + using Transformation = SquareMatrix< + _Scalar, + accumulate(int(_T<_Scalar>::Transformation::RowsAtCompileTime) ...) + >; + using Vector = Eigen::Matrix<_Scalar, Dim, 1>; +}; + +template class ... _T> +const constexpr std::array traits>::DimIdx; +template class ... _T> +const constexpr std::array traits>::DoFIdx; +template class ... _T> +const constexpr std::array traits>::RepSizeIdx; +template class ... _T> +const constexpr std::array traits>::TraIdx; +template class ... _T> +const constexpr int traits>::Dim; +template class ... _T> +const constexpr int traits>::DoF; +template class ... _T> +const constexpr int traits>::RepSize; + +} // namespace internal + +// +// Bundle LieGroup +// + +/** + * @brief Represents a Bundle (or Composite) element as + * described in Section IV of the reference paper + * (see also Example 7). + * + * A Bundle of Lie groups can be utilized as + * a single group with element-wise operations. This can be + * convenient when working with aggregate states that consist of + * multiple Lie group sub-states, like the example in Section VIIb + * of the reference paper. + * + * Example: create an element of the composite + * using double as the scalar type. + * + * > Bundle element; + */ +template class ... _T> +struct Bundle : BundleBase> +{ +private: + + static_assert(sizeof...(_T) > 0, "Must have at least one element in Bundle !"); + + using Base = BundleBase>; + using Type = Bundle<_Scalar, _T...>; + +protected: + + using Base::derived; + +public: + + template using Element = typename Base::template Element; + using Base::BundleSize; + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + Bundle() = default; + ~Bundle() = default; + + MANIF_COPY_CONSTRUCTOR(Bundle) + MANIF_MOVE_CONSTRUCTOR(Bundle) + + // Copy constructor + template + Bundle(const LieGroupBase<_DerivedOther> & o); + + MANIF_GROUP_ASSIGN_OP(Bundle) + + // LieGroup common API + + /** + * @brief Get a reference to the underlying DataType. + * @param[out] a reference to the underlying Eigen vector + */ + DataType & coeffs(); + + /** + * @brief Get a const reference to the underlying DataType. + * @param[out] a const reference to the underlying Eigen vector + */ + const DataType & coeffs() const; + + + // Bundle specific API + + /** + * @brief Construct from Bundle elements + */ + Bundle(const _T<_Scalar> & ... elements); + +protected: + + // Helper for the elements constructor + template + Bundle(internal::intseq<_Idx...>, const _T<_Scalar> & ... elements); + +protected: + + //! Underlying data (Eigen) vector + DataType data_; +}; + + +template class ... _T> +template +Bundle<_Scalar, _T...>::Bundle(const LieGroupBase<_DerivedOther> & o) +: Bundle(o.coeffs()) +{} + +template class ... _T> +Bundle<_Scalar, _T...>::Bundle(const _T<_Scalar> & ... elements) +: Bundle(internal::make_intseq_t{}, elements ...) +{} + +template class ... _T> +template +Bundle<_Scalar, _T...>::Bundle( + internal::intseq<_Idx...>, const _T<_Scalar> & ... elements +) +{ + // c++11 "fold expression" + auto l = {((data_.template segment::RepSize>( + std::get<_Idx>(internal::traits::RepSizeIdx) + ) = elements.coeffs()), 0) ...}; + static_cast(l); // compiler warning +} + +template class ... _T> +typename Bundle<_Scalar, _T...>::DataType & +Bundle<_Scalar, _T...>::coeffs() +{ + return data_; +} + +template class ... _T> +const typename Bundle<_Scalar, _T...>::DataType & +Bundle<_Scalar, _T...>::coeffs() const +{ + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLE_H_ diff --git a/include/manif/impl/bundle/BundleTangent.h b/include/manif/impl/bundle/BundleTangent.h new file mode 100644 index 00000000..239a5b6f --- /dev/null +++ b/include/manif/impl/bundle/BundleTangent.h @@ -0,0 +1,191 @@ +#ifndef _MANIF_MANIF_BUNDLETANGENT_H_ +#define _MANIF_MANIF_BUNDLETANGENT_H_ + +#include "manif/impl/bundle/BundleTangent_base.h" +#include "manif/impl/traits.h" + +namespace manif { + +// Forward declare for type traits specialization + +template class ... _T> struct Bundle; +template class ... _T> struct BundleTangent; + +namespace internal { + +//! Traits specialization +template class ... _T> +struct traits> +{ + // BundleTangent-specific traits + static constexpr std::size_t BundleSize = sizeof...(_T); + + using Elements = std::tuple::Tangent...>; + + template + using Element = typename std::tuple_element<_N, Elements>::type; + + template + using MapElement = Eigen::Map>; + + template + using MapConstElement = Eigen::Map>; + + static constexpr std::array DoFIdx = compute_indices<_T<_Scalar>::Tangent::DoF ...>(); + static constexpr std::array RepSizeIdx = compute_indices<_T<_Scalar>::Tangent::RepSize ...>(); + static constexpr std::array AlgIdx = compute_indices<_T<_Scalar>::Tangent::LieAlg::RowsAtCompileTime ...>(); + + // Regular traits + using Scalar = _Scalar; + + using LieGroup = Bundle<_Scalar, _T...>; + using Tangent = BundleTangent<_Scalar, _T...>; + + using Base = BundleTangentBase; + + static constexpr int Dim = accumulate(int(_T<_Scalar>::Tangent::Dim) ...); + static constexpr int DoF = accumulate(int(_T<_Scalar>::Tangent::DoF) ...); + static constexpr int RepSize = accumulate(int(_T<_Scalar>::Tangent::RepSize) ...); + + using DataType = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + using LieAlg = SquareMatrix< + Scalar, + accumulate(int(_T<_Scalar>::Tangent::LieAlg::RowsAtCompileTime) ...) + >; +}; + +template class ... _T> +const constexpr std::array traits>::DoFIdx; +template class ... _T> +const constexpr std::array traits>::RepSizeIdx; +template class ... _T> +const constexpr std::array traits>::AlgIdx; +template class ... _T> +const constexpr int traits>::Dim; +template class ... _T> +const constexpr int traits>::DoF; +template class ... _T> +const constexpr int traits>::RepSize; + +} // namespace internal + +// +// BundleTangent +// + +/** + * @brief Represents a BundleTangent element. + */ +template class ... _T> +struct BundleTangent : BundleTangentBase> +{ +private: + + static_assert(sizeof...(_T) > 0, "Must have at least one element in BundleTangent !"); + + using Base = BundleTangentBase>; + using Type = BundleTangent<_Scalar, _T...>; + +protected: + + using Base::derived; + +public: + + template using Element = typename Base::template Element; + using Base::BundleSize; + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + BundleTangent() = default; + ~BundleTangent() = default; + + MANIF_COPY_CONSTRUCTOR(BundleTangent) + MANIF_MOVE_CONSTRUCTOR(BundleTangent) + + // Copy constructors given base + template + BundleTangent(const TangentBase<_DerivedOther> & o); + + MANIF_TANGENT_ASSIGN_OP(BundleTangent) + + // Tangent common API + + /** + * @brief Get a reference to the underlying DataType. + */ + DataType & coeffs(); + + /** + * @brief Get a const reference to the underlying DataType. + */ + const DataType & coeffs() const; + + + // BundleTangent specific API + + /** + * @brief Construct from BundleTangent elements + */ + BundleTangent(const typename _T<_Scalar>::Tangent & ... elements); + +protected: + + // Helper for the elements constructor + template + BundleTangent( + internal::intseq<_Idx...>, + const typename _T<_Scalar>::Tangent & ... elements + ); + +protected: + + DataType data_; +}; + +template class ... _T> +template +BundleTangent<_Scalar, _T...>::BundleTangent(const TangentBase<_DerivedOther> & o) +: data_(o.coeffs()) +{} + +template class ... _T> +BundleTangent<_Scalar, _T...>::BundleTangent(const typename _T<_Scalar>::Tangent & ... elements) +: BundleTangent(internal::make_intseq_t{}, elements ...) +{} + +template class ... _T> +template +BundleTangent<_Scalar, _T...>::BundleTangent( + internal::intseq<_Idx...>, + const typename _T<_Scalar>::Tangent & ... elements +) { + // c++11 "fold expression" + auto l = {((data_.template segment::RepSize>( + std::get<_Idx>(internal::traits::RepSizeIdx) + ) = elements.coeffs()), 0) ...}; + static_cast(l); // compiler warning +} + +template class ... _T> +typename BundleTangent<_Scalar, _T...>::DataType & +BundleTangent<_Scalar, _T...>::coeffs() +{ + return data_; +} + +template class ... _T> +const typename BundleTangent<_Scalar, _T...>::DataType & +BundleTangent<_Scalar, _T...>::coeffs() const +{ + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLETANGENT_H_ diff --git a/include/manif/impl/bundle/BundleTangent_base.h b/include/manif/impl/bundle/BundleTangent_base.h new file mode 100644 index 00000000..0cb5d9c0 --- /dev/null +++ b/include/manif/impl/bundle/BundleTangent_base.h @@ -0,0 +1,439 @@ +#ifndef _MANIF_MANIF_BUNDLETANGENT_BASE_H_ +#define _MANIF_MANIF_BUNDLETANGENT_BASE_H_ + +#include "manif/impl/tangent_base.h" +#include "manif/impl/traits.h" + +namespace manif { + +/** + * @brief The base class of the Bundle tangent. + */ +template +struct BundleTangentBase : TangentBase<_Derived> +{ +private: + + using Base = TangentBase<_Derived>; + using Type = BundleTangentBase<_Derived>; + +public: + + /** + * @brief Number of elements in the BundleTangent + */ + static constexpr std::size_t BundleSize = internal::traits<_Derived>::BundleSize; + + using Elements = typename internal::traits<_Derived>::Elements; + + template + using Element = typename internal::traits<_Derived>::template Element; + + template + using MapElement = typename internal::traits<_Derived>::template MapElement; + + template + using MapConstElement = typename internal::traits<_Derived>::template MapConstElement; + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::data; + using Base::coeffs; + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(BundleTangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(BundleTangentBase) + + // Tangent common API + + /** + * @brief Hat operator. + * @return An element of the Lie algebra. + */ + LieAlg hat() const; + + /** + * @brief Exponential operator. + * @return An element of the Lie Group. + */ + LieGroup exp(OptJacobianRef J_m_t = {}) const; + + /** + * @brief This function is deprecated. + * Please considere using + * @ref exp instead. + */ + MANIF_DEPRECATED + LieGroup retract(OptJacobianRef J_m_t = {}) const; + + /** + * @brief Get the right Jacobian. + */ + Jacobian rjac() const; + + /** + * @brief Get the left Jacobian. + */ + Jacobian ljac() const; + + /** + * @brief Get the inverse of the right Jacobian. + */ + Jacobian rjacinv() const; + + /** + * @brief Get the inverse of the left Jacobian. + */ + Jacobian ljacinv() const; + + /** + * @brief + */ + Jacobian smallAdj() const; + + + // BundleTangent specific API + + /** + * @brief Access BundleTangent element as Map + * @tparam _Idx element index + */ + template + MapElement<_Idx> element(); + + /** + * @brief Access BundleTangent element as Map to const + * @tparam _Idx element index + */ + template + MapConstElement<_Idx> element() const; + +protected: + + template + LieAlg hat_impl(internal::intseq<_Idx...>) const; + + template + LieGroup exp_impl(OptJacobianRef J_m_t, internal::intseq<_Idx...>) const; + + template + Jacobian rjac_impl(internal::intseq<_Idx...>) const; + + template + Jacobian ljac_impl(internal::intseq<_Idx...>) const; + + template + Jacobian rjacinv_impl(internal::intseq<_Idx...>) const; + + template + Jacobian ljacinv_impl(internal::intseq<_Idx...>) const; + + template + Jacobian smallAdj_impl(internal::intseq<_Idx...>) const; +}; + + +template +typename BundleTangentBase<_Derived>::LieAlg +BundleTangentBase<_Derived>::hat() const +{ + return hat_impl(internal::make_intseq_t{}); +} + +template +template +typename BundleTangentBase<_Derived>::LieAlg +BundleTangentBase<_Derived>::hat_impl(internal::intseq<_Idx...>) const +{ + LieAlg ret = LieAlg::Zero(); + // c++11 "fold expression" + auto l = {((ret.template block< + Element<_Idx>::LieAlg::RowsAtCompileTime, + Element<_Idx>::LieAlg::RowsAtCompileTime + >( + std::get<_Idx>(internal::traits<_Derived>::AlgIdx), + std::get<_Idx>(internal::traits<_Derived>::AlgIdx) + ) = element<_Idx>().hat()), 0) ...}; + static_cast(l); // compiler warning + return ret; +} + +template +typename BundleTangentBase<_Derived>::LieGroup +BundleTangentBase<_Derived>::exp(OptJacobianRef J_m_t) const +{ + if (J_m_t) { + J_m_t->setZero(); + } + return exp_impl(J_m_t, internal::make_intseq_t{}); +} + +template +template +typename BundleTangentBase<_Derived>::LieGroup +BundleTangentBase<_Derived>::exp_impl( + OptJacobianRef J_m_t, internal::intseq<_Idx...> +) const +{ + if (J_m_t) { + return LieGroup( + element<_Idx>().exp( + J_m_t->template block< + Element<_Idx>::DoF, Element<_Idx>::DoF + >( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) + ) ... + ); + } + return LieGroup(element<_Idx>().exp() ...); +} + +template +typename BundleTangentBase<_Derived>::LieGroup +BundleTangentBase<_Derived>::retract(OptJacobianRef J_m_t) const +{ + return exp(J_m_t); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjac() const +{ + return rjac_impl(internal::make_intseq_t{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljac() const +{ + return ljac_impl(internal::make_intseq_t{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjacinv() const +{ + return rjacinv_impl(internal::make_intseq_t{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljacinv() const +{ + return ljacinv_impl(internal::make_intseq_t{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::smallAdj() const +{ + return smallAdj_impl(internal::make_intseq_t{}); +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjac_impl(internal::intseq<_Idx...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = {((Jr.template block::DoF, Element<_Idx>::DoF>( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) = element<_Idx>().rjac() ), 0) ...}; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljac_impl(internal::intseq<_Idx...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = {((Jr.template block::DoF, Element<_Idx>::DoF>( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) = element<_Idx>().ljac()), 0) ...}; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjacinv_impl(internal::intseq<_Idx...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = { + ((Jr.template block::DoF, Element<_Idx>::DoF>( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) = element<_Idx>().rjacinv()), 0) ... + }; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljacinv_impl(internal::intseq<_Idx...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = { + ((Jr.template block::DoF, Element<_Idx>::DoF>( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) = element<_Idx>().ljacinv()), 0) ... + }; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::smallAdj_impl(internal::intseq<_Idx...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = { + ((Jr.template block::DoF, Element<_Idx>::DoF>( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) = element<_Idx>().smallAdj()), 0) ... + }; + static_cast(l); // compiler warning + return Jr; +} + +template +template +auto BundleTangentBase<_Derived>::element() -> MapElement<_Idx> +{ + return MapElement<_Idx>( + static_cast<_Derived &>(*this).coeffs().data() + + std::get<_Idx>(internal::traits<_Derived>::RepSizeIdx) + ); +} + +template +template +auto BundleTangentBase<_Derived>::element() const -> MapConstElement<_Idx> +{ + return MapConstElement<_Idx>( + static_cast(*this).coeffs().data() + + std::get<_Idx>(internal::traits<_Derived>::RepSizeIdx) + ); +} + +namespace internal { + +/** + * @brief Generator specialization for BundleTangentBase objects. + */ +template +struct GeneratorEvaluator> +{ + static typename BundleTangentBase::LieAlg + run(const unsigned int i) + { + MANIF_CHECK( + i < BundleTangentBase::DoF, + "Index i must less than DoF!", + invalid_argument + ); + + return run(i, make_intseq_t{}); + } + + template + static typename BundleTangentBase::LieAlg + run(const unsigned int i, intseq<_Idx...>) + { + using LieAlg = typename BundleTangentBase::LieAlg; + LieAlg Ei = LieAlg::Zero(); + // c++11 "fold expression" + auto l = {((Ei.template block< + Derived::template Element<_Idx>::LieAlg::RowsAtCompileTime, + Derived::template Element<_Idx>::LieAlg::RowsAtCompileTime + >( + std::get<_Idx>(internal::traits::AlgIdx), + std::get<_Idx>(internal::traits::AlgIdx) + ) = ( + static_cast(i) >= std::get<_Idx>(internal::traits::DoFIdx) && + static_cast(i) < std::get<_Idx>(internal::traits::DoFIdx) + Derived::template Element<_Idx>::DoF + ) ? + Derived::template Element<_Idx>::Generator( + static_cast(i) - std::get<_Idx>(internal::traits::DoFIdx) + ) : + Derived::template Element<_Idx>::LieAlg::Zero() + ), 0) ...}; + static_cast(l); // compiler warning + return Ei; + } +}; + +/** + * @brief Random specialization for BundleTangent objects. + */ +template +struct RandomEvaluatorImpl> +{ + static void run(BundleTangentBase & m) + { + run(m, make_intseq_t{}); + } + + template + static void run(BundleTangentBase & m, intseq<_Idx...>) + { + m = typename BundleTangentBase::Tangent( + Derived::template Element<_Idx>::Random() ... + ); + } +}; + +//! @brief Vee specialization for BundleTangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + return vee_impl( + t, v, internal::make_intseq_t::BundleSize>{} + ); + } +}; + +template +void vee_impl(TL& t, const TR& v, internal::intseq<_Idx...>) { + // c++11 "fold expression" + auto l = {((t.template element<_Idx>().setVee( + v.template block< + TL::template Element<_Idx>::LieAlg::RowsAtCompileTime, + TL::template Element<_Idx>::LieAlg::RowsAtCompileTime + >( + std::get<_Idx>(internal::traits::AlgIdx), + std::get<_Idx>(internal::traits::AlgIdx) + ))), 0) ...}; + static_cast(l); // compiler warning +} + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLETANGENT_BASE_H_ diff --git a/include/manif/impl/bundle/BundleTangent_map.h b/include/manif/impl/bundle/BundleTangent_map.h new file mode 100644 index 00000000..0b91f46c --- /dev/null +++ b/include/manif/impl/bundle/BundleTangent_map.h @@ -0,0 +1,99 @@ +#ifndef _MANIF_MANIF_BUNDLETANGENT_MAP_H_ +#define _MANIF_MANIF_BUNDLETANGENT_MAP_H_ + +#include "manif/impl/bundle/BundleTangent.h" + +namespace manif { +namespace internal { + +/** + * @brief traits specialization for Eigen Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::DoF; + using Base = BundleTangentBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +/** + * @brief traits specialization for Eigen const Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::DoF; + using Base = BundleTangentBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::Bundle + */ +template class ... T> +class Map, 0> + : public manif::BundleTangentBase, 0>> +{ + using Base = manif::BundleTangentBase, 0>>; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::BundleSize; + + Map(Scalar * coeffs) : data_(coeffs) { } + + MANIF_TANGENT_MAP_ASSIGN_OP(BundleTangent) + + DataType & coeffs() {return data_;} + + const DataType & coeffs() const {return data_;} + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::BundleTangent + */ +template class ... T> +class Map, 0> + : public manif::BundleTangentBase, 0>> +{ + using Base = manif::BundleTangentBase, 0>>; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::BundleSize; + + Map(const Scalar * coeffs) : data_(coeffs) { } + + const DataType & coeffs() const {return data_;} + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_BUNDLETANGENT_MAP_H_ diff --git a/include/manif/impl/bundle/Bundle_base.h b/include/manif/impl/bundle/Bundle_base.h new file mode 100644 index 00000000..afc48bd3 --- /dev/null +++ b/include/manif/impl/bundle/Bundle_base.h @@ -0,0 +1,442 @@ +#ifndef _MANIF_MANIF_BUNDLE_BASE_H_ +#define _MANIF_MANIF_BUNDLE_BASE_H_ + +#include "manif/impl/lie_group_base.h" +#include "manif/impl/traits.h" + +namespace manif { + +/** + * @brief The base class of the Bundle group. + */ +template +struct BundleBase : LieGroupBase<_Derived> +{ +private: + + using Base = LieGroupBase<_Derived>; + using Type = BundleBase<_Derived>; + +public: + + /** + * @brief Number of elements in bundle + */ + static constexpr std::size_t BundleSize = internal::traits<_Derived>::BundleSize; + + using Elements = typename internal::traits<_Derived>::Elements; + + template + using Element = typename internal::traits<_Derived>::template Element; + + template + using MapElement = typename internal::traits<_Derived>::template MapElement; + + template + using MapConstElement = typename internal::traits<_Derived>::template MapConstElement; + + MANIF_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_AUTO_API + MANIF_INHERIT_GROUP_OPERATOR + + using Base::coeffs; + + using Transformation = typename internal::traits<_Derived>::Transformation; + + // LieGroup common API + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(BundleBase) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(BundleBase) + + /** + * @brief Get the inverse of this. + * @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. + */ + LieGroup inverse(OptJacobianRef J_minv_m = {}) const; + + /** + * @brief Get the corresponding Lie algebra element. + * @param[out] -optional- J_t_m Jacobian of the tangent wrt to this. + * @return The tangent of this. + */ + Tangent log(OptJacobianRef J_t_m = {}) const; + + /** + * @brief This function is deprecated. + * Please consider using + * @ref log instead. + */ + MANIF_DEPRECATED + Tangent lift(OptJacobianRef J_t_m = {}) const; + + /** + * @brief Composition of this and another Bundle element. + * @param[in] m Another Bundle element. + * @param[out] -optional- J_mc_ma Jacobian of the composition wrt this. + * @param[out] -optional- J_mc_mb Jacobian of the composition wrt m. + * @return The composition of 'this . m'. + */ + template + LieGroup compose( + const LieGroupBase<_DerivedOther> & m, + OptJacobianRef J_mc_ma = {}, + OptJacobianRef J_mc_mb = {} + ) const; + + /** + * @brief Bundle group action + * @param v vector. + * @param[out] -optional- J_vout_m The Jacobian of the new object wrt this. + * @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object. + * @return The translated vector. + */ + Vector act( + const Vector & v, + tl::optional>> J_vout_m = {}, + tl::optional>> J_vout_v = {} + ) const; + + /** + * @brief Get the adjoint matrix at this. + */ + Jacobian adj() const; + + + // Bundle-specific API + + /** + * @brief Get the element-diagonal transformation matrix + */ + Transformation transform() const; + + /** + * @brief Access Bundle element as Map + * @tparam _Idx element index + */ + template + MapElement<_Idx> element(); + + /** + * @brief Access Bundle element as Map to const + * @tparam _Idx element index + */ + template + MapConstElement<_Idx> element() const; + +protected: + + template + LieGroup inverse_impl(OptJacobianRef, internal::intseq<_Idx...>) const; + + template + Tangent log_impl(OptJacobianRef, internal::intseq<_Idx...>) const; + + template + LieGroup compose_impl( + const LieGroupBase<_DerivedOther> & m, + OptJacobianRef J_mc_ma, + OptJacobianRef J_mc_mb, + internal::intseq<_Idx...> + ) const; + + template + Vector act_impl( + const Vector & v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v, + internal::intseq<_Idx...> + ) const; + + template + Jacobian adj_impl(internal::intseq<_Idx...>) const; + + template + Transformation + transform_impl(internal::intseq<_Idx...>) const; +}; + + +template +typename BundleBase<_Derived>::Transformation +BundleBase<_Derived>::transform() const +{ + return transform_impl(internal::make_intseq_t{}); +} + +template +template +typename BundleBase<_Derived>::Transformation +BundleBase<_Derived>::transform_impl( + internal::intseq<_Idx...> +) const { + Transformation ret = Transformation::Zero(); + // cxx11 "fold expression" + auto l = + {((ret.template element< + Element<_Idx>::Dim+1, Element<_Idx>::Dim+1 + >( + std::get<_Idx>(internal::traits<_Derived>::TraIdx), + std::get<_Idx>(internal::traits<_Derived>::TraIdx) + ) = element<_Idx>().transform()), 0) ...}; + static_cast(l); // compiler warning + return ret; +} + +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::inverse(OptJacobianRef J_minv_m) const +{ + if (J_minv_m) { + J_minv_m->setZero(); + } + return inverse_impl(J_minv_m, internal::make_intseq_t{}); +} + +template +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::inverse_impl( + OptJacobianRef J_minv_m, internal::intseq<_Idx...> +) const { + if (J_minv_m) { + return LieGroup( + element<_Idx>().inverse( + J_minv_m->template block< + Element<_Idx>::DoF, + Element<_Idx>::DoF + >( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) + ) ... + ); + } + return LieGroup(element<_Idx>().inverse() ...); +} + +template +typename BundleBase<_Derived>::Tangent +BundleBase<_Derived>::log(OptJacobianRef J_t_m) const +{ + if (J_t_m) { + J_t_m->setZero(); + } + return log_impl(J_t_m, internal::make_intseq_t{}); +} + +template +template +typename BundleBase<_Derived>::Tangent +BundleBase<_Derived>::log_impl( + OptJacobianRef J_minv_m, + internal::intseq<_Idx...> +) const { + if (J_minv_m) { + return Tangent( + element<_Idx>().log( + J_minv_m->template block< + Element<_Idx>::DoF, + Element<_Idx>::DoF + >( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) + )... + ); + } + return Tangent(element<_Idx>().log() ...); +} + +template +typename BundleBase<_Derived>::Tangent +BundleBase<_Derived>::lift(OptJacobianRef J_t_m) const +{ + return log(J_t_m); +} + +template +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::compose( + const LieGroupBase<_DerivedOther> & m, + OptJacobianRef J_mc_ma, + OptJacobianRef J_mc_mb +) const { + if (J_mc_ma) { + J_mc_ma->setZero(); + } + if (J_mc_mb) { + J_mc_mb->setZero(); + } + return compose_impl(m, J_mc_ma, J_mc_mb, internal::make_intseq_t{}); +} + +template +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::compose_impl( + const LieGroupBase<_DerivedOther> & m, + OptJacobianRef J_mc_ma, + OptJacobianRef J_mc_mb, + internal::intseq<_Idx...> +) const { + return LieGroup( + element<_Idx>().compose( + static_cast(m).template element<_Idx>(), + J_mc_ma ? + J_mc_ma->template block< + Element<_Idx>::DoF, Element<_Idx>::DoF + >( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) : + tl::optional< + Eigen::Ref::DoF, Element<_Idx>::DoF>> + >{}, + J_mc_mb ? + J_mc_mb->template block< + Element<_Idx>::DoF, Element<_Idx>::DoF + >( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) : + tl::optional< + Eigen::Ref::DoF, Element<_Idx>::DoF>> + >{} + ) ... + ); +} + +template +typename BundleBase<_Derived>::Vector +BundleBase<_Derived>::act( + const typename BundleBase<_Derived>::Vector & v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v) const +{ + if (J_vout_m) { + J_vout_m->setZero(); + } + if (J_vout_v) { + J_vout_v->setZero(); + } + + return act_impl(v, J_vout_m, J_vout_v, internal::make_intseq_t{}); +} + +template +template +typename BundleBase<_Derived>::Vector +BundleBase<_Derived>::act_impl( + const typename BundleBase<_Derived>::Vector & v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v, + internal::intseq<_Idx...> +) const { + Vector ret; + // cxx11 "fold expression" + auto l = {((ret.template segment::Dim>( + std::get<_Idx>(internal::traits<_Derived>::DimIdx) + ) = element<_Idx>().act( + v.template segment::Dim>( + std::get<_Idx>(internal::traits<_Derived>::DimIdx) + ), + J_vout_m ? + J_vout_m->template block::Dim, Element<_Idx>::DoF>( + std::get<_Idx>(internal::traits<_Derived>::DimIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) : + tl::optional< + Eigen::Ref::Dim, Element<_Idx>::DoF>> + >{}, + J_vout_v ? + J_vout_v->template block::Dim, Element<_Idx>::Dim>( + std::get<_Idx>(internal::traits<_Derived>::DimIdx), + std::get<_Idx>(internal::traits<_Derived>::DimIdx) + ) : + tl::optional< + Eigen::Ref::Dim, Element<_Idx>::Dim>> + >{} + ) + ), 0) ...}; + static_cast(l); // compiler warning + return ret; +} + +template +typename BundleBase<_Derived>::Jacobian +BundleBase<_Derived>::adj() const +{ + return adj_impl(internal::make_intseq_t{}); +} + +template +template +typename BundleBase<_Derived>::Jacobian +BundleBase<_Derived>::adj_impl(internal::intseq<_Idx...>) const +{ + Jacobian adj = Jacobian::Zero(); + // cxx11 "fold expression" + auto l = {((adj.template block< + Element<_Idx>::DoF, Element<_Idx>::DoF + >( + std::get<_Idx>(internal::traits<_Derived>::DoFIdx), + std::get<_Idx>(internal::traits<_Derived>::DoFIdx) + ) = element<_Idx>().adj()), 0) ...}; + static_cast(l); // compiler warning + return adj; +} + +template +template +auto BundleBase<_Derived>::element() -> MapElement<_Idx> +{ + return MapElement<_Idx>( + static_cast<_Derived &>(*this).coeffs().data() + + std::get<_Idx>(internal::traits<_Derived>::RepSizeIdx) + ); +} + +template +template +auto BundleBase<_Derived>::element() const -> MapConstElement<_Idx> +{ + return MapConstElement<_Idx>( + static_cast(*this).coeffs().data() + + std::get<_Idx>(internal::traits<_Derived>::RepSizeIdx) + ); +} + +namespace internal { + +/** + * @brief Random specialization for Bundle objects. + */ +template +struct RandomEvaluatorImpl> +{ + static void run(BundleBase & m) + { + run(m, internal::make_intseq_t{}); + } + + template + static void run(BundleBase & m, internal::intseq<_Idx...>) + { + m = typename BundleBase::LieGroup( + BundleBase::template Element<_Idx>::Random() ... + ); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLE_BASE_H_ diff --git a/include/manif/impl/bundle/Bundle_map.h b/include/manif/impl/bundle/Bundle_map.h new file mode 100644 index 00000000..8a1c7a19 --- /dev/null +++ b/include/manif/impl/bundle/Bundle_map.h @@ -0,0 +1,97 @@ +#ifndef _MANIF_MANIF_BUNDLE_MAP_H_ +#define _MANIF_MANIF_BUNDLE_MAP_H_ + +#include "manif/impl/bundle/Bundle.h" + +namespace manif { +namespace internal { + +/** + * @brief traits specialization for Eigen Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::RepSize; + using Base = BundleBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +/** + * @brief traits specialization for Eigen const Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::RepSize; + using Base = BundleBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::Bundle + */ +template class ... T> +class Map, 0> + : public manif::BundleBase, 0>> +{ + using Base = manif::BundleBase, 0>>; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + using Base::BundleSize; + + Map(Scalar * coeffs) : data_(coeffs) { } + + MANIF_GROUP_MAP_ASSIGN_OP(Bundle) + + DataType & coeffs() {return data_;} + + const DataType & coeffs() const {return data_;} + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::Bundle + */ +template class ... T> +class Map, 0> + : public manif::BundleBase, 0>> +{ + using Base = manif::BundleBase, 0>>; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + using Base::BundleSize; + + Map(const Scalar * coeffs) : data_(coeffs) { } + + const DataType & coeffs() const {return data_;} + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_BUNDLE_MAP_H_ diff --git a/include/manif/impl/bundle/Bundle_properties.h b/include/manif/impl/bundle/Bundle_properties.h new file mode 100644 index 00000000..10e71490 --- /dev/null +++ b/include/manif/impl/bundle/Bundle_properties.h @@ -0,0 +1,33 @@ +#ifndef _MANIF_MANIF_BUNDLE_PROPERTIES_H_ +#define _MANIF_MANIF_BUNDLE_PROPERTIES_H_ + +#include "manif/impl/traits.h" + +namespace manif { + +// Forward declaration for type traits specialization +template struct BundleBase; +template struct BundleTangentBase; + +namespace internal { + +//! traits specialization +template +struct LieGroupProperties> +{ + static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension + static constexpr int DoF = traits<_Derived>::DoF; /// @brief Degrees of freedom +}; + +//! traits specialization +template +struct LieGroupProperties> +{ + static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension + static constexpr int DoF = traits<_Derived>::DoF; /// @brief Degrees of freedom +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLE_PROPERTIES_H_ diff --git a/include/manif/impl/cast.h b/include/manif/impl/cast.h new file mode 100644 index 00000000..60a17da9 --- /dev/null +++ b/include/manif/impl/cast.h @@ -0,0 +1,35 @@ +#ifndef _MANIF_MANIF_IMPL_CAST_H_ +#define _MANIF_MANIF_IMPL_CAST_H_ + +namespace manif { +namespace internal { + +template +struct CastEvaluatorImpl { + template + static auto run(const T& o) -> typename T::template LieGroupTemplate { + return typename T::template LieGroupTemplate( + o.coeffs().template cast() + ); + } +}; + +template +struct CastEvaluator : CastEvaluatorImpl { + using Base = CastEvaluatorImpl; + + CastEvaluator(const Derived& xptr) : xptr_(xptr) {} + + auto run() const -> typename Derived::template LieGroupTemplate { + return Base::run(xptr_); + } + +protected: + + const Derived& xptr_; +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_IMPL_CAST_H_ diff --git a/include/manif/impl/eigen.h b/include/manif/impl/eigen.h index 56d42150..faf67c9b 100644 --- a/include/manif/impl/eigen.h +++ b/include/manif/impl/eigen.h @@ -108,6 +108,10 @@ assert_cols_dim(x, dim); namespace manif { + +template +using SquareMatrix = Eigen::Matrix; + namespace internal { template< class Base, class Derived > @@ -165,6 +169,26 @@ skew(const Eigen::MatrixBase<_Derived>& v) -v(1), +v(0), T(0.) ).finished(); } +/** + * @brief Return either a 2x2 or a 3x3 skew matrix given a scalar or a 3-vector. + */ +template +typename std::enable_if<(internal::is_base_of_v, _Derived>() + && _Derived::RowsAtCompileTime == Eigen::Dynamic), + Eigen::Matrix>::type +skew(const Eigen::MatrixBase<_Derived>& v) +{ + using T = typename _Derived::Scalar; + + if (v.rows() == 1) { + return skew(v(0)); + } else if (v.rows() == 3) { + return skew(Eigen::Ref>(v)); + } else { + MANIF_THROW("Unexpected vector size in function skew."); + } +} + template Eigen::Matrix randPointInBall(Scalar radius) { diff --git a/include/manif/impl/lie_group_base.h b/include/manif/impl/lie_group_base.h index a63cb16e..945669a2 100644 --- a/include/manif/impl/lie_group_base.h +++ b/include/manif/impl/lie_group_base.h @@ -6,6 +6,7 @@ #include "manif/impl/eigen.h" #include "manif/impl/tangent_base.h" #include "manif/impl/assignment_assert.h" +#include "manif/impl/cast.h" #include "manif/constants.h" @@ -312,6 +313,21 @@ struct LieGroupBase template _Derived& operator *=(const LieGroupBase<_DerivedOther>& m); + //! Access the ith coeffs + auto operator [](const unsigned int i) const -> decltype(coeffs()[i]){ + return coeffs()[i]; + } + + //! Access the ith coeffs + auto operator [](const unsigned int i) -> decltype(coeffs()[i]){ + return coeffs()[i]; + } + + //! @brief The size of the underlying vector + constexpr unsigned int size() const { + return RepSize; + } + // Some static helpers //! Static helper to create a Lie group object set at Identity. @@ -400,7 +416,9 @@ template typename LieGroupBase<_Derived>::template LieGroupTemplate<_NewScalar> LieGroupBase<_Derived>::cast() const { - return LieGroupTemplate<_NewScalar>(coeffs().template cast<_NewScalar>()); + return internal::CastEvaluator< + typename internal::traits<_Derived>::Base, _NewScalar + >(derived()).run(); } template diff --git a/include/manif/impl/macro.h b/include/manif/impl/macro.h index cb4992a9..5d5fa8ff 100644 --- a/include/manif/impl/macro.h +++ b/include/manif/impl/macro.h @@ -2,6 +2,7 @@ #define _MANIF_MANIF_FWD_H_ #include // for std::runtime_error +#include // for std::forward #ifdef NDEBUG # ifndef MANIF_NO_DEBUG @@ -156,6 +157,7 @@ raise(Args&&... args) X& operator =(Eigen::MatrixBase<_EigenDerived>&& o) { coeffs() = std::move(o); return derived(); } #define MANIF_GROUP_MAP_ASSIGN_OP(X) \ + Map(const Map & o) : Base(), data_(o.coeffs()) { }\ Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\ template \ Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ @@ -200,6 +202,7 @@ raise(Args&&... args) X& operator =(Eigen::MatrixBase<_EigenDerived>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o); return derived(); } #define MANIF_TANGENT_MAP_ASSIGN_OP(X) \ + Map(const Map & o) : Base(), data_(o.coeffs()) { }\ Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\ template \ Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ diff --git a/include/manif/impl/rn/RnTangent.h b/include/manif/impl/rn/RnTangent.h index 094ac921..09cf5a39 100644 --- a/include/manif/impl/rn/RnTangent.h +++ b/include/manif/impl/rn/RnTangent.h @@ -93,15 +93,15 @@ template using R7Tangent = RnTangent<_Scalar, 7>; template using R8Tangent = RnTangent<_Scalar, 8>; template using R9Tangent = RnTangent<_Scalar, 9>; -MANIF_EXTRA_GROUP_TYPEDEF(R1Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R2Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R3Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R4Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R5Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R6Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R7Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R8Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R9Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R1Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R2Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R3Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R4Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R5Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R6Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R7Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R8Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R9Tangent) template template diff --git a/include/manif/impl/rn/RnTangent_base.h b/include/manif/impl/rn/RnTangent_base.h index 325bd886..a4f7b7c8 100644 --- a/include/manif/impl/rn/RnTangent_base.h +++ b/include/manif/impl/rn/RnTangent_base.h @@ -84,7 +84,7 @@ struct RnTangentBase : TangentBase<_Derived> Jacobian rjacinv() const; /** - * @brief Get the inverse of the right Jacobian of Rn. + * @brief Get the inverse of the left Jacobian of Rn. * @note See Eq. (191). * @see ljac. */ @@ -122,7 +122,7 @@ template typename RnTangentBase<_Derived>::LieAlg RnTangentBase<_Derived>::hat() const { - LieAlg t_hat = LieAlg::Constant(0); + LieAlg t_hat = LieAlg::Zero(); t_hat.template topRightCorner() = coeffs(); return t_hat; } @@ -161,7 +161,7 @@ template typename RnTangentBase<_Derived>::Jacobian RnTangentBase<_Derived>::smallAdj() const { - static const Jacobian smallAdj = Jacobian::Constant(0); + static const Jacobian smallAdj = Jacobian::Zero(); return smallAdj; } @@ -184,7 +184,7 @@ struct GeneratorEvaluator> using LieAlg = typename RnTangentBase::LieAlg; - LieAlg Ei = LieAlg::Constant(0); + LieAlg Ei = LieAlg::Zero(); Ei(i, RnTangentBase::DoF) = 1; @@ -202,6 +202,24 @@ struct RandomEvaluatorImpl> } }; +//! @brief Bracket specialization for RnTangentBase objects. +template +struct BracketEvaluatorImpl> { + template + static typename Derived::Tangent run(const TL&, const TR&) { + return Derived::Tangent::Zero(); + } +}; + +//! @brief Vee specialization for RnTangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() = v.template topRightCorner(); + } +}; + } // namespace internal } // namespace manif diff --git a/include/manif/impl/rn/Rn_base.h b/include/manif/impl/rn/Rn_base.h index 7843d1c0..ab9be11e 100644 --- a/include/manif/impl/rn/Rn_base.h +++ b/include/manif/impl/rn/Rn_base.h @@ -156,8 +156,6 @@ RnBase<_Derived>::compose( OptJacobianRef J_mc_ma, OptJacobianRef J_mc_mb) const { - using std::abs; - static_assert( std::is_base_of, _DerivedOther>::value, "Argument does not inherit from RnBase !"); diff --git a/include/manif/impl/se2/SE2Tangent_base.h b/include/manif/impl/se2/SE2Tangent_base.h index 50149db0..05ac86ab 100644 --- a/include/manif/impl/se2/SE2Tangent_base.h +++ b/include/manif/impl/se2/SE2Tangent_base.h @@ -480,6 +480,15 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SE2TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(0, 2), v(1, 2), v(1, 0); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/se2/SE2_base.h b/include/manif/impl/se2/SE2_base.h index fbd56e58..b4fbd1ed 100644 --- a/include/manif/impl/se2/SE2_base.h +++ b/include/manif/impl/se2/SE2_base.h @@ -424,6 +424,17 @@ struct AssignmentEvaluatorImpl> } }; +//! @brief Cast specialization for SE2Base objects. +template +struct CastEvaluatorImpl, NewScalar> { + template + static auto run(const T& o) -> typename Derived::template LieGroupTemplate { + return typename Derived::template LieGroupTemplate( + NewScalar(o.x()), NewScalar(o.y()), NewScalar(o.angle()) + ); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/se3/SE3.h b/include/manif/impl/se3/SE3.h index 95b5fd5e..9e98de12 100644 --- a/include/manif/impl/se3/SE3.h +++ b/include/manif/impl/se3/SE3.h @@ -154,12 +154,17 @@ SE3<_Scalar>::SE3(const LieGroupBase<_DerivedOther>& o) // } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +// Temporarily disable this warning which causes false positive with GCC 12 +// See e.g. https://gcc.gnu.org/bugzilla/show_bug.cgi?id=106247 template SE3<_Scalar>::SE3(const Translation& t, const Eigen::Quaternion& q) : SE3((DataType() << t, q.coeffs() ).finished()) { // } +#pragma GCC diagnostic pop template SE3<_Scalar>::SE3(const Translation& t, const Eigen::AngleAxis& a) diff --git a/include/manif/impl/se3/SE3Tangent_base.h b/include/manif/impl/se3/SE3Tangent_base.h index ebb8f635..a4860c90 100644 --- a/include/manif/impl/se3/SE3Tangent_base.h +++ b/include/manif/impl/se3/SE3Tangent_base.h @@ -458,6 +458,15 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SE3TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(0, 3), v(1, 3), v(2, 3), v(2, 1), v(0, 2), v(1, 0); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/se3/SE3_base.h b/include/manif/impl/se3/SE3_base.h index cc84a652..f7a69c30 100644 --- a/include/manif/impl/se3/SE3_base.h +++ b/include/manif/impl/se3/SE3_base.h @@ -443,7 +443,7 @@ struct RandomEvaluatorImpl> } }; -//! @brief Assignment assert specialization for SE2Base objects +//! @brief Assignment assert specialization for SE3Base objects template struct AssignmentEvaluatorImpl> { @@ -461,6 +461,20 @@ struct AssignmentEvaluatorImpl> } }; +//! @brief Cast specialization for SE3Base objects. +template +struct CastEvaluatorImpl, NewScalar> { + template + static auto run(const T& o) -> typename Derived::template LieGroupTemplate { + const typename SE3Base::QuaternionDataType q = o.quat(); + const typename SE3Base::Translation t = o.translation(); + + return typename Derived::template LieGroupTemplate( + t.template cast(), q.template cast().normalized() + ); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/se_2_3/SE_2_3Tangent_base.h b/include/manif/impl/se_2_3/SE_2_3Tangent_base.h index 781b3769..2d44b9c3 100644 --- a/include/manif/impl/se_2_3/SE_2_3Tangent_base.h +++ b/include/manif/impl/se_2_3/SE_2_3Tangent_base.h @@ -478,6 +478,17 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SE_2_3TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(0, 3), v(1, 3), v(2, 3), + v(2, 1), v(0, 2), v(1, 0), + v(0, 4), v(1, 4), v(2, 4); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/se_2_3/SE_2_3_base.h b/include/manif/impl/se_2_3/SE_2_3_base.h index 00a23701..329155ce 100644 --- a/include/manif/impl/se_2_3/SE_2_3_base.h +++ b/include/manif/impl/se_2_3/SE_2_3_base.h @@ -47,6 +47,7 @@ struct SE_2_3Base : LieGroupBase<_Derived> using Rotation = typename internal::traits<_Derived>::Rotation; using Translation = typename internal::traits<_Derived>::Translation; using LinearVelocity = typename internal::traits<_Derived>::LinearVelocity; + using Transformation = Eigen::Matrix; using Isometry = Eigen::Matrix; /**< Double direct spatial isometry*/ using QuaternionDataType = Eigen::Quaternion; @@ -118,6 +119,14 @@ struct SE_2_3Base : LieGroupBase<_Derived> // SE_2_3 specific functions + /** + * Get the isometry object (double direct isometry). + * @note T = | R t v| + * | 1 | + * | 1| + */ + Transformation transform() const; + /** * Get the isometry object (double direct isometry). * @note T = | R t v| @@ -195,8 +204,8 @@ struct SE_2_3Base : LieGroupBase<_Derived> }; template -typename SE_2_3Base<_Derived>::Isometry -SE_2_3Base<_Derived>::isometry() const +typename SE_2_3Base<_Derived>::Transformation +SE_2_3Base<_Derived>::transform() const { Eigen::Matrix T; T.template topLeftCorner<3,3>() = rotation(); @@ -207,6 +216,13 @@ SE_2_3Base<_Derived>::isometry() const return T; } +template +typename SE_2_3Base<_Derived>::Isometry +SE_2_3Base<_Derived>::isometry() const +{ + return Isometry(transform()); +} + template typename SE_2_3Base<_Derived>::Rotation SE_2_3Base<_Derived>::rotation() const @@ -452,6 +468,23 @@ struct AssignmentEvaluatorImpl> } }; +//! @brief Cast specialization for SE_2_3Base objects. +template +struct CastEvaluatorImpl, NewScalar> { + template + static auto run(const T& o) -> typename Derived::template LieGroupTemplate { + const typename SE_2_3Base::QuaternionDataType q = o.quat(); + const typename SE_2_3Base::Translation t = o.translation(); + const typename SE_2_3Base::LinearVelocity v = o.linearVelocity(); + + return typename Derived::template LieGroupTemplate( + t.template cast(), + q.template cast().normalized(), + v.template cast() + ); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/sgal3/SGal3.h b/include/manif/impl/sgal3/SGal3.h new file mode 100644 index 00000000..66312d08 --- /dev/null +++ b/include/manif/impl/sgal3/SGal3.h @@ -0,0 +1,254 @@ +#ifndef _MANIF_MANIF_SGAL3_H_ +#define _MANIF_MANIF_SGAL3_H_ + +#include "manif/impl/sgal3/SGal3_base.h" + +namespace manif { + +// Forward declare for type traits specialization +template struct SGal3; +template struct SGal3Tangent; + +namespace internal { + +//! Traits specialization +template +struct traits> { + using Scalar = _Scalar; + + using LieGroup = SGal3<_Scalar>; + using Tangent = SGal3Tangent<_Scalar>; + + using Base = SGal3Base>; + + static constexpr int Dim = LieGroupProperties::Dim; + static constexpr int DoF = LieGroupProperties::DoF; + static constexpr int RepSize = 11; + + /// @todo would be nice to concat vec3 + quaternion + vec3 + t + using DataType = Eigen::Matrix; + + using Jacobian = Eigen::Matrix; + using Rotation = Eigen::Matrix; + using Translation = Eigen::Matrix; + using LinearVelocity = Eigen::Matrix; + using Vector = Eigen::Matrix; +}; + +} // namespace internal +} // namespace manif + +namespace manif { + +// +// LieGroup +// + +/** + * @brief Represent an element of SGal3. + */ +template +struct SGal3 : SGal3Base> { +private: + + using Base = SGal3Base>; + using Type = SGal3<_Scalar>; + +protected: + + using Base::derived; + +public: + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_COMPLETE_GROUP_TYPEDEF + using Translation = typename Base::Translation; + using Quaternion = Eigen::Quaternion; + using LinearVelocity = typename Base::LinearVelocity; + + MANIF_INHERIT_GROUP_API + using Base::rotation; + using Base::normalize; + + SGal3() = default; + ~SGal3() = default; + + MANIF_COPY_CONSTRUCTOR(SGal3) + MANIF_MOVE_CONSTRUCTOR(SGal3) + + template + SGal3(const LieGroupBase<_DerivedOther>& o); + + MANIF_GROUP_ASSIGN_OP(SGal3) + + /** + * @brief Constructor given a translation, a unit quaternion and a linear velocity. + * @param[in] t A translation vector. + * @param[in] q A unit quaternion. + * @param[in] v A linear velocity vector. + * @param[in] time A time. + * @throws manif::invalid_argument on un-normalized complex number. + */ + SGal3( + const Translation& t, + const Eigen::Quaternion& q, + const LinearVelocity& v, + const Scalar time + ); + + /** + * @brief Constructor given a translation, an angle axis and a linear velocity. + * @param[in] t A translation vector. + * @param[in] angle_axis An angle-axis. + * @param[in] v A linear velocity vector. + * @param[in] time A time. + */ + SGal3( + const Translation& t, + const Eigen::AngleAxis& angle_axis, + const LinearVelocity& v, + const Scalar time + ); + + /** + * @brief Constructor given a translation, SO3 element and a linear velocity. + * @param[in] t A translation vector. + * @param[in] SO3 An element of SO3. + * @param[in] v A linear velocity vector. + * @param[in] time A time. + */ + SGal3( + const Translation& t, + const SO3& SO3, + const LinearVelocity& v, + const Scalar time + ); + + /** + * @brief Constructor given translation components, + * roll-pitch-yaw angles and linear velocity components + * @param[in] x The x component of the translation. + * @param[in] y The y component of the translation. + * @param[in] z The z component of the translation. + * @param[in] roll The roll angle. + * @param[in] pitch The pitch angle. + * @param[in] yaw The yaw angle. + * @param[in] vx The x component of the linear velocity. + * @param[in] vy The y component of the linear velocity. + * @param[in] vz The z component of the linear velocity. + * @param[in] t time. + */ + SGal3( + const Scalar x, const Scalar y, const Scalar z, + const Scalar roll, const Scalar pitch, const Scalar yaw, + const Scalar vx, const Scalar vy, const Scalar vz, + const Scalar t + ); + + /** + * @brief Constructor from a 3D Eigen::Isometry relevant to SE(3) and a linear velocity + * @param[in] h a isometry object from Eigen defined for SE(3) + * @param[in] v a linear velocity vector. + * @note overall, this should be a double direct spatial isometry, + */ + SGal3( + const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h, + const LinearVelocity& v, + const Scalar t + ); + + // LieGroup common API + + DataType& coeffs(); + const DataType& coeffs() const; + + // SGal3 specific API + +protected: + + DataType data_; +}; + +MANIF_EXTRA_GROUP_TYPEDEF(SGal3) + +template +template +SGal3<_Scalar>::SGal3(const LieGroupBase<_DerivedOther>& o) : SGal3(o.coeffs()) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Translation& t, + const Eigen::Quaternion& q, + const LinearVelocity& v, + const Scalar time +) : SGal3((DataType() << t, q.coeffs(), v, time).finished()) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Translation& t, + const Eigen::AngleAxis& a, + const LinearVelocity& v, + const Scalar time +) : SGal3(t, Quaternion(a), v, time) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Scalar x, const Scalar y, const Scalar z, + const Scalar roll, const Scalar pitch, const Scalar yaw, + const Scalar vx, const Scalar vy, const Scalar vz, + const Scalar t +) : SGal3( + Translation(x,y,z), + Eigen::Quaternion( + Eigen::AngleAxis(yaw, Eigen::Matrix::UnitZ()) * + Eigen::AngleAxis(pitch, Eigen::Matrix::UnitY()) * + Eigen::AngleAxis(roll, Eigen::Matrix::UnitX()) + ), + LinearVelocity(vx, vy, vz), + t +) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Translation& t, + const SO3& so3, + const LinearVelocity& v, + const Scalar time +) : SGal3(t, so3.quat(), v, time) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Eigen::Transform<_Scalar, 3, Eigen::Isometry>& h, + const LinearVelocity& v, + const Scalar t +) : SGal3(h.translation(), Eigen::Quaternion<_Scalar>(h.rotation()), v, t) { + // +} + + +template +typename SGal3<_Scalar>::DataType& +SGal3<_Scalar>::coeffs() { + return data_; +} + +template +const typename SGal3<_Scalar>::DataType& +SGal3<_Scalar>::coeffs() const { + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3_H_ diff --git a/include/manif/impl/sgal3/SGal3Tangent.h b/include/manif/impl/sgal3/SGal3Tangent.h new file mode 100644 index 00000000..97692f18 --- /dev/null +++ b/include/manif/impl/sgal3/SGal3Tangent.h @@ -0,0 +1,106 @@ +#ifndef _MANIF_MANIF_SGAL3TANGENT_H_ +#define _MANIF_MANIF_SGAL3TANGENT_H_ + +#include "manif/impl/sgal3/SGal3Tangent_base.h" + +namespace manif { +namespace internal { + +//! Traits specialization +template +struct traits> { + using Scalar = _Scalar; + + using LieGroup = SGal3<_Scalar>; + using Tangent = SGal3Tangent<_Scalar>; + + using Base = SGal3TangentBase; + + static constexpr int Dim = LieGroupProperties::Dim; + static constexpr int DoF = LieGroupProperties::DoF; + static constexpr int RepSize = DoF; + + using DataType = Eigen::Matrix; + + using Jacobian = Eigen::Matrix; + using LieAlg = Eigen::Matrix; +}; + +} // namespace internal +} // namespace manif + +namespace manif { + +// +// Tangent +// + +/** + * @brief Represents an element of tangent space of SGal3. + */ +template +struct SGal3Tangent : SGal3TangentBase> { +private: + + using Base = SGal3TangentBase>; + using Type = SGal3Tangent<_Scalar>; + +protected: + + using Base::derived; + +public: + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + SGal3Tangent() = default; + ~SGal3Tangent() = default; + + MANIF_COPY_CONSTRUCTOR(SGal3Tangent) + MANIF_MOVE_CONSTRUCTOR(SGal3Tangent) + + template + SGal3Tangent(const TangentBase<_DerivedOther>& o); + + MANIF_TANGENT_ASSIGN_OP(SGal3Tangent) + + // Tangent common API + + DataType& coeffs(); + const DataType& coeffs() const; + + // SGal3Tangent specific API + +protected: + + DataType data_; +}; + +MANIF_EXTRA_TANGENT_TYPEDEF(SGal3Tangent); + +template +template +SGal3Tangent<_Scalar>::SGal3Tangent(const TangentBase<_DerivedOther>& o) + : data_(o.coeffs()) { + // +} + +template +typename SGal3Tangent<_Scalar>::DataType& +SGal3Tangent<_Scalar>::coeffs() { + return data_; +} + +template +const typename SGal3Tangent<_Scalar>::DataType& +SGal3Tangent<_Scalar>::coeffs() const { + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3TANGENT_H_ diff --git a/include/manif/impl/sgal3/SGal3Tangent_base.h b/include/manif/impl/sgal3/SGal3Tangent_base.h new file mode 100644 index 00000000..f756ab9d --- /dev/null +++ b/include/manif/impl/sgal3/SGal3Tangent_base.h @@ -0,0 +1,635 @@ +#ifndef _MANIF_MANIF_SGAL3TANGENT_BASE_H_ +#define _MANIF_MANIF_SGAL3TANGENT_BASE_H_ + +#include "manif/impl/sgal3/SGal3_properties.h" +#include "manif/impl/tangent_base.h" +#include "manif/impl/so3/SO3Tangent_map.h" +#include "manif/impl/se3/SE3Tangent.h" + +namespace manif { + +// +// Tangent +// + +/** + * @brief The base class of the SGal3 tangent. + */ +template +struct SGal3TangentBase : TangentBase<_Derived> { +private: + + using Base = TangentBase<_Derived>; + using Type = SGal3TangentBase<_Derived>; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using LinBlock = typename DataType::template FixedSegmentReturnType<3>::Type; + using AngBlock = typename DataType::template FixedSegmentReturnType<3>::Type; + using ConstLinBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type; + using ConstAngBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type; + + using Base::data; + using Base::coeffs; + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SGal3TangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(SGal3TangentBase) + + // Tangent common API + + /** + * @brief Hat operator of SGal3. + * @return An element of the Lie algebra se_2_3. + * @note See Eq. (169). + */ + LieAlg hat() const; + + /** + * @brief Get the SGal3 element. + * @param[out] -optional- J_m_t Jacobian of the SGal3 element wrt this. + * @return The SGal3 element. + * @note This is the exp() map with the argument in vector form. + */ + LieGroup exp(OptJacobianRef J_m_t = {}) const; + + /** + * @brief This function is deprecated. + * Please considere using + * @ref exp instead. + */ + MANIF_DEPRECATED + LieGroup retract(OptJacobianRef J_m_t = {}) const; + + /** + * @brief Get the right Jacobian of SGal3. + */ + Jacobian rjac() const; + + /** + * @brief Get the left Jacobian of SGal3. + */ + Jacobian ljac() const; + + /** + * @brief Get the small adjoint matrix ad() of SGal3 + * that maps isomorphic tangent vectors of SGal3 + * @return + */ + Jacobian smallAdj() const; + + // SGal3Tangent specific API + + //! @brief Get the linear translation part. + LinBlock lin(); + const ConstLinBlock lin() const; + + //! @brief Get the angular part. + AngBlock ang(); + const ConstAngBlock ang() const; + + //! @brief Get the linear velocity part + LinBlock lin2(); + const ConstLinBlock lin2() const; + + Scalar t() const; + +public: /// @todo make protected + + const Eigen::Map> asSO3() const { + return Eigen::Map>(coeffs().data()+6); + } + + Eigen::Map> asSO3() { + return Eigen::Map>(coeffs().data()+6); + } + + static void fillE( + Eigen::Ref> E, + const Eigen::Map>& so3 + ); +}; + +template +typename SGal3TangentBase<_Derived>::LieGroup +SGal3TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const { + if (J_m_t) { + *J_m_t = rjac(); + } + + const Eigen::Map> so3 = asSO3(); + const typename SO3::Jacobian so3_ljac = so3.ljac(); + + Eigen::Matrix E; + fillE(E, so3); + + return LieGroup( + so3_ljac * lin() + (E * (t() * lin2())), + so3.exp(), + so3_ljac * lin2(), + t() + ); +} + +template +typename SGal3TangentBase<_Derived>::LieGroup +SGal3TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const { + return exp(J_m_t); +} + +template +typename SGal3TangentBase<_Derived>::LieAlg +SGal3TangentBase<_Derived>::hat() const { + LieAlg sgal3; + + sgal3.template topLeftCorner<3, 3>() = skew(ang()); + sgal3.template block<3, 1>(0, 3) = lin2(); + sgal3.template topRightCorner<3, 1>() = lin(); + sgal3(3, 4) = t(); + + sgal3.template bottomLeftCorner<2, 4>().setZero(); + sgal3(4, 4) = Scalar(0); + + return sgal3; +} + +template +typename SGal3TangentBase<_Derived>::Jacobian +SGal3TangentBase<_Derived>::rjac() const { + + return (-*this).ljac(); + + // Those were verified against auto diff + + // Jacobian Jr = Jacobian::Zero(); + // Jr.template topLeftCorner<3, 3>() = asSO3().rjac(); + // // Jr.template block<3, 3>(0, 3) = ??; + // // Jr.template block<3, 3>(0, 6) = ??; + + // // Jr.template block<3, 1>(0, 9) = ??; + + // Jr.template block<3, 3>(3, 3) = Jr.template topLeftCorner<3,3>(); + // SE3Tangent::fillQ( + // Jr.template block<3, 3>(3, 6), -coeffs().template segment<6>(3) + // ); + + // Jr.template block<3, 3>(6, 6) = Jr.template topLeftCorner<3,3>(); + + // Jr(9, 9) = Scalar(1); + + // Jr.template bottomLeftCorner<7, 3>().setZero(); + // Jr.template block<4, 3>(6, 3).setZero(); + // Jr.template block<1, 3>(9, 6).setZero(); + // Jr.template block<6, 1>(3, 9).setZero(); + + // return Jr; +} + +template +typename SGal3TangentBase<_Derived>::Jacobian +SGal3TangentBase<_Derived>::ljac() const { + using ConstRef33 = const Eigen::Ref>; + using ConstRef61 = const Eigen::Ref>; + + using Diag = typename Eigen::DiagonalMatrix; + auto I33 = [](const Scalar d){ return Diag(d, d, d).toDenseMatrix(); }; + + using std::sqrt; + using std::cos; + using std::sin; + + /** Structure of the left Jacobian according to J. Kelly + * + * Jl = [ D -L*t N E*nu + * 0 D M 0 + * 0 0 D 0 + * 0 0 0 1 ] + * + * with N = N1 - N2. + * + * Tangent space is tau = [rho ; nu ; theta ; t] in R^10 + * + * Matrix blocks D, E, L, M, N, N1, N2 are referred to in the comments and + * correspond to eqs. in Kelly's paper: + * + * D: (18) = Jl_SO3(theta) + * E: (19) + * L: (32) + * M: (33) = Q(nu,theta) + * N: (34) = N1 - N2 + * N1: (35) = Q(rho,theta) + * N2: (36) + * + * Note that we use the following temporary blocks for computation + * + * Jl = [ . . E . + * V . . rho + * W W^2 . theta + * . . . . ] + * + */ + + Jacobian Jl; + + // theta vector + const Eigen::Map> so3 = asSO3(); + + // Skew matrix W = theta^ + Jl.template block<3, 3>(3, 0) = so3.hat(); + ConstRef33 W = Jl.template block<3, 3>(3, 0); + + // Skew matrix W^2 + Jl.template block<3, 3>(6, 3) = W * W; + ConstRef33 WW = Jl.template block<3, 3>(6, 3); + + // Skew matrix V = nu^ + Jl.template block<3, 3>(6, 0) = skew(lin2()); + ConstRef33 V = Jl.template block<3, 3>(6, 0); + + // Angles and trigonometry + const Scalar theta_sq = so3.coeffs().squaredNorm(); + // rotation angle + const Scalar theta = sqrt(theta_sq); + const Scalar theta_cu = theta * theta_sq; + + const Scalar sin_t = sin(theta); + const Scalar cos_t = cos(theta); + + // Blocks D + Jl.template topLeftCorner<3, 3>() = so3.ljac(); + Jl.template block<3, 3>(3, 3) = Jl.template topLeftCorner<3, 3>(); + Jl.template block<3, 3>(6, 6) = Jl.template topLeftCorner<3, 3>(); + + // Block E + // Note - we use here a temporary block to hold E + Jl.template block<3, 3>(0, 6) = I33(Scalar(0.5)); + if (theta_sq > Constants::eps) { + const Scalar A = (theta - sin_t) / theta_sq / theta; + const Scalar B = ( + theta_sq + Scalar(2) * cos_t - Scalar(2) + ) / (Scalar(2) * theta_sq * theta_sq); + + Jl.template block<3, 3>(0, 6).noalias() += A * W + B * WW; + } + + // Block E * nu + Jl.template block<3, 1>(0, 9) = Jl.template block<3, 3>(0, 6) * lin2(); + + // Block L + Scalar cA, cB; + // small angle approx. + if (theta_cu > Constants::eps) { + cA = (sin_t - theta * cos_t) / theta_cu; + cB = ( + theta_sq + Scalar(2) * (Scalar(1) - theta * sin_t - cos_t) + ) / (Scalar(2) * theta_sq * theta_sq); + } else { + cA = Scalar(1./3.) - Scalar(1./30.) * theta_sq; + cB = Scalar(1./8.); + } + + // Block - L * t + Jl.template block<3, 3>(0, 3).noalias() = -t() * ( + // Block L + I33(Scalar(0.5)) + cA * W + cB * WW + ); + + // Block M = Q(nu, theta) + SE3Tangent::fillQ( + Jl.template block<3, 3>(3, 6), coeffs().template segment<6>(3) + ); + + // Block N1, part of N. N1 = Q(rho, theta) + Jl.template block<6, 1>(3, 9) << lin(), ang(); + ConstRef61 rho_theta = Jl.template block<6, 1>(3, 9); + // block N1 = Q(rho,theta) + SE3Tangent::fillQ(Jl.template block<3, 3>(0, 6), rho_theta); + + // Block N2, part of N + Scalar cC, cD, cE, cF; + if (theta_cu > Constants::eps) { + cA = (Scalar(2) - theta * sin_t - Scalar(2) * cos_t) / theta_cu / theta; + cB = ( + theta_cu + Scalar(6) * theta + Scalar(6) * theta * cos_t - Scalar(12) * sin_t + ) / (Scalar(6) * theta_cu * theta_sq); + cC = ( + Scalar(12) * sin_t - theta_cu - Scalar(3) * theta_sq * sin_t - Scalar(12) * theta * cos_t + ) / (Scalar(6) * theta_cu * theta_sq); + cD = ( + Scalar(4) + theta_sq * (Scalar(1) + cos_t) - Scalar(4) * (theta * sin_t + cos_t) + ) / (Scalar(2) * theta_cu * theta_cu); + cE = (theta_sq + Scalar(2) * (cos_t - Scalar(1))) / (Scalar(2) * theta_cu * theta); + cF = (theta_cu + Scalar(6) * (sin_t - theta)) / (Scalar(6) * theta_cu * theta_sq); + } else { + cA = Scalar(1. / 12.); + cB = Scalar(1. / 24.); + cC = Scalar(1. / 10.); + cD = Scalar(1. / 240.); + cE = Scalar(1. / 24.); + cF = Scalar(1. / 120.); + } + + // Block N = N1 - N2 + Jl.template block<3, 3>(0, 6) -= ( + // Block N2 + t() / Scalar(6) * V + + (cA * W + cB * WW) * (t() * V) + + cC * (W * V * (t() * W)) + + cD * (WW * V * (t() * W)) + + t() * V * (cE * W + cF * WW) + ); + + // Block 1 + Jl(9, 9) = Scalar(1); + + // Blocks of zeros + Jl.template bottomLeftCorner<7, 3>().setZero(); + Jl.template block<4, 3>(6, 3).setZero(); + Jl.template block<1, 3>(9, 6).setZero(); + Jl.template block<6, 1>(3, 9).setZero(); + + return Jl; +} + +template +typename SGal3TangentBase<_Derived>::Jacobian +SGal3TangentBase<_Derived>::smallAdj() const { + Jacobian smallAdj; + + smallAdj.template topLeftCorner<3,3>() = skew(ang()); + smallAdj.template block<3, 3>(0, 3) = -t() * Eigen::Matrix3d::Identity(); + smallAdj.template block<3, 3>(0, 6) = skew(lin()); + smallAdj.template block<3, 1>(0, 9) = lin2(); + + smallAdj.template block<3, 3>(3, 3) = smallAdj.template topLeftCorner<3,3>(); + smallAdj.template block<3, 3>(3, 6) = skew(lin2()); + + smallAdj.template block<3, 3>(6, 6) = smallAdj.template topLeftCorner<3,3>(); + + smallAdj.template block<7, 3>(3, 0).setZero(); + smallAdj.template block<4, 3>(6, 3).setZero(); + + smallAdj.template block<1, 3>(9, 6).setZero(); + smallAdj.template block<7, 1>(3, 9).setZero(); + + return smallAdj; +} + +// SGal3Tangent specific API + +template +typename SGal3TangentBase<_Derived>::LinBlock +SGal3TangentBase<_Derived>::lin() { + return coeffs().template head<3>(); +} + +template +const typename SGal3TangentBase<_Derived>::ConstLinBlock +SGal3TangentBase<_Derived>::lin() const { + return coeffs().template head<3>(); +} + +template +typename SGal3TangentBase<_Derived>::LinBlock +SGal3TangentBase<_Derived>::lin2() { + return coeffs().template segment<3>(3); +} + +template +const typename SGal3TangentBase<_Derived>::ConstLinBlock +SGal3TangentBase<_Derived>::lin2() const { + return coeffs().template segment<3>(3); +} + +template +typename SGal3TangentBase<_Derived>::AngBlock +SGal3TangentBase<_Derived>::ang() { + return coeffs().template segment<3>(6); +} + +template +const typename SGal3TangentBase<_Derived>::ConstAngBlock +SGal3TangentBase<_Derived>::ang() const { + return coeffs().template segment<3>(6); +} + +template +typename SGal3TangentBase<_Derived>::Scalar +SGal3TangentBase<_Derived>::t() const { + return coeffs()(9); +} + +template +void SGal3TangentBase<_Derived>::fillE( + Eigen::Ref> E, + const Eigen::Map>& so3 +) { + using I = typename Eigen::DiagonalMatrix; + + const Scalar theta_sq = so3.coeffs().squaredNorm(); + + E.noalias() = I(Scalar(0.5), Scalar(0.5), Scalar(0.5)).toDenseMatrix(); + + // small angle approx. + if (theta_sq < Constants::eps) { + return; + } + + const Scalar theta = sqrt(theta_sq); // rotation angle + + const Scalar A = (theta - sin(theta)) / theta_sq / theta; + const Scalar B = (theta_sq + Scalar(2) * cos(theta) - Scalar(2)) / (Scalar(2) * theta_sq * theta_sq); + + const typename SO3Tangent::LieAlg W = so3.hat(); + + E.noalias() += A * W + B * W * W; +} + +namespace internal { + +//! @brief Generator specialization for SGal3TangentBase objects. +template +struct GeneratorEvaluator> { + static typename SGal3TangentBase::LieAlg + run(const unsigned int i) { + using LieAlg = typename SGal3TangentBase::LieAlg; + using Scalar = typename SGal3TangentBase::Scalar; + + switch (i) { + case 0: { + static const LieAlg E0( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + + ).finished() + ); + return E0; + } + case 1: { + static const LieAlg E1( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E1; + } + case 2: { + static const LieAlg E2( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E2; + } + case 3: { + static const LieAlg E3( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E3; + } + case 4: { + static const LieAlg E4( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E4; + } + case 5: { + static const LieAlg E5( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E5; + } + case 6: { + static const LieAlg E6( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), + Scalar(0), Scalar(1), Scalar( 0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0) + ).finished() + ); + return E6; + } + case 7: { + static const LieAlg E7( + ( + LieAlg() << + Scalar( 0), Scalar(0), Scalar(1), Scalar(0), Scalar(0), + Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E7; + } + case 8: { + static const LieAlg E8( + ( + LieAlg() << + Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0), + Scalar(1), Scalar( 0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E8; + } + case 9: { + static const LieAlg E9( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E9; + } + default: + MANIF_THROW("Index i must be in [0,9]!", invalid_argument); + break; + } + + return LieAlg{}; + } +}; + +//! @brief Random specialization for SGal3TangentBase objects. +template +struct RandomEvaluatorImpl> { + static void run(SGal3TangentBase& m) { + // in [-1,1] + m.coeffs().setRandom(); + // In ball of radius PI + m.coeffs().template segment<3>(6) = randPointInBall(MANIF_PI).template cast(); + } +}; + +//! @brief Vee specialization for SGal3TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(0, 4), v(1, 4), v(2, 4), + v(0, 3), v(1, 3), v(2, 3), + v(2, 1), v(0, 2), v(1, 0), + v(3, 4); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3TANGENT_BASE_H_ diff --git a/include/manif/impl/sgal3/SGal3Tangent_map.h b/include/manif/impl/sgal3/SGal3Tangent_map.h new file mode 100644 index 00000000..74fc04bd --- /dev/null +++ b/include/manif/impl/sgal3/SGal3Tangent_map.h @@ -0,0 +1,85 @@ +#ifndef _MANIF_MANIF_SGAL3TANGENT_MAP_H_ +#define _MANIF_MANIF_SGAL3TANGENT_MAP_H_ + +#include "manif/impl/sgal3/SGal3Tangent.h" + +namespace manif { +namespace internal { + +//! @brief traits specialization for Eigen Map +template +struct traits, 0> > + : public traits> { + using typename traits>::Scalar; + using traits>::DoF; + using DataType = Eigen::Map, 0>; + using Base = SGal3TangentBase, 0>>; +}; + +//! @brief traits specialization for Eigen Map +template +struct traits, 0> > + : public traits> { + using typename traits>::Scalar; + using traits>::DoF; + using DataType = Eigen::Map, 0>; + using Base = SGal3TangentBase, 0>>; +}; + +} // namespace internal +} // namespace manif + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::SGal3 + */ +template +class Map, 0> + : public manif::SGal3TangentBase, 0> > { + using Base = manif::SGal3TangentBase, 0> >; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + Map(Scalar* coeffs) : data_(coeffs) { } + + MANIF_TANGENT_MAP_ASSIGN_OP(SGal3Tangent) + + DataType& coeffs() { return data_; } + const DataType& coeffs() const { return data_; } + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::SGal3 + */ +template +class Map, 0> + : public manif::SGal3TangentBase, 0> > { + using Base = manif::SGal3TangentBase, 0> >; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + Map(const Scalar* coeffs) : data_(coeffs) { } + + const DataType& coeffs() const { return data_; } + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_SGAL3TANGENT_MAP_H_ diff --git a/include/manif/impl/sgal3/SGal3_base.h b/include/manif/impl/sgal3/SGal3_base.h new file mode 100644 index 00000000..07e7e044 --- /dev/null +++ b/include/manif/impl/sgal3/SGal3_base.h @@ -0,0 +1,484 @@ +#ifndef _MANIF_MANIF_SGAL3_BASE_H_ +#define _MANIF_MANIF_SGAL3_BASE_H_ + +#include "manif/impl/sgal3/SGal3_properties.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/so3/SO3_map.h" +#include "manif/impl/se3/SE3_map.h" + +namespace manif { + +// +// LieGroup +// + +/** + * @brief The base class of the SGal3 group. + * @note See "All About the Galilean Group SGal(3)" J. Kelly. + * https://arxiv.org/abs/2312.07555 + * + */ +template +struct SGal3Base : LieGroupBase<_Derived> { +private: + + using Base = LieGroupBase<_Derived>; + using Type = SGal3Base<_Derived>; + +public: + + MANIF_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_AUTO_API + MANIF_INHERIT_GROUP_OPERATOR + + using Base::coeffs; + + using Rotation = typename internal::traits<_Derived>::Rotation; + using Translation = typename internal::traits<_Derived>::Translation; + using LinearVelocity = typename internal::traits<_Derived>::LinearVelocity; + using Time = Scalar; + using Transformation = Eigen::Matrix; + using Isometry = Eigen::Matrix; /**< Double direct spatial isometry*/ + using QuaternionDataType = Eigen::Quaternion; + + // LieGroup common API + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SGal3Base) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(SGal3Base) + + /** + * @brief Get the inverse. + * @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. + */ + LieGroup inverse(OptJacobianRef J_minv_m = {}) const; + + /** + * @brief Get the SGal3 corresponding Lie algebra element in vector form. + * @param[out] -optional- J_t_m Jacobian of the tangent wrt to this. + * @return The SGal3 tangent of this. + * @note This is the log() map in vector form. + * @see SGal3Tangent. + */ + Tangent log(OptJacobianRef J_t_m = {}) const; + + /** + * @brief This function is deprecated. + * Please considere using + * @ref log instead. + */ + MANIF_DEPRECATED + Tangent lift(OptJacobianRef J_t_m = {}) const; + + /** + * @brief Composition of this and another SGal3 element. + * @param[in] m Another SGal3 element. + * @param[out] -optional- J_mc_ma Jacobian of the composition wrt this. + * @param[out] -optional- J_mc_mb Jacobian of the composition wrt m. + * @return The composition of 'this . m'. + */ + template + LieGroup compose( + const LieGroupBase<_DerivedOther>& m, + OptJacobianRef J_mc_ma = {}, + OptJacobianRef J_mc_mb = {} + ) const; + + /** + * @brief Get the action + * @param[in] p A 3D point. + * @param[out] -optional- J_pout_m The Jacobian of the new object wrt this. + * @param[out] -optional- J_pout_p The Jacobian of the new object wrt input object. + */ + template + Eigen::Matrix + act( + const Eigen::MatrixBase<_EigenDerived> &p, + tl::optional>> J_pout_m = {}, + tl::optional>> J_pout_p = {} + ) const; + + /** + * @brief Get the adjoint matrix of SGal3 at this. + */ + Jacobian adj() const; + + // SGal3 specific functions + + /** + * Get the isometry object (double direct isometry). + * @note T = | R v t| + * | 1 s| + * | 1| + */ + Transformation transform() const; + + /** + * Get the isometry object (double direct isometry). + * @note T = | R v t| + * | 1 s| + * | 1| + */ + Isometry isometry() const; + + /** + * @brief Get the rotational part of this as a rotation matrix. + */ + Rotation rotation() const; + + /** + * @brief Get the rotational part of this as a quaternion. + */ + QuaternionDataType quat() const; + + /** + * @brief Get the translational part in vector form. + */ + Translation translation() const; + + /** + * @brief Get the x component of the translational part. + */ + Scalar x() const; + + /** + * @brief Get the y component of translational part. + */ + Scalar y() const; + + /** + * @brief Get the z component of translational part. + */ + Scalar z() const; + + /** + * @brief Get the linear velocity part in vector form. + */ + LinearVelocity linearVelocity() const; + + /** + * @brief Get the x component of the linear velocity part. + */ + Scalar vx() const; + + /** + * @brief Get the y component of linear velocity part. + */ + Scalar vy() const; + + /** + * @brief Get the z component of linear velocity part. + */ + Scalar vz() const; + + /** + * @brief Get the time. + */ + Scalar t() const; + + /** + * @brief Normalize the underlying quaternion. + */ + void normalize(); + +public: /// @todo make protected + + Eigen::Map> asSO3() const { + return Eigen::Map>(coeffs().data()+3); + } + + Eigen::Map> asSO3() { + return Eigen::Map>(coeffs().data()+3); + } +}; + +template +typename SGal3Base<_Derived>::Transformation +SGal3Base<_Derived>::transform() const { + Eigen::Matrix T; + T.template topLeftCorner<3, 3>() = rotation(); + T.template block<3, 1>(0, 3) = linearVelocity(); + T.template topRightCorner<3, 1>() = translation(); + T.template bottomLeftCorner<2, 3>().setZero(); + T.template bottomRightCorner<2, 2>().setIdentity(); + T(3, 4) = t(); + return T; +} + +template +typename SGal3Base<_Derived>::Isometry +SGal3Base<_Derived>::isometry() const { + return Isometry(transform()); +} + +template +typename SGal3Base<_Derived>::Rotation +SGal3Base<_Derived>::rotation() const { + return asSO3().rotation(); +} + +template +typename SGal3Base<_Derived>::QuaternionDataType +SGal3Base<_Derived>::quat() const { + return asSO3().quat(); +} + +template +typename SGal3Base<_Derived>::Translation +SGal3Base<_Derived>::translation() const { + return coeffs().template head<3>(); +} + +template +typename SGal3Base<_Derived>::LinearVelocity +SGal3Base<_Derived>::linearVelocity() const { + return coeffs().template segment<3>(7); +} + +template +typename SGal3Base<_Derived>::LieGroup +SGal3Base<_Derived>::inverse(OptJacobianRef J_minv_m) const { + if (J_minv_m) { + (*J_minv_m) = -adj(); + } + + const SO3 so3inv = asSO3().inverse(); + + return LieGroup( + -so3inv.act((translation()-t()*linearVelocity())), + so3inv, + -so3inv.act(linearVelocity()), + -t() + ); +} + +template +typename SGal3Base<_Derived>::Tangent +SGal3Base<_Derived>::log(OptJacobianRef J_t_m) const { + const SO3Tangent so3tan = asSO3().log(); + + Eigen::Matrix E; + Tangent::fillE(E, Eigen::Map>(so3tan.data())); + + const LinearVelocity nu = so3tan.ljacinv() * linearVelocity(); + + Tangent tan( + ( + typename Tangent::DataType() << + so3tan.ljacinv() * (translation() - E * (t() * nu)), nu, so3tan.coeffs(), t() + ).finished() + ); + + if (J_t_m) { + // Jr^-1 + (*J_t_m) = tan.rjacinv(); + } + + return tan; +} + +template +typename SGal3Base<_Derived>::Tangent +SGal3Base<_Derived>::lift(OptJacobianRef J_t_m) const { + return log(J_t_m); +} + +template +template +typename SGal3Base<_Derived>::LieGroup +SGal3Base<_Derived>::compose( + const LieGroupBase<_DerivedOther>& m, + OptJacobianRef J_mc_ma, + OptJacobianRef J_mc_mb +) const { + static_assert( + std::is_base_of, _DerivedOther>::value, + "Argument does not inherit from SGal3Base !" + ); + + const auto& m_sgal3 = static_cast&>(m); + + if (J_mc_ma) { + (*J_mc_ma) = m.inverse().adj(); + } + + if (J_mc_mb) { + J_mc_mb->setIdentity(); + } + + return LieGroup( + rotation() * m_sgal3.translation() + m_sgal3.t() * linearVelocity() + translation(), + asSO3() * m_sgal3.asSO3(), + rotation() * m_sgal3.linearVelocity() + linearVelocity(), + t() + m_sgal3.t() + ); +} + +template +template +Eigen::Matrix::Scalar, 3, 1> +SGal3Base<_Derived>::act( + const Eigen::MatrixBase<_EigenDerived> &p, + tl::optional>> J_pout_m, + tl::optional>> J_pout_p +) const { + assert_vector_dim(p, 3); + + const Rotation R(rotation()); + + if (J_pout_m) { + J_pout_m->template topLeftCorner<3, 3>() = R; + J_pout_m->template block<3, 3>(0, 3).setZero(); + J_pout_m->template block<3, 3>(0, 6).noalias() = -R * skew(p); + J_pout_m->template topRightCorner<3, 1>() = linearVelocity(); + } + + if (J_pout_p) { + (*J_pout_p) = R; + } + + return translation() + R * p; +} + + +template +typename SGal3Base<_Derived>::Jacobian +SGal3Base<_Derived>::adj() const { + /// + /// this is + /// Ad(g) = | R -R.tau [(t-v.tau)]x.R v| + /// | 0 R [v]x.R 0| + /// | 0 0 R 0| + /// | 0 0 0 1| + /// + /// considering vee(log(g)) = (rho;v;w;iota) + + Jacobian Adj; + Adj.template topLeftCorner<3, 3>() = rotation(); + Adj.template block<3, 3>(0, 3).noalias() = -t() * Adj.template topLeftCorner<3, 3>(); + Adj.template block<3, 3>(0, 6).noalias() = + skew(translation() - t() * linearVelocity()) * Adj.template topLeftCorner<3, 3>(); + Adj.template topRightCorner<3, 1>() = linearVelocity(); + + Adj.template block<3, 3>(3, 3) = Adj.template topLeftCorner<3, 3>(); + Adj.template block<3, 3>(3, 6).noalias() = + skew(linearVelocity()) * Adj.template topLeftCorner<3, 3>(); + + Adj.template block<3, 3>(6, 6) = Adj.template topLeftCorner<3, 3>(); + + Adj.template bottomLeftCorner<7, 3>().setZero(); + Adj.template block<4, 3>(6, 3).setZero(); + Adj.template block<1, 3>(9, 6).setZero(); + Adj.template block<6, 1>(3, 9).setZero(); + + Adj(9, 9) = Scalar(1); + + return Adj; +} + +// SGal3 specific function + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::x() const { + return coeffs()(0); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::y() const { + return coeffs()(1); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::z() const { + return coeffs()(2); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::vx() const { + return coeffs()(7); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::vy() const { + return coeffs()(8); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::vz() const { + return coeffs()(9); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::t() const { + return coeffs()(10); +} + +template +void SGal3Base<_Derived>::normalize() { + coeffs().template segment<4>(3).normalize(); +} + +namespace internal { + +//! @brief Random specialization for SGal3Base objects. +template +struct RandomEvaluatorImpl> { + template + static void run(T& m) { + using Scalar = typename SGal3Base::Scalar; + using LieGroup = typename SGal3Base::LieGroup; + + typename LieGroup::DataType data = LieGroup::DataType::Random(); + data.template segment<4>(3) = randQuat().coeffs(); + + m = LieGroup(data); + } +}; + +//! @brief Assignment assert specialization for SGal3Base objects +template +struct AssignmentEvaluatorImpl> { + template + static void run_impl(const T& data) { + using std::abs; + MANIF_ASSERT( + abs(data.template segment<4>(3).norm()-typename SGal3Base::Scalar(1)) < + Constants::Scalar>::eps, + "SGal3 assigned data not normalized !", + manif::invalid_argument + ); + MANIF_UNUSED_VARIABLE(data); + } +}; + +//! @brief Cast specialization for SGal3Base objects. +template +struct CastEvaluatorImpl, NewScalar> { + template + static auto run(const T& o) -> typename Derived::template LieGroupTemplate { + return typename Derived::template LieGroupTemplate( + o.translation().template cast(), + o.quat().template cast().normalized(), + o.linearVelocity().template cast(), + NewScalar(o.t()) + ); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3_BASE_H_ diff --git a/include/manif/impl/sgal3/SGal3_map.h b/include/manif/impl/sgal3/SGal3_map.h new file mode 100644 index 00000000..6f28b9b7 --- /dev/null +++ b/include/manif/impl/sgal3/SGal3_map.h @@ -0,0 +1,84 @@ +#ifndef _MANIF_MANIF_SGAL3_MAP_H_ +#define _MANIF_MANIF_SGAL3_MAP_H_ + +#include "manif/impl/sgal3/SGal3.h" + +namespace manif { +namespace internal { + +//! @brief traits specialization for Eigen Map +template +struct traits, 0> > : public traits> { + using typename traits>::Scalar; + using traits>::RepSize; + using Base = SGal3Base, 0>>; + using DataType = Eigen::Map, 0>; +}; + +//! @brief traits specialization for Eigen Map const +template +struct traits,0> > + : public traits> { + using typename traits>::Scalar; + using traits>::RepSize; + using Base = SGal3Base, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::SGal3 + */ +template +class Map, 0> + : public manif::SGal3Base, 0> > { + using Base = manif::SGal3Base, 0> >; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + using Base::rotation; + + Map(Scalar* coeffs) : data_(coeffs) { } + + MANIF_GROUP_MAP_ASSIGN_OP(SGal3) + + DataType& coeffs() { return data_; } + const DataType& coeffs() const { return data_; } + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::SGal3 + */ +template +class Map, 0> + : public manif::SGal3Base, 0> > { + using Base = manif::SGal3Base, 0> >; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + // using Base::rotation; + + Map(const Scalar* coeffs) : data_(coeffs) { } + + const DataType& coeffs() const { return data_; } + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_SGAL3_MAP_H_ diff --git a/include/manif/impl/sgal3/SGal3_properties.h b/include/manif/impl/sgal3/SGal3_properties.h new file mode 100644 index 00000000..3b30035d --- /dev/null +++ b/include/manif/impl/sgal3/SGal3_properties.h @@ -0,0 +1,31 @@ +#ifndef _MANIF_MANIF_SGAL3_PROPERTIES_H_ +#define _MANIF_MANIF_SGAL3_PROPERTIES_H_ + +#include "manif/impl/traits.h" + +namespace manif { + +// Forward declaration +template struct SGal3Base; +template struct SGal3TangentBase; + +namespace internal { + +//! traits specialization +template +struct LieGroupProperties> { + static constexpr int Dim = 3; /// @brief Space dimension + static constexpr int DoF = 10; /// @brief Degrees of freedom +}; + +//! traits specialization +template +struct LieGroupProperties> { + static constexpr int Dim = 3; /// @brief Space dimension + static constexpr int DoF = 10; /// @brief Degrees of freedom +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3_PROPERTIES_H_ diff --git a/include/manif/impl/so2/SO2Tangent_base.h b/include/manif/impl/so2/SO2Tangent_base.h index 8811c401..8d03c605 100644 --- a/include/manif/impl/so2/SO2Tangent_base.h +++ b/include/manif/impl/so2/SO2Tangent_base.h @@ -225,6 +225,15 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SO2TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(1, 0); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/so2/SO2_base.h b/include/manif/impl/so2/SO2_base.h index 82919522..ea1506ab 100644 --- a/include/manif/impl/so2/SO2_base.h +++ b/include/manif/impl/so2/SO2_base.h @@ -340,6 +340,15 @@ struct AssignmentEvaluatorImpl> } }; +//! @brief Cast specialization for SO2Base objects. +template +struct CastEvaluatorImpl, NewScalar> { + template + static auto run(const T& o) -> typename Derived::template LieGroupTemplate { + return typename Derived::template LieGroupTemplate(NewScalar(o.angle())); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/so3/SO3Tangent_base.h b/include/manif/impl/so3/SO3Tangent_base.h index c14939d1..de9f7a3c 100644 --- a/include/manif/impl/so3/SO3Tangent_base.h +++ b/include/manif/impl/so3/SO3Tangent_base.h @@ -327,6 +327,15 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SO3TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(2, 1), v(0, 2), v(1, 0); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/so3/SO3_base.h b/include/manif/impl/so3/SO3_base.h index 303c22c9..308649b5 100644 --- a/include/manif/impl/so3/SO3_base.h +++ b/include/manif/impl/so3/SO3_base.h @@ -403,7 +403,7 @@ struct RandomEvaluatorImpl> } }; -//! @brief Assignment assert specialization for SE2Base objects +//! @brief Assignment assert specialization for SO3Base objects template struct AssignmentEvaluatorImpl> { @@ -421,6 +421,19 @@ struct AssignmentEvaluatorImpl> } }; +//! @brief Cast specialization for SO3Base objects. +template +struct CastEvaluatorImpl, NewScalar> { + template + static auto run(const T& o) -> typename Derived::template LieGroupTemplate { + const typename SO3Base::QuaternionDataType q = o.quat(); + + return typename Derived::template LieGroupTemplate( + q.template cast().normalized() + ); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/tangent_base.h b/include/manif/impl/tangent_base.h index ab5ac214..6d5178a4 100644 --- a/include/manif/impl/tangent_base.h +++ b/include/manif/impl/tangent_base.h @@ -5,6 +5,8 @@ #include "manif/impl/traits.h" #include "manif/impl/generator.h" #include "manif/impl/random.h" +#include "manif/impl/bracket.h" +#include "manif/impl/vee.h" #include "manif/impl/eigen.h" #include "manif/constants.h" @@ -98,6 +100,14 @@ struct TangentBase */ _Derived& setRandom(); + /** + * @brief Set the Tangent object this from an object in the Lie algebra. + * @param[in] a An object in the Lie algebra. + * @return A reference to this. + */ + template + _Derived& setVee(const Eigen::MatrixBase<_EigenDerived>& a); + // Minimum API // Those functions must be implemented in the Derived class ! @@ -255,6 +265,16 @@ struct TangentBase */ Jacobian smallAdj() const; + /** + * @brief Compute the Lie bracket [this,b] in vector form. + * + * @tparam _DerivedOther + * @param b Another tangent object of the same group. + * @return The Lie bracket [this,b] in vector form. + */ + template + Tangent bracket(const TangentBase<_DerivedOther>& b) const; + /** * @brief Evaluate whether this and v are 'close'. * @details This evaluation is performed element-wise. @@ -320,6 +340,21 @@ struct TangentBase //! @brief Divide the underlying vector with a scalar. Tangent operator /=(const Scalar scalar); + //! Access the ith coeffs + auto operator [](const unsigned int i) const -> decltype(coeffs()[i]){ + return coeffs()[i]; + } + + //! Access the ith coeffs + auto operator [](const unsigned int i) -> decltype(coeffs()[i]){ + return coeffs()[i]; + } + + //! @brief The size of the underlying vector + constexpr unsigned int size() const { + return RepSize; + } + // static helpers //! Static helper the create a Tangent object set to Zero. @@ -331,6 +366,30 @@ struct TangentBase //! Static helper to get a Basis of the Lie group. static InnerWeightsMatrix InnerWeights(); + /** + * @brief Compute the Lie bracket [a,b] in vector form. + * + * @tparam _DerivedOther + * @param a A Tangent object. + * @param b A second Tangent object. + * @return The Lie bracket [a,b] in vector form. + */ + template + static Tangent Bracket( + const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b + ); + + /** + * @brief Instantiate a Tangent from a Lie algebra object. + * + * @tparam _EigenDerived + * @param alg A tangent object expressed in the Lie algebra. + * @return a Tangent object. + * @see hat + */ + template + static Tangent Vee(const Eigen::MatrixBase<_EigenDerived>& alg); + protected: inline _Derived& derived() & noexcept { return *static_cast< _Derived* >(this); } @@ -425,6 +484,13 @@ _Derived& TangentBase<_Derived>::setRandom() return derived(); } +template +template +_Derived& TangentBase<_Derived>::setVee(const Eigen::MatrixBase<_EigenDerived>& a) { + internal::VeeEvaluator::Base>(derived()).run(a); + return derived(); +} + template typename TangentBase<_Derived>::LieGroup TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const @@ -607,6 +673,17 @@ TangentBase<_Derived>::smallAdj() const return derived().smallAdj(); } +template +template +typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::bracket( + const TangentBase<_DerivedOther>& b +) const { + return internal::BracketEvaluator< + typename internal::traits<_Derived>::Base, + typename internal::traits<_DerivedOther>::Base + >(derived(), b.derived()).run(); +} + template template bool TangentBase<_Derived>::isApprox( @@ -680,6 +757,22 @@ TangentBase<_Derived>::InnerWeights() typename internal::traits<_Derived>::Base>::run(); } +template +template +typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Bracket( + const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b +) { + return a.bracket(b); +} + +template +template +typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Vee( + const Eigen::MatrixBase<_EigenDerived>& alg +) { + return Tangent().setVee(alg); +} + // Math template diff --git a/include/manif/impl/traits.h b/include/manif/impl/traits.h index 93124a82..17a7c704 100644 --- a/include/manif/impl/traits.h +++ b/include/manif/impl/traits.h @@ -1,9 +1,14 @@ #ifndef _MANIF_MANIF_TRAITS_H_ #define _MANIF_MANIF_TRAITS_H_ +#include #include namespace manif { + +template struct LieGroupBase; +template struct TangentBase; + namespace internal { //template @@ -39,13 +44,17 @@ struct traitscast; /** * @brief Traits to change the scalar type of a template class - * @note given using FooDouble = Foo + * @note given using FooDouble = Foo * using FooFloat = typename traitscast::cast; */ -template