├── .codecov.yml ├── .github └── workflows │ ├── ci.yml │ ├── docs.yml │ └── release.yml ├── .gitignore ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── appveyor.yml ├── benchmark ├── CMakeLists.txt ├── benchmark_manif.cpp ├── benchmark_quat.cpp ├── benchmark_rn.cpp ├── benchmark_se2.cpp ├── benchmark_se3.cpp ├── benchmark_se_2_3.cpp ├── benchmark_so2.cpp ├── benchmark_so3.cpp └── common_benchmark.h ├── cmake ├── manifConfig.cmake.in └── modules │ └── FindEigen3.cmake ├── docs ├── .custom_wordlist.txt ├── .gitignore ├── .sphinx │ ├── .markdownlint.json │ ├── .pre-commit-config.yaml │ ├── .wordlist.txt │ ├── _static │ │ ├── favicon copy.png │ │ ├── favicon.png │ │ └── project_specific.css │ ├── _templates │ │ └── header.html │ ├── get_vale_conf.py │ ├── metrics │ │ └── source_metrics.sh │ ├── pa11y.json │ ├── requirements.txt │ └── spellingcheck.yaml ├── Doxyfile ├── Makefile ├── conf.py ├── explanation │ ├── autodiff.md │ ├── index.md │ ├── operations.md │ ├── projects.md │ ├── publication.md │ └── tangent.md ├── howto │ ├── ceres.md │ ├── documentation.md │ ├── index.md │ └── tmpl-code.md ├── index.md ├── reference │ ├── api-cpp.md │ ├── api-python.md │ ├── examples-cpp.md │ ├── examples-python.md │ └── index.md ├── reuse │ └── links.txt └── tutorial │ ├── cpp.md │ ├── index.md │ └── python.md ├── examples ├── CMakeLists.txt ├── bundle_sam.cpp ├── scripts │ └── plot_interpolation.m ├── se2_DeCasteljau.cpp ├── se2_average.cpp ├── se2_interpolation.cpp ├── se2_localization.cpp ├── se2_localization.py ├── se2_localization_ukfm.cpp ├── se2_points_generator.h ├── se2_sam.cpp ├── se2_sam.py ├── se3_localization.cpp ├── se3_localization.py ├── se3_localization_iekf.cpp ├── se3_sam.cpp ├── se3_sam.py ├── se3_sam_selfcalib.cpp ├── se3_sam_selfcalib.py ├── se_2_3_localization.cpp ├── se_2_3_localization.py └── so2_average.cpp ├── external └── tl │ └── tl │ └── optional.hpp ├── include └── manif │ ├── Bundle.h │ ├── Rn.h │ ├── SE2.h │ ├── SE3.h │ ├── SE_2_3.h │ ├── SGal3.h │ ├── SO2.h │ ├── SO3.h │ ├── algorithms │ ├── average.h │ ├── bezier.h │ ├── decasteljau.h │ └── interpolation.h │ ├── autodiff │ ├── autodiff.h │ ├── constants.h │ └── local_parameterization.h │ ├── ceres │ ├── ceres.h │ ├── ceres_utils.h │ ├── constants.h │ ├── constraint.h │ ├── local_parametrization.h │ ├── manifold.h │ └── objective.h │ ├── constants.h │ ├── functions.h │ ├── impl │ ├── assignment_assert.h │ ├── bracket.h │ ├── bundle │ │ ├── Bundle.h │ │ ├── BundleTangent.h │ │ ├── BundleTangent_base.h │ │ ├── BundleTangent_map.h │ │ ├── Bundle_base.h │ │ ├── Bundle_map.h │ │ └── Bundle_properties.h │ ├── cast.h │ ├── eigen.h │ ├── generator.h │ ├── lie_group_base.h │ ├── macro.h │ ├── random.h │ ├── rn │ │ ├── Rn.h │ │ ├── RnTangent.h │ │ ├── RnTangent_base.h │ │ ├── RnTangent_map.h │ │ ├── Rn_base.h │ │ ├── Rn_map.h │ │ └── Rn_properties.h │ ├── se2 │ │ ├── SE2.h │ │ ├── SE2Tangent.h │ │ ├── SE2Tangent_base.h │ │ ├── SE2Tangent_map.h │ │ ├── SE2_base.h │ │ ├── SE2_map.h │ │ └── SE2_properties.h │ ├── se3 │ │ ├── SE3.h │ │ ├── SE3Tangent.h │ │ ├── SE3Tangent_base.h │ │ ├── SE3Tangent_map.h │ │ ├── SE3_base.h │ │ ├── SE3_map.h │ │ └── SE3_properties.h │ ├── se_2_3 │ │ ├── SE_2_3.h │ │ ├── SE_2_3Tangent.h │ │ ├── SE_2_3Tangent_base.h │ │ ├── SE_2_3Tangent_map.h │ │ ├── SE_2_3_base.h │ │ ├── SE_2_3_map.h │ │ └── SE_2_3_properties.h │ ├── sgal3 │ │ ├── SGal3.h │ │ ├── SGal3Tangent.h │ │ ├── SGal3Tangent_base.h │ │ ├── SGal3Tangent_map.h │ │ ├── SGal3_base.h │ │ ├── SGal3_map.h │ │ └── SGal3_properties.h │ ├── so2 │ │ ├── SO2.h │ │ ├── SO2Tangent.h │ │ ├── SO2Tangent_base.h │ │ ├── SO2Tangent_map.h │ │ ├── SO2_base.h │ │ ├── SO2_map.h │ │ └── SO2_properties.h │ ├── so3 │ │ ├── SO3.h │ │ ├── SO3Tangent.h │ │ ├── SO3Tangent_base.h │ │ ├── SO3Tangent_map.h │ │ ├── SO3_base.h │ │ ├── SO3_map.h │ │ └── SO3_properties.h │ ├── tangent_base.h │ ├── traits.h │ ├── utils.h │ └── vee.h │ └── manif.h ├── package.xml ├── paper ├── Lie_theory_cheat_sheet.pdf ├── paper.bib └── paper.md ├── pyproject.toml ├── python ├── CMakeLists.txt ├── bindings_lie_group_base.h ├── bindings_manif.cpp ├── bindings_optional.h ├── bindings_rn.cpp ├── bindings_se2.cpp ├── bindings_se3.cpp ├── bindings_se_2_3.cpp ├── bindings_sgal3.cpp ├── bindings_so2.cpp ├── bindings_so3.cpp └── bindings_tangent_base.h ├── setup.cfg ├── setup.py └── test ├── CMakeLists.txt ├── autodiff ├── CMakeLists.txt ├── gtest_autodiff.cpp ├── gtest_autodiff_reverse.cpp └── gtest_common_tester_autodiff.h ├── bundle ├── CMakeLists.txt ├── gtest_bundle.cpp ├── gtest_bundle_large.cpp └── gtest_bundle_single_group.cpp ├── ceres ├── CMakeLists.txt ├── ceres_test_utils.h ├── gtest_bundle_ceres.cpp ├── gtest_rn_ceres.cpp ├── gtest_se23_ceres.cpp ├── gtest_se2_ceres.cpp ├── gtest_se2_ceres_autodiff.cpp ├── gtest_se3_ceres.cpp ├── gtest_sgal3_ceres.cpp ├── gtest_so2_ceres.cpp └── gtest_so3_ceres.cpp ├── common_tester.h ├── gtest └── CMakeLists.txt.in ├── gtest_eigen_utils.h ├── gtest_manif_utils.h ├── gtest_misc.cpp ├── python ├── CMakeLists.txt ├── test_manif.py ├── test_se2.py ├── test_se3.py ├── test_se_2_3.py ├── test_sgal3.py ├── test_so2.py └── test_so3.py ├── rn ├── CMakeLists.txt └── gtest_rn.cpp ├── se2 ├── CMakeLists.txt ├── gtest_se2.cpp ├── gtest_se2_map.cpp ├── gtest_se2_tangent.cpp └── gtest_se2_tangent_map.cpp ├── se3 ├── CMakeLists.txt └── gtest_se3.cpp ├── se_2_3 ├── CMakeLists.txt └── gtest_se_2_3.cpp ├── sgal3 ├── CMakeLists.txt └── gtest_sgal3.cpp ├── so2 ├── CMakeLists.txt ├── gtest_so2.cpp ├── gtest_so2_map.cpp ├── gtest_so2_tangent.cpp └── gtest_so2_tangent_map.cpp ├── so3 ├── CMakeLists.txt └── gtest_so3.cpp └── test_func.h /.codecov.yml: -------------------------------------------------------------------------------- 1 | codecov: 2 | notify: 3 | require_ci_to_pass: yes 4 | 5 | coverage: 6 | precision: 2 7 | round: down 8 | range: "70...100" 9 | 10 | comment: 11 | layout: "header, diff" 12 | behavior: default 13 | require_changes: false 14 | 15 | ignore: 16 | - "external" 17 | - "test" 18 | -------------------------------------------------------------------------------- /.github/workflows/release.yml: -------------------------------------------------------------------------------- 1 | name: release 2 | on: 3 | push: 4 | tags: 5 | - '*' 6 | pull_request: 7 | branches: 8 | - devel # master only when ready 9 | - master 10 | workflow_dispatch: 11 | 12 | jobs: 13 | 14 | build-sdist: 15 | runs-on: ubuntu-latest 16 | steps: 17 | - name: Checkout 18 | uses: actions/checkout@v4 19 | - run: git fetch --prune --unshallow 20 | 21 | - name: Install Python 22 | uses: actions/setup-python@v5 23 | with: 24 | python-version: 3.12 25 | 26 | - name: Setup apt 27 | run: | 28 | sudo apt update 29 | sudo apt install -y libeigen3-dev 30 | 31 | - name: Setup 32 | run: | 33 | python3 -m pip install --upgrade pip 34 | pip3 install build 35 | 36 | - name: Build sdist 37 | run: python3 -m build --sdist -o dist/ 38 | 39 | - name: Build wheel 40 | run: python3 -m build --wheel -o dist/ 41 | 42 | - name: Upload artifacts 43 | uses: actions/upload-artifact@v4 44 | with: 45 | name: dist 46 | path: dist/* 47 | # path: | 48 | # path/*.whl 49 | # path/*.tar.gz 50 | 51 | upload_pypi: 52 | needs: build-sdist 53 | runs-on: ubuntu-latest 54 | steps: 55 | 56 | - uses: actions/download-artifact@v4 57 | with: 58 | name: dist 59 | path: dist 60 | 61 | - name: Inspect dist folder 62 | run: ls -lah dist/ 63 | 64 | # @todo: see https://github.com/diegoferigo/manif/pull/1#discussion_r668531581 65 | # - uses: pypa/gh-action-pypi-publish@master 66 | # if: | 67 | # github.repository == 'artivis/manif' && 68 | # ((github.event_name == 'release' && github.event.action == 'published') || 69 | # (github.event_name == 'push' && github.ref == 'refs/heads/main')) 70 | # with: 71 | # user: __token__ 72 | # password: ${{ secrets.PYPI_TOKEN }} 73 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.user 2 | .cproject 3 | .project 4 | .settings 5 | .tags 6 | build* 7 | docs/xml 8 | docs/html 9 | docs/site 10 | docs/m.css 11 | *doxygen_warnings.txt 12 | *.cache 13 | .vscode 14 | *.egg-info 15 | *.so 16 | *__pycache__ 17 | .pytest_cache 18 | dist 19 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing 2 | 3 | ## General guidelines 4 | 5 | **manif** is developed according to Vincent Driessen's [Gitflow Workflow][git-workflow]. 6 | This means, 7 | 8 | - the `master` branch is for releases only. 9 | - development is done on feature branches. 10 | - finished features are integrated via PullRequests into the branch `devel`. 11 | 12 | For a PullRequest to get merged into `devel`, it must pass 13 | 14 | - Review by one of the maintainers. 15 | - Are the changes introduced in scope of **manif**? 16 | - Is the documentation updated? 17 | - Are enough reasonable tests added? 18 | - Will these changes break the API? 19 | - Do the new changes follow the current style of naming? 20 | - Compile / Test / Run on all target environments. 21 | 22 | Note: The test suite detailed below is run in CI for many targets environments including, 23 | 24 | - Ubuntu 16.04/18.04/20.04 25 | - MacOS 10.15 26 | - Visual Studio 15 27 | 28 | ## Development environment 29 | 30 | We will detail here how to set up a development environment. 31 | It is recommended to use containers if you do not want to install the dependencies on your host. 32 | You may refer to [this blog post](lxd-post) to set up a LXD container. 33 | 34 | First let us clone the **manif** repo, 35 | 36 | ```terminal 37 | git clone https://github.com/artivis/manif.git 38 | cd manif 39 | ``` 40 | 41 | Let's install all dependencies for development and testing, 42 | 43 | ```terminal 44 | apt install libeigen3-dev 45 | python3 -m pip install "pybind11[global]" pytest numpy 46 | ``` 47 | 48 | We can now build **manif**, its Python wrappers and all tests, 49 | 50 | ```terminal 51 | mkdir build && cd build 52 | cmake -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_BINDINGS=ON -DBUILD_EXAMPLES=ON .. 53 | make 54 | ``` 55 | 56 | To run the C++ tests execute the following, 57 | 58 | ```terminal 59 | ctest --output-on-failure 60 | ``` 61 | 62 | To run the Python tests, 63 | 64 | ```terminal 65 | cd manif 66 | pytest 67 | ``` 68 | 69 | [//]: # (URLs) 70 | 71 | [git-workflow]: http://nvie.com/posts/a-successful-git-branching-model 72 | [lxd-post]: https://artivis.github.io/post/2020/lxc -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Jeremie Deray 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | branches: 2 | only: 3 | - master 4 | - devel 5 | 6 | skip_commits: 7 | files: 8 | - '**/.doxygen.txt' 9 | - '**/.gitignore' 10 | - '**/.travis.yml' 11 | - '**/*.md' 12 | - '**/LICENSE.txt' 13 | 14 | os: Visual Studio 2015 15 | 16 | environment: 17 | matrix: 18 | # @TODO Does not compile on VS14 19 | # - generator: "Visual Studio 14 Win64" 20 | # configuration: Release 21 | # build_tests: ON 22 | # build_ceres_tests: OFF 23 | # build_examples: ON 24 | 25 | - generator: "Visual Studio 15 Win64" 26 | APPVEYOR_BUILD_WORKER_IMAGE: Visual Studio 2017 27 | configuration: Release 28 | build_tests: ON 29 | build_ceres_tests: OFF 30 | build_examples: ON 31 | 32 | # @TODO Disabled until Ceres install is fixed 33 | # - generator: "Visual Studio 15 Win64" 34 | # APPVEYOR_BUILD_WORKER_IMAGE: Visual Studio 2017 35 | # configuration: Release 36 | # build_tests: ON 37 | # build_ceres_tests: ON 38 | # build_examples: ON 39 | 40 | clone_folder: c:\projects\manif 41 | 42 | build: 43 | project: c:\projects\manif\build\manif.sln 44 | 45 | install: 46 | - ps: | 47 | Write-Output "Generator: $env:generator" 48 | Write-Output "Env:Configuation: $env:configuration" 49 | if (Test-Path env:APPVEYOR_PULL_REQUEST_NUMBER) { 50 | Write-Output "This is a pull request build" 51 | } 52 | 53 | # Get Eigen 3.3-beta1 54 | - ps: wget -O eigen3.zip https://gitlab.com/libeigen/eigen/-/archive/3.3-beta1/eigen-3.3-beta1.zip 55 | - cmd: 7z x eigen3.zip -o"C:\projects" -y > nul 56 | # - cmd: $env:CMAKE_INCLUDE_PATH = "C:\projects\eigen-eigen-ce5a455b34c0;$env:CMAKE_INCLUDE_PATH" 57 | 58 | build_script: 59 | # Install Ceres. 60 | # @TODO does not work atm, Ceres does not find Eigen 61 | - ps: | 62 | if ($env:build_ceres_tests -eq "ON") 63 | { 64 | Write-Output "Building Ceres." 65 | cd c:\projects 66 | git clone --branch=1.14.0 --single-branch git://github.com/ceres-solver/ceres-solver.git ceres-solver 67 | cd ceres-solver 68 | md _build 69 | cd _build 70 | $eigen_include = "-DEIGEN3_INCLUDE_DIR=C:\projects\eigen-3.3-beta1" 71 | cmake -G "$env:generator" $eigen_include -DMINIGLOG=ON -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF .. 72 | cmake --build . --config $env:configuration --target install 73 | } 74 | 75 | # Build manif 76 | - ps: | 77 | cd c:\projects\manif 78 | md _build 79 | cd _build 80 | $btests = "-DBUILD_TESTING=$env:build_tests" 81 | $bexamples = "-DBUILD_EXAMPLES=$env:build_examples" 82 | $eigen_include = "-DEIGEN3_INCLUDE_DIR=C:\projects\eigen-3.3-beta1" 83 | cmake -G "$env:generator" $eigen_include $btests $bexamples .. 84 | cmake --build . --config $env:configuration 85 | 86 | test_script: 87 | - ps: ctest -VV -C $env:configuration --output-on-failure 88 | 89 | notifications: 90 | - provider: Email 91 | to: deray.jeremie@gmail.com 92 | on_build_success: false 93 | on_build_failure: true 94 | on_build_status_changed: false 95 | -------------------------------------------------------------------------------- /benchmark/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(benchmark QUIET) 2 | 3 | if(NOT benchmark_FOUND) 4 | # If not found, download it 5 | include(FetchContent) 6 | FetchContent_Declare( 7 | googlebenchmark 8 | GIT_REPOSITORY https://github.com/google/benchmark.git 9 | GIT_TAG v1.5.2 10 | ) 11 | set(BENCHMARK_ENABLE_TESTING OFF) 12 | FetchContent_MakeAvailable(googlebenchmark) 13 | endif() 14 | 15 | # small helper function 16 | function(manif_add_benchmark target) 17 | add_executable(${target} ${ARGN}) 18 | # add_dependencies(${target} gtest) 19 | target_link_libraries(${target} ${PROJECT_NAME} benchmark::benchmark) 20 | 21 | if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") 22 | # GCC is not strict enough by default, so enable most of the warnings. 23 | target_compile_options(${target} PRIVATE 24 | -Werror=all 25 | -Werror=extra 26 | # When the library is built using GCC it is necessary to link 27 | # with the pthread library due to how GCC implements std::thread. 28 | # See https://github.com/google/benchmark#platform-specific-build-instructions 29 | -pthread 30 | ) 31 | endif() 32 | endfunction() 33 | 34 | manif_add_benchmark(benchmark_so2 benchmark_so2.cpp) 35 | manif_add_benchmark(benchmark_se2 benchmark_se2.cpp) 36 | manif_add_benchmark(benchmark_so3 benchmark_so3.cpp) 37 | manif_add_benchmark(benchmark_se3 benchmark_se3.cpp) 38 | manif_add_benchmark(benchmark_se_2_3 benchmark_se_2_3.cpp) 39 | manif_add_benchmark(benchmark_rn benchmark_rn.cpp) 40 | 41 | manif_add_benchmark(benchmark_manif benchmark_manif.cpp) 42 | 43 | manif_add_benchmark(benchmark_quat benchmark_quat.cpp) 44 | 45 | set(CXX_17_TEST_TARGETS 46 | benchmark_so2 47 | benchmark_se2 48 | benchmark_so3 49 | benchmark_se3 50 | benchmark_se_2_3 51 | benchmark_rn 52 | 53 | benchmark_manif 54 | 55 | benchmark_quat 56 | ) 57 | 58 | # Set required C++17 flag 59 | set_property(TARGET ${CXX_17_TEST_TARGETS} PROPERTY CXX_STANDARD 17) 60 | set_property(TARGET ${CXX_17_TEST_TARGETS} PROPERTY CXX_STANDARD_REQUIRED ON) 61 | set_property(TARGET ${CXX_17_TEST_TARGETS} PROPERTY CXX_EXTENSIONS OFF) 62 | -------------------------------------------------------------------------------- /benchmark/benchmark_manif.cpp: -------------------------------------------------------------------------------- 1 | #include "common_benchmark.h" 2 | 3 | #include 4 | 5 | using namespace manif; 6 | 7 | MANIF_BENCHMARK(R5d) 8 | MANIF_BENCHMARK(SO2d) 9 | MANIF_BENCHMARK(SE2d) 10 | MANIF_BENCHMARK(SO3d) 11 | MANIF_BENCHMARK(SE3d) 12 | MANIF_BENCHMARK(SE_2_3d) 13 | 14 | BENCHMARK_MAIN(); -------------------------------------------------------------------------------- /benchmark/benchmark_rn.cpp: -------------------------------------------------------------------------------- 1 | #include "common_benchmark.h" 2 | 3 | #include 4 | 5 | using namespace manif; 6 | 7 | MANIF_BENCHMARK(R5d) 8 | 9 | BENCHMARK_MAIN(); -------------------------------------------------------------------------------- /benchmark/benchmark_se2.cpp: -------------------------------------------------------------------------------- 1 | #include "common_benchmark.h" 2 | 3 | #include 4 | 5 | using namespace manif; 6 | 7 | MANIF_BENCHMARK(SE2d) 8 | 9 | BENCHMARK_MAIN(); -------------------------------------------------------------------------------- /benchmark/benchmark_se3.cpp: -------------------------------------------------------------------------------- 1 | #include "common_benchmark.h" 2 | 3 | #include 4 | 5 | using namespace manif; 6 | 7 | MANIF_BENCHMARK(SE3d) 8 | 9 | BENCHMARK_MAIN(); -------------------------------------------------------------------------------- /benchmark/benchmark_se_2_3.cpp: -------------------------------------------------------------------------------- 1 | #include "common_benchmark.h" 2 | 3 | #include 4 | 5 | using namespace manif; 6 | 7 | MANIF_BENCHMARK(SE_2_3d) 8 | 9 | BENCHMARK_MAIN(); -------------------------------------------------------------------------------- /benchmark/benchmark_so2.cpp: -------------------------------------------------------------------------------- 1 | #include "common_benchmark.h" 2 | 3 | #include 4 | 5 | using namespace manif; 6 | 7 | MANIF_BENCHMARK(SO2d) 8 | 9 | BENCHMARK_MAIN(); -------------------------------------------------------------------------------- /benchmark/benchmark_so3.cpp: -------------------------------------------------------------------------------- 1 | #include "common_benchmark.h" 2 | 3 | #include 4 | 5 | using namespace manif; 6 | 7 | MANIF_BENCHMARK(SO3d) 8 | 9 | BENCHMARK_MAIN(); -------------------------------------------------------------------------------- /cmake/manifConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include(CMakeFindDependencyMacro) 4 | @Eigen3_DEPENDENCY@ 5 | @tl-optional_DEPENDENCY@ 6 | 7 | if(NOT TARGET @PROJECT_NAME@) 8 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") 9 | endif() 10 | 11 | # Explicitly set ${PROJECT_NAME}_INCLUDE_DIRS 12 | get_target_property(@PROJECT_NAME@_interface_includes 13 | @PROJECT_NAME_CAPS@::@PROJECT_NAME@ INTERFACE_INCLUDE_DIRECTORIES) 14 | 15 | set(@PROJECT_NAME@_INCLUDE_DIRS ${@PROJECT_NAME@_interface_includes}) 16 | 17 | set(@PROJECT_NAME@_LIBRARY ${@PROJECT_NAME@_LIBRARY}) 18 | set(@PROJECT_NAME@_LIBRARIES ${@PROJECT_NAME@_LIBRARY}) 19 | 20 | check_required_components("@PROJECT_NAME@") 21 | -------------------------------------------------------------------------------- /docs/.custom_wordlist.txt: -------------------------------------------------------------------------------- 1 | # Leave a blank line at the end of this file to support concatenation 2 | cjk 3 | cryptographically 4 | dvipng 5 | fonts 6 | freefont 7 | github 8 | GPG 9 | gyre 10 | https 11 | lang 12 | latexmk 13 | otf 14 | plantuml 15 | tex 16 | texlive 17 | TOC 18 | utils 19 | WCAG 20 | xetex 21 | xindy 22 | kustom 23 | wordlist 24 | txt 25 | -------------------------------------------------------------------------------- /docs/.gitignore: -------------------------------------------------------------------------------- 1 | # Environment 2 | *env*/ 3 | .sphinx/venv/ 4 | 5 | # Sphinx 6 | .sphinx/warnings.txt 7 | .sphinx/.wordlist.dic 8 | .sphinx/.doctrees/ 9 | .sphinx/node_modules/ 10 | 11 | # Vale 12 | .sphinx/styles/* 13 | .sphinx/vale.ini 14 | 15 | # Build outputs 16 | _build* 17 | 18 | # Node.js 19 | package*.json 20 | 21 | # Unrelated cache and config files 22 | .DS_Store 23 | __pycache__ 24 | .idea/ 25 | .vscode/ 26 | 27 | api/ 28 | -------------------------------------------------------------------------------- /docs/.sphinx/.markdownlint.json: -------------------------------------------------------------------------------- 1 | { 2 | "default": false, 3 | "MD003": { 4 | "style": "atx" 5 | }, 6 | "MD013": { 7 | "code_blocks": false, 8 | "tables": false, 9 | "stern": true, 10 | "line_length": 150 11 | }, 12 | "MD014": true, 13 | "MD018": true, 14 | "MD022": true, 15 | "MD023": true, 16 | "MD026": { 17 | "punctuation": ".,;。，；" 18 | }, 19 | "MD031": { 20 | "list_items": false 21 | }, 22 | "MD032": true, 23 | "MD035": true, 24 | "MD042": true, 25 | "MD045": true, 26 | "MD052": true 27 | } -------------------------------------------------------------------------------- /docs/.sphinx/.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: local 3 | hooks: 4 | - id: make-spelling 5 | name: Run make spelling 6 | entry: make -C docs spelling 7 | language: system 8 | pass_filenames: false 9 | files: ^docs/.*\.(rst|md|txt)$ 10 | 11 | - id: make-linkcheck 12 | name: Run make linkcheck 13 | entry: make -C docs linkcheck 14 | language: system 15 | pass_filenames: false 16 | files: ^docs/.*\.(rst|md|txt)$ 17 | 18 | - id: make-woke 19 | name: Run make woke 20 | entry: make -C docs woke 21 | language: system 22 | pass_filenames: false 23 | files: ^docs/.*\.(rst|md|txt)$ 24 | -------------------------------------------------------------------------------- /docs/.sphinx/.wordlist.txt: -------------------------------------------------------------------------------- 1 | ACME 2 | ACME's 3 | addons 4 | AGPLv 5 | API 6 | APIs 7 | balancer 8 | Charmhub 9 | CLI 10 | DCO 11 | Diátaxis 12 | Dqlite 13 | dropdown 14 | EBS 15 | EKS 16 | enablement 17 | favicon 18 | Furo 19 | Git 20 | GitHub 21 | Grafana 22 | IAM 23 | installable 24 | JSON 25 | Juju 26 | Kubeflow 27 | Kubernetes 28 | Launchpad 29 | linter 30 | LTS 31 | LXD 32 | Makefile 33 | Makefiles 34 | Matrix 35 | Mattermost 36 | MicroCeph 37 | MicroCloud 38 | MicroOVN 39 | MyST 40 | namespace 41 | namespaces 42 | NodePort 43 | Numbat 44 | observability 45 | OEM 46 | OLM 47 | Permalink 48 | pre 49 | Quickstart 50 | ReadMe 51 | reST 52 | reStructuredText 53 | roadmap 54 | RTD 55 | subdirectories 56 | subfolders 57 | subtree 58 | TODO 59 | Ubuntu 60 | UI 61 | UUID 62 | VM 63 | webhook 64 | YAML 65 | -------------------------------------------------------------------------------- /docs/.sphinx/_static/favicon copy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artivis/manif/ad00494251d8ee2d16c3d3b82a65757c94fc48ec/docs/.sphinx/_static/favicon copy.png -------------------------------------------------------------------------------- /docs/.sphinx/_static/favicon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artivis/manif/ad00494251d8ee2d16c3d3b82a65757c94fc48ec/docs/.sphinx/_static/favicon.png -------------------------------------------------------------------------------- /docs/.sphinx/_static/project_specific.css: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artivis/manif/ad00494251d8ee2d16c3d3b82a65757c94fc48ec/docs/.sphinx/_static/project_specific.css -------------------------------------------------------------------------------- /docs/.sphinx/_templates/header.html: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/.sphinx/get_vale_conf.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import requests 4 | import os 5 | 6 | DIR = os.getcwd() 7 | 8 | def main(): 9 | if os.path.exists(f"{DIR}/.sphinx/styles/Canonical"): 10 | print("Vale directory exists") 11 | else: 12 | os.makedirs(f"{DIR}/.sphinx/styles/Canonical") 13 | 14 | url = ( 15 | "https://api.github.com/repos/canonical/praecepta/" 16 | + "contents/styles/Canonical" 17 | ) 18 | r = requests.get(url) 19 | for item in r.json(): 20 | download = requests.get(item["download_url"]) 21 | file = open(".sphinx/styles/Canonical/" + item["name"], "w") 22 | file.write(download.text) 23 | file.close() 24 | 25 | # Update dictionary 26 | if os.path.exists(f"{DIR}/.sphinx/styles/config/dictionaries"): 27 | print("Dictionary directory exists") 28 | else: 29 | os.makedirs(f"{DIR}/.sphinx/styles/config/dictionaries") 30 | url = ( 31 | "https://api.github.com/repos/canonical/praecepta/" 32 | + "contents/styles/config/dictionaries" 33 | ) 34 | r = requests.get(url) 35 | for item in r.json(): 36 | download = requests.get(item["download_url"]) 37 | file = open(".sphinx/styles/config/dictionaries/" + item["name"], "w") 38 | file.write(download.text) 39 | file.close() 40 | 41 | if os.path.exists(f"{DIR}/.sphinx/styles/config/vocabularies/Canonical"): 42 | print("Vocab directory exists") 43 | else: 44 | os.makedirs(f"{DIR}/.sphinx/styles/config/vocabularies/Canonical") 45 | 46 | url = ( 47 | "https://api.github.com/repos/canonical/praecepta/" 48 | + "contents/styles/config/vocabularies/Canonical" 49 | ) 50 | r = requests.get(url) 51 | for item in r.json(): 52 | download = requests.get(item["download_url"]) 53 | file = open( 54 | ".sphinx/styles/config/vocabularies/Canonical/" + item["name"], 55 | "w" 56 | ) 57 | file.write(download.text) 58 | file.close() 59 | config = requests.get( 60 | "https://raw.githubusercontent.com/canonical/praecepta/main/vale.ini" 61 | ) 62 | file = open(".sphinx/vale.ini", "w") 63 | file.write(config.text) 64 | file.close() 65 | 66 | 67 | if __name__ == "__main__": 68 | main() 69 | -------------------------------------------------------------------------------- /docs/.sphinx/metrics/source_metrics.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # shellcheck disable=all 3 | 4 | VENV=".sphinx/venv/bin/activate" 5 | 6 | files=0 7 | words=0 8 | readabilityWords=0 9 | readabilitySentences=0 10 | readabilitySyllables=0 11 | readabilityAverage=0 12 | readable=true 13 | 14 | # measure number of files (.rst and .md), excluding those in .sphinx dir 15 | files=$(find . -type d -path './.sphinx' -prune -o -type f \( -name '*.md' -o -name '*.rst' \) -print | wc -l) 16 | 17 | # calculate metrics only if source files are present 18 | if [ "$files" -eq 0 ]; then 19 | echo "There are no source files to calculate metrics" 20 | else 21 | # measure raw total number of words, excluding those in .sphinx dir 22 | words=$(find . -type d -path './.sphinx' -prune -o \( -name '*.md' -o -name '*.rst' \) -exec cat {} + | wc -w) 23 | 24 | # calculate readability for markdown source files 25 | echo "Activating virtual environment to run vale..." 26 | source "${VENV}" 27 | 28 | for file in *.md *.rst; do 29 | if [ -f "$file" ]; then 30 | readabilityWords=$(vale ls-metrics "$file" | grep '"words"' | sed 's/[^0-9]*//g') 31 | readabilitySentences=$(vale ls-metrics "$file" | grep '"sentences"' | sed 's/[^0-9]*//g') 32 | readabilitySyllables=$(vale ls-metrics "$file" | grep '"syllables"' | sed 's/[^0-9]*//g') 33 | fi 34 | done 35 | 36 | echo "Deactivating virtual environment..." 37 | deactivate 38 | 39 | # calculate mean number of words 40 | if [ "$files" -ge 1 ]; then 41 | meanval=$((readabilityWords / files)) 42 | else 43 | meanval=$readabilityWords 44 | fi 45 | 46 | readabilityAverage=$(echo "scale=2; 0.39 * ($readabilityWords / $readabilitySentences) + (11.8 * ($readabilitySyllables / $readabilityWords)) - 15.59" | bc) 47 | 48 | # cast average to int for comparison 49 | readabilityAverageInt=$(echo "$readabilityAverage / 1" | bc) 50 | 51 | # value below 8 is considered readable 52 | if [ "$readabilityAverageInt" -lt 8 ]; then 53 | readable=true 54 | else 55 | readable=false 56 | fi 57 | 58 | # summarise latest metrics 59 | echo "Summarising metrics for source files (.md, .rst)..." 60 | echo -e "\ttotal files: $files" 61 | echo -e "\ttotal words (raw): $words" 62 | echo -e "\ttotal words (prose): $readabilityWords" 63 | echo -e "\taverage word count: $meanval" 64 | echo -e "\treadability: $readabilityAverage" 65 | echo -e "\treadable: $readable" 66 | fi 67 | -------------------------------------------------------------------------------- /docs/.sphinx/pa11y.json: -------------------------------------------------------------------------------- 1 | { 2 | "chromeLaunchConfig": { 3 | "args": [ 4 | "--no-sandbox" 5 | ] 6 | }, 7 | "reporter": "cli", 8 | "standard": "WCAG2AA" 9 | } 10 | -------------------------------------------------------------------------------- /docs/.sphinx/requirements.txt: -------------------------------------------------------------------------------- 1 | canonical-sphinx[full] 2 | sphinx-autobuild 3 | sphinxcontrib-svg2pdfconverter[CairoSVG] 4 | sphinx-last-updated-by-git 5 | sphinxcontrib-youtube 6 | breathe 7 | exhale 8 | -------------------------------------------------------------------------------- /docs/.sphinx/spellingcheck.yaml: -------------------------------------------------------------------------------- 1 | matrix: 2 | - name: rST files 3 | aspell: 4 | lang: en 5 | d: en_GB 6 | dictionary: 7 | wordlists: 8 | - .sphinx/.wordlist.txt 9 | - .custom_wordlist.txt 10 | output: .sphinx/.wordlist.dic 11 | sources: 12 | - _build/**/*.html 13 | pipeline: 14 | - pyspelling.filters.html: 15 | comments: false 16 | attributes: 17 | - title 18 | - alt 19 | ignores: 20 | - code 21 | - pre 22 | - spellexception 23 | - link 24 | - title 25 | - div.relatedlinks 26 | - strong.command 27 | - div.visually-hidden 28 | - img 29 | - a.p-navigation__link 30 | - a.contributor 31 | -------------------------------------------------------------------------------- /docs/explanation/index.md: -------------------------------------------------------------------------------- 1 | # Explanations 2 | 3 | **Discussion and clarification** of key topics. 4 | 5 | Such as, 6 | 7 | ````{grid} 1 1 2 2 8 | ```{grid-item-card} [Operations on Lie groups](operations) 9 | 10 | Notes on operations supported in **manif**. 11 | ``` 12 | ```{grid-item-card} [Tangent spaces](tangent) 13 | 14 | Notes on tangent spaces and their representation in **manif**. 15 | ``` 16 | ```` 17 | 18 | ````{grid} 1 1 2 2 19 | ```{grid-item-card} [Notes on auto-differentiation](autodiff) 20 | 21 | Notes on auto-differentiation and how that plays with **manif** own Jacobians. 22 | ``` 23 | ```{grid-item-card} [Papers & courses](publication) 24 | 25 | Find here all the references for **manif** associated publications and video courses. 26 | ``` 27 | ```` 28 | 29 | ```{toctree} 30 | :hidden: 31 | 32 | operations 33 | tangent 34 | autodiff 35 | publication 36 | projects 37 | ``` 38 | -------------------------------------------------------------------------------- /docs/explanation/operations.md: -------------------------------------------------------------------------------- 1 | # Operations on Lie groups 2 | 3 | **manif** supports the following operations with Lie groups, 4 | 5 | % Include content from [../../README.md](../../README.md) 6 | 7 | ```{include} ../../README.md 8 | :start-after: 9 | :end-before: 10 | ``` 11 | -------------------------------------------------------------------------------- /docs/explanation/projects.md: -------------------------------------------------------------------------------- 1 | # Who uses manif 2 | 3 | A curated list of work and projects using **manif**. 4 | 5 | ## They cite us 6 | 7 | You may find on Google Scholar publications citing either: 8 | 9 | - the paper [`"A micro Lie theory for state estimation in robotics", J. Sola, J. Deray, D. Atchuthan`](publication.md#micro-lie-theory) accompanying **manif** - at [Google Scholar][jsola18-scholar] 10 | - the paper [`"Manif: A micro Lie theory library for state estimation in robotics applications" J. Deray, J. Sola`](publication.md#manif) - at [Google Scholar][deray20-scholar] 11 | 12 | ## They use manif 13 | 14 | - [`lie-group-controllers`][lie-group-controllers-repo], header-only C++ libraries containing controllers designed for Lie Groups. 15 | - [`bipedal-locomotion-framework`][bipedal-locomotion-framework], the project is a suite of libraries for achieving bipedal locomotion on humanoid robots. 16 | - [`kalmanif`][kalmanif], a small collection of Kalman Filters on Lie groups. 17 | - [`cpp_filter`][cpp_filter], Kalman filters on Lie groups using C++ and **manif**. 18 | 19 | Your project is not listed here? Let [us know](https://github.com/artivis/manif/issues) about it! 20 | 21 | [//]: # (URLs) 22 | 23 | [jsola18-scholar]: https://scholar.google.com/scholar?cites=16456301708818968338 24 | [deray20-scholar]: https://scholar.google.com/scholar?cites=1235228860941456363 25 | 26 | [lie-group-controllers-repo]: https://github.com/dic-iit/lie-group-controllers 27 | [bipedal-locomotion-framework]: https://github.com/dic-iit/bipedal-locomotion-framework 28 | [kalmanif]: https://github.com/artivis/kalmanif 29 | [cpp_filter]: https://github.com/aalbaali/cpp_filter 30 | -------------------------------------------------------------------------------- /docs/explanation/tangent.md: -------------------------------------------------------------------------------- 1 | # Tangent spaces 2 | 3 | % Include content from [../../README.md](../../README.md) 4 | 5 | ```{include} ../../README.md 6 | :language: md 7 | :start-after: 8 | :end-before: 9 | ``` 10 | -------------------------------------------------------------------------------- /docs/howto/documentation.md: -------------------------------------------------------------------------------- 1 | # Build the documentation 2 | 3 | To build the documentation, 4 | make sure you have Doxygen installed: 5 | 6 | ```bash 7 | sudo apt-get install -y doxygen 8 | ``` 9 | 10 | and execute the following: 11 | 12 | ```bash 13 | cd docs 14 | make run 15 | ``` 16 | 17 | The documentation is now available at [`http://localhost:8000`](http://localhost:8000). 18 | -------------------------------------------------------------------------------- /docs/howto/index.md: -------------------------------------------------------------------------------- 1 | # How-to 2 | 3 | **Step-by-step guides** covering key operations and common tasks. 4 | 5 | Here are a couple examples, 6 | 7 | ````{grid} 1 1 2 2 8 | ```{grid-item-card} [Write generic code](tmpl-code) 9 | 10 | How to write template functions for any **manif** group/tangent. 11 | ``` 12 | ```{grid-item-card} [Use with Ceres](ceres) 13 | 14 | How to use **manif** with the Ceres solver. 15 | ``` 16 | ```` 17 | 18 | ```{toctree} 19 | :hidden: 20 | 21 | ceres 22 | tmpl-code 23 | documentation 24 | ``` 25 | -------------------------------------------------------------------------------- /docs/howto/tmpl-code.md: -------------------------------------------------------------------------------- 1 | # Write generic code 2 | 3 | All Lie group & tangent classes defined in **manif** have in common that they inherit from a templated base class using the [CRTP][crtp] idiom. 4 | It allows one to write generic code abstracting the Lie group details. 5 | 6 | 7 | 8 | ## Simple example 9 | 10 | Let us write a simple function that take any group object and prints some information about it, 11 | 12 | ```cpp 13 | #include 14 | #include 15 | 16 | using namespace manif; 17 | 18 | template 19 | void print(const LieGroupBase& g) { 20 | std::cout << "Degrees of freedom: " << g::DoF << "\n" 21 | << "Underlying representation vector size: " << g::RepSize << "\n" 22 | << "Current values: " << g << "\n; 23 | } 24 | 25 | int main() { 26 | SE2d p_2d; 27 | print(p_2d); 28 | 29 | SE3d p_3d; 30 | print(p_3d); 31 | } 32 | ``` 33 | 34 | ## Multiple templated arguments 35 | 36 | Let us write a function that takes two group objects and performs some computation, 37 | 38 | ```cpp 39 | #include 40 | 41 | using namespace manif; 42 | 43 | template 44 | typename DerivedA::Scalar ominusSquaredWeightedNorm( 45 | const LieGroupBase& state, 46 | const LieGroupBase& state_other 47 | ) { 48 | return (state - state_other).squaredWeightedNorm(); 49 | } 50 | 51 | int main() { 52 | SE2d state = SE2d::Random(); 53 | 54 | SE2d::DataType state_other_data = SE2d::DataType::Random(); 55 | 56 | Eigen::Map state_other_map(state_other_data.data()); 57 | 58 | double osn = ominusSquaredWeightedNorm(state, state_other_map); 59 | 60 | ... 61 | } 62 | ``` 63 | 64 | [//]: # (URLs) 65 | 66 | [crtp]: https://en.wikipedia.org/wiki/Curiously_recurring_template_pattern -------------------------------------------------------------------------------- /docs/index.md: -------------------------------------------------------------------------------- 1 | # manif documentation 2 | 3 | % Include content from [../README.md](../README.md) 4 | 5 | ```{include} ../README.md 6 | :start-after: 7 | :end-before: 8 | ``` 9 | 10 | 11 | 12 | 17 | 18 | [barrau15]: https://arxiv.org/pdf/1410.1465.pdf 19 | [fourmy19]: https://hal.science/hal-02183498/document 20 | [kelly24]: https://arxiv.org/abs/2312.07555 21 | [jsola18]: http://arxiv.org/abs/1812.01537 22 | [jsola-iri-lecture]: https://www.youtube.com/watch?v=nHOcoIyJj2o 23 | [cheat_sheet]: paper/Lie_theory_cheat_sheet.pdf 24 | [IRI-UPC]: https://www.iri.upc.edu/ 25 | 26 | --- 27 | 28 | ## In this documentation 29 | 30 | ````{grid} 1 1 2 2 31 | ```{grid-item-card} [Tutorials](tutorial/index) 32 | 33 | **Start here** with an hands-on introduction to **manif** for new users, guiding you through installation and using manif in your project. 34 | ``` 35 | 36 | ```{grid-item-card} [How-to guides](howto/index) 37 | 38 | **Step-by-step guides** covering key operations and common tasks. 39 | ``` 40 | ```` 41 | 42 | ````{grid} 1 1 2 2 43 | ```{grid-item-card} [Reference](reference/index) 44 | 45 | **Technical information**. 46 | ``` 47 | ```{grid-item-card} [Explanation](explanation/index) 48 | 49 | **Discussion and clarification** of key topics. 50 | ``` 51 | ```` 52 | 53 | ```{toctree} 54 | :hidden: 55 | 56 | tutorial/index 57 | howto/index 58 | reference/index 59 | explanation/index 60 | ``` 61 | -------------------------------------------------------------------------------- /docs/reference/api-cpp.md: -------------------------------------------------------------------------------- 1 | # C++ API 2 | 3 | The following documentation presents the C++ API. 4 | 5 | ```{eval-rst} 6 | .. include:: ../api/class_view_hierarchy.rst.include 7 | ``` 8 | 9 | ```{eval-rst} 10 | .. include:: ../api/file_view_hierarchy.rst.include 11 | ``` 12 | -------------------------------------------------------------------------------- /docs/reference/api-python.md: -------------------------------------------------------------------------------- 1 | # Python3 API 2 | 3 | The following documentation presents the Python3 API. 4 | 5 | The Python3 API is generated using [pybind11](https://pybind11.readthedocs.io/en/stable/). 6 | 7 | ```{eval-rst} 8 | .. automodule:: manifpy 9 | :imported-members: 10 | :members: 11 | :undoc-members: 12 | :member-order: groupwise 13 | :show-inheritance: 14 | ``` 15 | -------------------------------------------------------------------------------- /docs/reference/examples-cpp.md: -------------------------------------------------------------------------------- 1 | # C++ Examples 2 | 3 | The project provides some self-contained and self-explained executables implementing some real problems. 4 | Their source code is located in [`examples/`](https://github.com/artivis/manif/tree/devel/examples) folder. 5 | 6 | These demos are: 7 | 8 | - [`se2_localization.cpp`](https://github.com/artivis/manif/tree/devel/examples/se2_localization.cpp): 2D robot localization based on fixed landmarks using SE2 as robot poses. This implements the example V.A in the paper. 9 | - [`se2_localization_ukfm.cpp`](https://github.com/artivis/manif/tree/devel/examples/se2_localization_ukfm.cpp): 2D robot localization based on fixed landmarks using SE2 as robot poses. This implements the filter described in ['A Code for Unscented Kalman Filtering on Manifolds (UKF-M)'][brossard-ukfm], M. Brossard, A. Barrau and S. Bonnabel. 10 | - [`se3_localization.cpp`](https://github.com/artivis/manif/tree/devel/examples/se3_localization.cpp): 3D robot localization based on fixed landmarks using SE3 as robot poses. This re-implements the example above but in 3D. 11 | - [`se2_sam.cpp`](https://github.com/artivis/manif/tree/devel/examples/se2_sam.cpp): 2D smoothing and mapping (SAM) with simultaneous estimation of robot poses and landmark locations, based on SE2 robot poses. This implements a the example V.B in the paper. 12 | - [`se3_sam.cpp`](https://github.com/artivis/manif/tree/devel/examples/se3_sam.cpp): 3D smoothing and mapping (SAM) with simultaneous estimation of robot poses and landmark locations, based on SE3 robot poses. This implements a 3D version of the example V.B in the paper. 13 | - [`se3_sam_selfcalib.cpp`](https://github.com/artivis/manif/tree/devel/examples/se3_sam_selfcalib.cpp): 3D smoothing and mapping (SAM) with self-calibration, with simultaneous estimation of robot poses, landmark locations and sensor parameters, based on SE3 robot poses. This implements a 3D version of the example V.C in the paper. 14 | - [`se_2_3_localization.cpp`](https://github.com/artivis/manif/tree/devel/examples/se_2_3_localization.cpp): A strap down IMU model based 3D robot localization, with measurements of fixed landmarks, using SE_2_3 as extended robot poses (translation, rotation and linear velocity). 15 | 16 | To build the demos, pass the related flag to CMake, 17 | 18 | ```bash 19 | cmake -DBUILD_EXAMPLES=ON .. 20 | make 21 | cd examples 22 | ./se2_localization 23 | ``` 24 | 25 | [//]: # (URLs) 26 | 27 | [brossard-ukfm]: https://arxiv.org/pdf/2002.00878.pdf 28 | -------------------------------------------------------------------------------- /docs/reference/examples-python.md: -------------------------------------------------------------------------------- 1 | # Python3 Examples 2 | 3 | The project provides some self-contained and self-explained executables implementing some real problems. 4 | Their source code is located in [`examples/`](https://github.com/artivis/manif/tree/devel/examples) folder. 5 | 6 | These demos are: 7 | 8 | - [`se2_localization.py`](https://github.com/artivis/manif/tree/devel/examples/se2_localization.py): 2D robot localization based on fixed landmarks using SE2 as robot poses. This implements the example V.A in the paper. 9 | - [`se3_localization.py`](https://github.com/artivis/manif/tree/devel/examples/se3_localization.py): 3D robot localization based on fixed landmarks using SE3 as robot poses. This re-implements the example above but in 3D. 10 | - [`se2_sam.py`](https://github.com/artivis/manif/tree/devel/examples/se2_sam.py): 2D smoothing and mapping (SAM) with simultaneous estimation of robot poses and landmark locations, based on SE2 robot poses. This implements a the example V.B in the paper. 11 | - [`se3_sam.py`](https://github.com/artivis/manif/tree/devel/examples/se3_sam.py): 3D smoothing and mapping (SAM) with simultaneous estimation of robot poses and landmark locations, based on SE3 robot poses. This implements a 3D version of the example V.B in the paper. 12 | - [`se3_sam_selfcalib.py`](https://github.com/artivis/manif/tree/devel/examples/se3_sam_selfcalib.py): 3D smoothing and mapping (SAM) with self-calibration, with simultaneous estimation of robot poses, landmark locations and sensor parameters, based on SE3 robot poses. This implements a 3D version of the example V.C in the paper. 13 | - [`se_2_3_localization.py`](https://github.com/artivis/manif/tree/devel/examples/se_2_3_localization.py): A strap down IMU model based 3D robot localization, with measurements of fixed landmarks, using SE_2_3 as extended robot poses (translation, rotation and linear velocity). 14 | 15 | To run a demo, simply go to the `examples/` folder and run: 16 | 17 | ```bash 18 | cd examples 19 | python3 se2_localization.py 20 | ``` 21 | -------------------------------------------------------------------------------- /docs/reference/index.md: -------------------------------------------------------------------------------- 1 | # References 2 | 3 | **Technical information**. 4 | 5 | ````{grid} 1 1 2 2 6 | ```{grid-item-card} [C++ API](../api/manif_api) 7 | 8 | The C++ API documentation. 9 | ``` 10 | ```{grid-item-card} [C++ Examples](examples-cpp) 11 | 12 | Several self-contained and self-explained C++ examples implementing some real problems. 13 | ``` 14 | ```` 15 | 16 | ````{grid} 1 1 2 2 17 | ```{grid-item-card} [Python3 API](api-python) 18 | 19 | The Python3 API documentation. 20 | ``` 21 | ```{grid-item-card} [Python3 Examples](examples-python) 22 | 23 | Several self-contained and self-explained Python3 examples implementing some real problems. 24 | ``` 25 | ```` 26 | 27 | ```{toctree} 28 | :hidden: 29 | :maxdepth: 0 30 | 31 | api-cpp 32 | examples-cpp 33 | api-python 34 | examples-python 35 | ``` 36 | -------------------------------------------------------------------------------- /docs/reuse/links.txt: -------------------------------------------------------------------------------- 1 | .. _reStructuredText style guide: https://canonical-documentation-with-sphinx-and-readthedocscom.readthedocs-hosted.com/style-guide/ 2 | .. _MyST style guide: https://canonical-documentation-with-sphinx-and-readthedocscom.readthedocs-hosted.com/style-guide-myst/ 3 | .. _Read the Docs at Canonical: https://library.canonical.com/documentation/read-the-docs-at-canonical 4 | .. _How to publish documentation on Read the Docs: https://library.canonical.com/documentation/publish-on-read-the-docs 5 | .. _Example product documentation: https://canonical-example-product-documentation.readthedocs-hosted.com/ 6 | .. _`Sphinx configuration`: https://www.sphinx-doc.org/en/master/usage/configuration.html 7 | .. _`Sphinx extensions`: https://www.sphinx-doc.org/en/master/usage/extensions/index.html 8 | .. _`file-wide metadata`: https://www.sphinx-doc.org/en/master/usage/restructuredtext/field-lists.html 9 | .. _`Furo documentation`: https://pradyunsg.me/furo/quickstart/ 10 | .. _`Hiding Contents sidebar`: https://pradyunsg.me/furo/customisation/toc/ 11 | .. _`Sphinx`: https://www.sphinx-doc.org/ 12 | .. _Canonical Sphinx: https://github.com/canonical/canonical-sphinx 13 | .. _change log: https://github.com/canonical/sphinx-docs-starter-pack/wiki/Change-log 14 | .. _Open Graph: https://ogp.me/ 15 | .. _manual import: https://readthedocs.com/dashboard/import/manual/ 16 | .. _How to connect your Read the Docs account to your Git provider: https://docs.readthedocs.com/platform/stable/guides/connecting-git-account.html 17 | .. _How to manually configure a Git repository integration: https://docs.readthedocs.io/en/stable/guides/setup/git-repo-manual.html 18 | .. _reStructuredText: https://www.sphinx-doc.org/en/master/usage/restructuredtext/index.html 19 | .. _Markdown: https://commonmark.org/ 20 | .. _MyST: https://myst-parser.readthedocs.io/ 21 | .. _Diátaxis: https://diataxis.fr/ 22 | .. _Pa11y: https://pa11y.org/ 23 | .. _Pa11y readme: https://github.com/pa11y/pa11y#command-line-configuration 24 | .. _More useful markup: https://canonical-documentation-with-sphinx-and-readthedocscom.readthedocs-hosted.com/style-guide/#more-useful-markup 25 | .. _Vale: https://vale.sh/ 26 | .. _Vale rules: https://github.com/canonical/praecepta 27 | .. _Web Content Accessibility Guidelines (WCAG) 2.2: https://www.w3.org/TR/WCAG22/ 28 | .. _Level AA conformance: https://www.w3.org/WAI/WCAG2AA-Conformance 29 | 30 | .. SHORTCUTS 31 | .. |RST| replace:: :abbr:`reST (reStructuredText)` 32 | -------------------------------------------------------------------------------- /docs/tutorial/cpp.md: -------------------------------------------------------------------------------- 1 | # Getting started in C++ 2 | 3 | **manif** has been designed for an easy integration to larger projects, 4 | 5 | - a single dependency on [Eigen][eigen] 6 | - header-only 7 | - templated on the underlying scalar type so that one can use its own 8 | - and C++11, since not everyone gets to enjoy the latest version, especially in industry 9 | 10 | ## Installation 11 | 12 | Prior to installing **manif**, 13 | we have to install its main dependency `Eigen3`: 14 | 15 | `````{tab-set} 16 | ````{tab-item} Ubuntu 17 | :sync: key1 18 | ```bash 19 | apt-get install libeigen3-dev 20 | ``` 21 | ```` 22 | ````{tab-item} OS X 23 | :sync: key2 24 | ```bash 25 | brew install eigen 26 | ``` 27 | ```` 28 | ````` 29 | 30 | **manif** also depends on [lt::optional][optional-repo] however it is included in the [`external/`](https://github.com/artivis/manif/tree/devel/external) folder of the project. 31 | 32 | Next, clone the repository locally if you haven't done so already: 33 | 34 | ```bash 35 | git clone https://github.com/artivis/manif.git 36 | cd manif 37 | ``` 38 | 39 | To buid and install **manif**, 40 | use the following commands: 41 | 42 | ```bash 43 | mkdir build && cd build 44 | cmake .. 45 | make install 46 | ``` 47 | 48 | ## Use manif in a project 49 | 50 | To use **manif** in a project, 51 | edit the `CMakeLists.txt` and add the following directives to first find `Eigen3`: 52 | 53 | ```cmake 54 | # Find the Eigen library 55 | find_package(Eigen3 REQUIRED) 56 | 57 | # Add eigen3 include directories to the target 58 | target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) 59 | ``` 60 | 61 | Next, add the following directives to find `manif` and add its include directories to the compliation target: 62 | 63 | ```cmake 64 | # Find the manif library 65 | find_package(manif REQUIRED) 66 | 67 | # Add manif include directories to the target 68 | target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${manif_INCLUDE_DIRS}) 69 | ``` 70 | 71 | Now that the project is able to find `manif` and its headers, 72 | it can be included in the code, for instance: 73 | 74 | ```cpp 75 | #include 76 | 77 | ... 78 | 79 | auto state = manif::SE3d::Identity(); 80 | 81 | ... 82 | 83 | ``` 84 | 85 | From there, have a look at the howtos for further informations 86 | as well as the [examples](../reference/examples-cpp.md) to find several self-contained and self-explained executables implementing some real problems. 87 | 88 | [//]: # (URLs) 89 | 90 | [eigen]: http://eigen.tuxfamily.org 91 | [crtp]: https://en.wikipedia.org/wiki/Curiously_recurring_template_pattern 92 | [optional-repo]: https://github.com/TartanLlama/optional 93 | [ceres-jet]: http://ceres-solver.org/automatic_derivatives.html#dual-numbers-jets 94 | -------------------------------------------------------------------------------- /docs/tutorial/index.md: -------------------------------------------------------------------------------- 1 | # Tutorials 2 | 3 | Hands-on introduction to **manif** for new users, 4 | guiding you through installation and using manif in your project. 5 | 6 | You can get started with **manif** in either C++ or Python3. 7 | 8 | Have a look at the following tutorials: 9 | 10 | ````{grid} 1 1 2 2 11 | ```{grid-item-card} [Getting started in C++](cpp) 12 | 13 | 14 | ``` 15 | 16 | ```{grid-item-card} [Getting started in Python3](python) 17 | 18 | 19 | ``` 20 | ```` 21 | 22 | ```{toctree} 23 | :hidden: 24 | 25 | cpp 26 | python 27 | ``` 28 | -------------------------------------------------------------------------------- /docs/tutorial/python.md: -------------------------------------------------------------------------------- 1 | # Getting started in Python3 2 | 3 | **manif** provides Python3 bindings, 4 | called **manifpy**, 5 | allowing to prototype and develop directly in Python3. 6 | 7 | ## Installation 8 | 9 | ### From conda 10 | 11 | **manifpy** can be installed directly from [conda-forge][conda-manifpy]: 12 | 13 | ```bash 14 | conda install -c conda-forge manifpy 15 | ``` 16 | 17 | ### From source 18 | 19 | The Python3 wrappers are generated using [pybind11][pybind11-rtd]. 20 | So first, we need to install it, 21 | however it must be available directly in the environment root so that `CMake` can find it. 22 | 23 | To do so use: 24 | 25 | ```bash 26 | python3 -m pip install "pybind11[global]" 27 | ``` 28 | 29 | Prior to installing **manifpy**, 30 | we have to install its main dependency `Eigen3`: 31 | 32 | `````{tab-set} 33 | ````{tab-item} Ubuntu 34 | :sync: key1 35 | ```bash 36 | apt-get install libeigen3-dev 37 | ``` 38 | ```` 39 | ````{tab-item} OS X 40 | :sync: key2 41 | ```bash 42 | brew install eigen 43 | ``` 44 | ```` 45 | ````` 46 | 47 | Next, clone the repository locally if you haven't done so already: 48 | 49 | ```bash 50 | git clone https://github.com/artivis/manif.git 51 | cd manif 52 | ``` 53 | 54 | To buid and install **manif**, 55 | use the following commands: 56 | 57 | ```bash 58 | python3 -m pip install . 59 | ``` 60 | 61 | ## Use manif in a project 62 | 63 | ```python 64 | from manifpy import SE3 65 | 66 | ... 67 | 68 | state = SE3.Identity() 69 | 70 | ... 71 | ``` 72 | 73 | From there, have a look at the howtos for further informations 74 | as well as the [examples](../reference/examples-python.md) to find several self-contained and self-explained examples implementing some real problems. 75 | 76 | [//]: # (URLs) 77 | 78 | [pybind11-rtd]: https://pybind11.readthedocs.io/en/stable/index.html 79 | [conda-manifpy]: https://anaconda.org/conda-forge/manifpy 80 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(so2_average so2_average.cpp) 2 | 3 | add_executable(se2_average se2_average.cpp) 4 | add_executable(se2_interpolation se2_interpolation.cpp) 5 | add_executable(se2_decasteljau se2_DeCasteljau.cpp) 6 | add_executable(se2_localization se2_localization.cpp) 7 | add_executable(se2_localization_ukfm se2_localization_ukfm.cpp) 8 | add_executable(se2_sam se2_sam.cpp) 9 | 10 | add_executable(se3_localization se3_localization.cpp) 11 | add_executable(se3_sam se3_sam.cpp) 12 | add_executable(se3_sam_selfcalib se3_sam_selfcalib.cpp) 13 | 14 | add_executable(se_2_3_localization se_2_3_localization.cpp) 15 | 16 | set(CXX_11_EXAMPLE_TARGETS 17 | 18 | # SO2 19 | so2_average 20 | 21 | # SE2 22 | se2_interpolation 23 | se2_decasteljau 24 | se2_average 25 | se2_localization 26 | se2_sam 27 | 28 | se2_localization_ukfm 29 | 30 | # SE3 31 | se3_localization 32 | se3_sam 33 | se3_sam_selfcalib 34 | 35 | # SE_2_3 36 | se_2_3_localization 37 | ) 38 | 39 | if(NOT MSVC) 40 | add_executable(bundle_sam bundle_sam.cpp) 41 | 42 | set(CXX_11_EXAMPLE_TARGETS 43 | 44 | ${CXX_11_EXAMPLE_TARGETS} 45 | 46 | # Bundle 47 | bundle_sam 48 | ) 49 | endif() 50 | 51 | # Link to manif 52 | foreach(target ${CXX_11_EXAMPLE_TARGETS}) 53 | target_link_libraries(${target} ${PROJECT_NAME}) 54 | 55 | if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") 56 | # GCC is not strict enough by default, so enable most of the warnings. 57 | target_compile_options(${target} PRIVATE 58 | -Werror=all 59 | -Werror=extra 60 | ) 61 | endif() 62 | 63 | endforeach() 64 | 65 | # Set required C++11 flag 66 | set_property(TARGET ${CXX_11_EXAMPLE_TARGETS} PROPERTY CXX_STANDARD 11) 67 | set_property(TARGET ${CXX_11_EXAMPLE_TARGETS} PROPERTY CXX_STANDARD_REQUIRED ON) 68 | set_property(TARGET ${CXX_11_EXAMPLE_TARGETS} PROPERTY CXX_EXTENSIONS OFF) 69 | -------------------------------------------------------------------------------- /examples/scripts/plot_interpolation.m: -------------------------------------------------------------------------------- 1 | clc; clear; #close all; 2 | 3 | path = "/home/user/"; 4 | 5 | %file_base = "se2_interp_slerp"; 6 | %file_base = "se2_interp_cubic"; 7 | file_base = "se2_interp_cnsmooth"; 8 | 9 | extension = ".csv"; 10 | 11 | file = fullfile(path, [file_base extension]); 12 | 13 | data = load(file); 14 | 15 | 'num k points' 16 | num_k_pts = data(1,1) 17 | 18 | 'Interpolation method' 19 | interp_method = data(1,2) 20 | 21 | switch interp_method 22 | case 0 23 | method = 'SLERP' 24 | case 1 25 | method = 'CUBIC' 26 | case 2 27 | method = 'CN smooth' 28 | otherwise 29 | method = 'Unknown' 30 | end 31 | 32 | title = strcat('SE2 Interpolation (', method, ')'); 33 | 34 | 'num interpolated points' 35 | total_pts = size(data(2+num_k_pts:end,1), 1) 36 | 37 | r = 0.1; % magnitude (length) of arrow to plot 38 | 39 | x = data(2:end,1); 40 | y = data(2:end,2); 41 | t = data(2:end,3); 42 | 43 | u = r * cos(t); % convert polar (theta,r) to cartesian 44 | v = r * sin(t); 45 | 46 | figure; 47 | quiver(x(1:num_k_pts),y(1:num_k_pts),u(1:num_k_pts),v(1:num_k_pts),'color',[0 0 1]); 48 | hold on 49 | quiver(x(num_k_pts+1:end),y(num_k_pts+1:end),u(num_k_pts+1:end),v(num_k_pts+1:end),'color',[1 0 0]); 50 | set(get(gca, 'title'), 'string', title); 51 | hold off 52 | 53 | img_file = fullfile(path, [file_base '.png']); 54 | saveas(gcf, img_file); 55 | 56 | return; -------------------------------------------------------------------------------- /examples/se2_DeCasteljau.cpp: -------------------------------------------------------------------------------- 1 | #include "se2_points_generator.h" 2 | #include "manif/algorithms/decasteljau.h" 3 | 4 | #include 5 | 6 | /** 7 | * @brief An example of the use of the decasteljau algorithm. 8 | */ 9 | 10 | int main(int argc, char** argv) 11 | { 12 | if (argc < 4) 13 | { 14 | std::cout << "Usage: .se2_decateljau

\n"; 15 | std::cout << "\t with k: number of initial points on the 8-shaped curve.\n"; 16 | std::cout << "\t with d: degree of smoothness of the generated curve.\n"; 17 | std::cout << "\t with p: number of points to generate between consecutive points of the initial curve.\n"; 18 | return EXIT_SUCCESS; 19 | } 20 | 21 | int k, d, p; 22 | 23 | k = atoi(argv[1]); 24 | d = atoi(argv[2]); 25 | p = atoi(argv[3]); 26 | 27 | const auto points = manif::generateSE2PointsOnHeightShape(k); 28 | 29 | std::cout << "Initial points:\n"; 30 | for (const auto& point : points) 31 | std::cout << point.x() << "," 32 | << point.y() << "," 33 | << point.angle() << "\n"; 34 | 35 | std::cout << "Generated points:\n"; 36 | const auto curve = manif::decasteljau(points, d, p, true); 37 | 38 | for (const auto& point : curve) 39 | std::cout << point.x() << "," 40 | << point.y() << "," 41 | << point.angle() << "\n"; 42 | 43 | return EXIT_SUCCESS; 44 | } 45 | -------------------------------------------------------------------------------- /examples/se2_average.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/SE2.h" 2 | #include "manif/algorithms/average.h" 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | /** 10 | * @brief An example of the use of the average algorithms. 11 | */ 12 | 13 | int main(int /*argc*/, char** /*argv*/) 14 | { 15 | // Generate 4 'close' points 16 | 17 | std::vector> points; 18 | 19 | // points.emplace_back(1, 1, 3.*MANIF_PI/4.); 20 | // points.emplace_back(1, 3, 5.*MANIF_PI/8.); 21 | // points.emplace_back(3, 1, MANIF_PI/4.); 22 | // points.emplace_back(3, 3, 3.*MANIF_PI/8.); 23 | 24 | points.emplace_back(-std::sqrt(2.)/2., std::sqrt(2.)/2., MANIF_PI/4.); 25 | points.emplace_back( std::sqrt(2.), 0., 0.); 26 | points.emplace_back(-std::sqrt(2.)/2., -std::sqrt(2.)/2., -MANIF_PI/4.); 27 | 28 | std::cout << "Initial points:\n"; 29 | for (const auto& p : points) 30 | std::cout << p.x() << "," 31 | << p.y() << "," 32 | << p.angle() << "\n"; 33 | std::cout << "\n"; 34 | 35 | auto average = manif::average_biinvariant(points, 1e-9, 100); 36 | 37 | std::cout << "Average biinvariant:\n"; 38 | std::cout << average.x() << "," 39 | << average.y() << "," 40 | << average.angle() << "\n\n"; 41 | 42 | std::cout << "Approximate expected average:\n"; 43 | std::cout << "0.2171, 0, 0\n\n"; 44 | 45 | // average = manif::average_frechet_left(points, 1e-9, 100); 46 | 47 | // std::cout << "Average Frechet left:\n"; 48 | // std::cout << average.x() << "," 49 | // << average.y() << "," 50 | // << average.angle() << "\n\n"; 51 | 52 | // average = manif::average_frechet_right(points, 1e-9, 100); 53 | 54 | // std::cout << "Average Frechet right:\n"; 55 | // std::cout << average.x() << "," 56 | // << average.y() << "," 57 | // << average.angle() << "\n\n"; 58 | 59 | return EXIT_SUCCESS; 60 | } 61 | -------------------------------------------------------------------------------- /examples/se2_points_generator.h: -------------------------------------------------------------------------------- 1 | #include "manif/SE2.h" 2 | #include 3 | 4 | namespace manif { 5 | 6 | /** 7 | * @brief Generate k SE2 points on an height shape curve. 8 | * @param[in] k Number of points to generate. 9 | * @return vector of k points. 10 | */ 11 | std::vector 12 | generateSE2PointsOnHeightShape(const unsigned int k) 13 | { 14 | // Generate some k points on 8-shaped curve 15 | std::vector states; 16 | states.reserve(k); 17 | 18 | const double x = std::cos(0); 19 | const double y = std::sin(0)/2; 20 | states.emplace_back(x,y,MANIF_PI/2); 21 | 22 | double t = 0; 23 | for (unsigned int i=1; i 5 | #include 6 | 7 | /** 8 | * @brief An example of the use of the average biinvariant algorithm. 9 | */ 10 | 11 | int main(int argc, char**) 12 | { 13 | if (argc > 1) 14 | { 15 | std::cout << "Usage: .so2_average\n"; 16 | } 17 | 18 | // Generate 4 points around PI/2 19 | 20 | std::vector points; 21 | 22 | points.emplace_back( MANIF_PI/4.); 23 | points.emplace_back(3.*MANIF_PI/8.); 24 | points.emplace_back(5.*MANIF_PI/8.); 25 | points.emplace_back(3.*MANIF_PI/4.); 26 | 27 | std::cout << "Initial points:\n"; 28 | for (const auto& p : points) 29 | std::cout << p.angle() << "\n"; 30 | 31 | auto average = manif::average_biinvariant(points); 32 | 33 | std::cout << "Average point:\n"; 34 | std::cout << average.angle() << "\n"; 35 | 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /include/manif/Bundle.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_BUNDLE_H_ 2 | #define _MANIF_BUNDLE_H_ 3 | 4 | #include "manif/impl/macro.h" 5 | #include "manif/impl/utils.h" 6 | #include "manif/impl/lie_group_base.h" 7 | #include "manif/impl/tangent_base.h" 8 | 9 | #include "manif/impl/bundle/Bundle_properties.h" 10 | #include "manif/impl/bundle/Bundle_base.h" 11 | #include "manif/impl/bundle/Bundle_map.h" 12 | #include "manif/impl/bundle/Bundle.h" 13 | #include "manif/impl/bundle/BundleTangent_base.h" 14 | #include "manif/impl/bundle/BundleTangent.h" 15 | #include "manif/impl/bundle/BundleTangent_map.h" 16 | 17 | #endif // _MANIF_BUNDLE_H_ 18 | -------------------------------------------------------------------------------- /include/manif/Rn.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_RN_H_ 2 | #define _MANIF_RN_H_ 3 | 4 | #include "manif/impl/macro.h" 5 | #include "manif/impl/utils.h" 6 | #include "manif/impl/lie_group_base.h" 7 | #include "manif/impl/tangent_base.h" 8 | 9 | #include "manif/impl/rn/Rn_properties.h" 10 | #include "manif/impl/rn/Rn_base.h" 11 | #include "manif/impl/rn/RnTangent_base.h" 12 | #include "manif/impl/rn/Rn.h" 13 | #include "manif/impl/rn/RnTangent.h" 14 | #include "manif/impl/rn/Rn_map.h" 15 | #include "manif/impl/rn/RnTangent_map.h" 16 | 17 | #endif // _MANIF_RN_H_ 18 | -------------------------------------------------------------------------------- /include/manif/SE2.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_SE2_H_ 2 | #define _MANIF_SE2_H_ 3 | 4 | #include "manif/impl/macro.h" 5 | #include "manif/impl/lie_group_base.h" 6 | #include "manif/impl/tangent_base.h" 7 | 8 | #include "manif/impl/se2/SE2_properties.h" 9 | #include "manif/impl/se2/SE2_base.h" 10 | #include "manif/impl/se2/SE2Tangent_base.h" 11 | #include "manif/impl/se2/SE2.h" 12 | #include "manif/impl/se2/SE2Tangent.h" 13 | #include "manif/impl/se2/SE2_map.h" 14 | #include "manif/impl/se2/SE2Tangent_map.h" 15 | 16 | #endif /* _MANIF_SE2_H_ */ 17 | -------------------------------------------------------------------------------- /include/manif/SE3.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_SE3_H_ 2 | #define _MANIF_SE3_H_ 3 | 4 | #include "manif/impl/macro.h" 5 | #include "manif/impl/lie_group_base.h" 6 | #include "manif/impl/tangent_base.h" 7 | 8 | #include "manif/impl/se3/SE3_properties.h" 9 | #include "manif/impl/se3/SE3_base.h" 10 | #include "manif/impl/se3/SE3Tangent_base.h" 11 | #include "manif/impl/se3/SE3.h" 12 | #include "manif/impl/se3/SE3Tangent.h" 13 | #include "manif/impl/se3/SE3_map.h" 14 | #include "manif/impl/se3/SE3Tangent_map.h" 15 | 16 | #endif /* _MANIF_SE3_H_ */ 17 | -------------------------------------------------------------------------------- /include/manif/SE_2_3.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_SE_2_3_H_ 2 | #define _MANIF_SE_2_3_H_ 3 | 4 | #include "manif/impl/macro.h" 5 | #include "manif/impl/lie_group_base.h" 6 | #include "manif/impl/tangent_base.h" 7 | 8 | #include "manif/impl/se_2_3/SE_2_3_properties.h" 9 | #include "manif/impl/se_2_3/SE_2_3_base.h" 10 | #include "manif/impl/se_2_3/SE_2_3Tangent_base.h" 11 | #include "manif/impl/se_2_3/SE_2_3.h" 12 | #include "manif/impl/se_2_3/SE_2_3Tangent.h" 13 | #include "manif/impl/se_2_3/SE_2_3_map.h" 14 | #include "manif/impl/se_2_3/SE_2_3Tangent_map.h" 15 | 16 | #endif /* _MANIF_SE_2_3_H_ */ 17 | -------------------------------------------------------------------------------- /include/manif/SGal3.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_SGAL3_H_ 2 | #define _MANIF_SGAL3_H_ 3 | 4 | #include "manif/impl/macro.h" 5 | #include "manif/impl/lie_group_base.h" 6 | #include "manif/impl/tangent_base.h" 7 | 8 | #include "manif/impl/sgal3/SGal3_properties.h" 9 | #include "manif/impl/sgal3/SGal3_base.h" 10 | #include "manif/impl/sgal3/SGal3Tangent_base.h" 11 | #include "manif/impl/sgal3/SGal3.h" 12 | #include "manif/impl/sgal3/SGal3Tangent.h" 13 | #include "manif/impl/sgal3/SGal3_map.h" 14 | #include "manif/impl/sgal3/SGal3Tangent_map.h" 15 | 16 | #endif // _MANIF_SGAL3_H_ 17 | -------------------------------------------------------------------------------- /include/manif/SO2.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_SO2_H_ 2 | #define _MANIF_SO2_H_ 3 | 4 | #include "manif/impl/macro.h" 5 | #include "manif/impl/utils.h" 6 | #include "manif/impl/lie_group_base.h" 7 | #include "manif/impl/tangent_base.h" 8 | 9 | #include "manif/impl/so2/SO2_properties.h" 10 | #include "manif/impl/so2/SO2_base.h" 11 | #include "manif/impl/so2/SO2Tangent_base.h" 12 | #include "manif/impl/so2/SO2.h" 13 | #include "manif/impl/so2/SO2Tangent.h" 14 | #include "manif/impl/so2/SO2_map.h" 15 | #include "manif/impl/so2/SO2Tangent_map.h" 16 | 17 | #endif /* _MANIF_SO2_H_ */ 18 | -------------------------------------------------------------------------------- /include/manif/SO3.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_SO3_H_ 2 | #define _MANIF_SO3_H_ 3 | 4 | #include "manif/impl/macro.h" 5 | #include "manif/impl/lie_group_base.h" 6 | #include "manif/impl/tangent_base.h" 7 | 8 | #include "manif/impl/so3/SO3_properties.h" 9 | #include "manif/impl/so3/SO3_base.h" 10 | #include "manif/impl/so3/SO3Tangent_base.h" 11 | #include "manif/impl/so3/SO3.h" 12 | #include "manif/impl/so3/SO3Tangent.h" 13 | #include "manif/impl/so3/SO3_map.h" 14 | #include "manif/impl/so3/SO3Tangent_map.h" 15 | 16 | #endif /* _MANIF_SO3_H_ */ 17 | -------------------------------------------------------------------------------- /include/manif/algorithms/bezier.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_BEZIER_H_ 2 | #define _MANIF_MANIF_BEZIER_H_ 3 | 4 | #include "manif/impl/lie_group_base.h" 5 | #include "manif/algorithms/interpolation.h" 6 | 7 | #include 8 | 9 | namespace manif { 10 | 11 | /** 12 | * @brief Curve fitting using the DeCasteljau algorithm 13 | * on Lie groups. 14 | * 15 | * @param trajectory, a discretized trajectory. 16 | * @param degree, the degree of smoothness of the fitted curve. 17 | * @param k_interp, the number of points to interpolate 18 | * between two consecutive points of the trajectory. 19 | * interpolate k_interp for t in ]0,1]. 20 | * @param closed_curve Whether the input trajectory is closed or not. 21 | * If true, the first and the last points of the input trajectory are used 22 | * to interpolate points inbetween. Default false. 23 | * @return The interpolated smooth trajectory 24 | * 25 | * @note A naive implementation of the DeCasteljau algorithm 26 | * on Lie groups. 27 | * 28 | * @link https://www.wikiwand.com/en/De_Casteljau%27s_algorithm 29 | */ 30 | 31 | template 32 | std::vector 33 | computeBezierCurve(const std::vector& control_points, 34 | const unsigned int degree, 35 | const unsigned int k_interp) 36 | { 37 | MANIF_CHECK(control_points.size() > 2, "Oups0"); 38 | MANIF_CHECK(degree <= control_points.size(), "Oups1"); 39 | MANIF_CHECK(k_interp > 0, "Oups2"); 40 | 41 | const unsigned int n_segments = 42 | std::floor(double(control_points.size()-degree)/(degree-1)+1); 43 | 44 | std::vector> segments_control_points; 45 | for (unsigned int t=0; t()); 48 | 49 | // Retrieve control points of the current segment 50 | for (int n=0; n curve; 65 | for (unsigned int s=0; s(t)/(segment_k_interp); 71 | 72 | LieGroup Qc = LieGroup::Identity(); 73 | 74 | // recursive chunk of the algo, 75 | // compute tmp control points. 76 | for (int i=0; ilog() * 79 | polynomialBernstein((double)degree, (double)i, (double)t_01)); 80 | } 81 | 82 | curve.push_back(Qc); 83 | } 84 | } 85 | 86 | return curve; 87 | } 88 | 89 | } /* namespace manif */ 90 | 91 | #endif /* _MANIF_MANIF_BEZIER_H_ */ 92 | -------------------------------------------------------------------------------- /include/manif/autodiff/autodiff.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_AUTODIFF_AUTODIFF_H_ 2 | #define _MANIF_MANIF_AUTODIFF_AUTODIFF_H_ 3 | 4 | #include "manif/autodiff/constants.h" 5 | #include "manif/autodiff/local_parameterization.h" 6 | 7 | namespace manif::internal { 8 | // @note: Unfortunately HigherOrderDual is a non-deducible context 9 | // template 10 | // struct is_ad> : std::integral_constant { }; 11 | 12 | using dual0thf = autodiff::HigherOrderDual<0, float>; 13 | using dual1stf = autodiff::HigherOrderDual<1, float>; 14 | using dual2ndf = autodiff::HigherOrderDual<2, float>; 15 | using dual3rdf = autodiff::HigherOrderDual<3, float>; 16 | using dual4thf = autodiff::HigherOrderDual<4, float>; 17 | 18 | template <> struct is_ad : std::integral_constant { }; 19 | template <> struct is_ad : std::integral_constant { }; 20 | template <> struct is_ad : std::integral_constant { }; 21 | template <> struct is_ad : std::integral_constant { }; 22 | template <> struct is_ad : std::integral_constant { }; 23 | 24 | template <> struct is_ad : std::integral_constant { }; 25 | template <> struct is_ad : std::integral_constant { }; 26 | template <> struct is_ad : std::integral_constant { }; 27 | template <> struct is_ad : std::integral_constant { }; 28 | template <> struct is_ad : std::integral_constant { }; 29 | 30 | template 31 | struct is_ad> : std::integral_constant { }; 32 | } // namespace manif 33 | 34 | namespace autodiff::detail { 35 | /// @brief VectorTraits specialization for Derived of LieGroupBase 36 | template 37 | struct VectorTraits::value>> { 38 | using ValueType = typename T::Scalar; 39 | 40 | template 41 | using ReplaceValueType = typename T::template LieGroupTemplate; 42 | }; 43 | 44 | /// @brief VectorTraits specialization for Derived of TangentBase 45 | template 46 | struct VectorTraits::value>> { 47 | using ValueType = typename T::Scalar; 48 | 49 | template 50 | using ReplaceValueType = typename T::template TangentTemplate; 51 | }; 52 | } // namespace autodiff::detail 53 | 54 | #endif // _MANIF_MANIF_AUTODIFF_AUTODIFF_H_ -------------------------------------------------------------------------------- /include/manif/autodiff/constants.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_AUTODIFF_CONSTANTS_H_ 2 | #define _MANIF_MANIF_AUTODIFF_CONSTANTS_H_ 3 | 4 | namespace { 5 | // size_t without includes. 6 | using size_type = decltype(alignof(char)); 7 | } 8 | 9 | namespace autodiff { 10 | namespace detail { 11 | template struct Dual; 12 | template class Real; 13 | } // namespace detail 14 | } // namespace autodiff 15 | 16 | namespace manif { 17 | 18 | /// @brief Specialize Constants traits for autodiff::Dual type 19 | template 20 | struct Constants> { 21 | static const autodiff::detail::Dual eps; 22 | }; 23 | 24 | template 25 | const autodiff::detail::Dual 26 | Constants>::eps = 27 | autodiff::detail::Dual(Constants::eps); 28 | 29 | /// @brief Specialize Constants traits for autodiff::Real type 30 | template 31 | struct Constants> { 32 | static const autodiff::detail::Real eps; 33 | }; 34 | 35 | template 36 | const autodiff::detail::Real 37 | Constants>::eps = 38 | autodiff::detail::Real(Constants::eps); 39 | 40 | } // namespace manif 41 | 42 | #endif // _MANIF_MANIF_AUTODIFF_CONSTANTS_H_ 43 | -------------------------------------------------------------------------------- /include/manif/autodiff/local_parameterization.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_AUTODIFF_LOCAL_PARAMETRIZATION_H_ 2 | #define _MANIF_MANIF_AUTODIFF_LOCAL_PARAMETRIZATION_H_ 3 | 4 | namespace manif { 5 | 6 | template 7 | Eigen::Matrix 8 | autodiffLocalParameterizationJacobian(const manif::LieGroupBase& _state) { 9 | 10 | using Scalar = typename Derived::Scalar; 11 | using LieGroup = typename Derived::template LieGroupTemplate; 12 | using Tangent = typename Derived::Tangent::template TangentTemplate; 13 | 14 | using Jac = Eigen::Matrix; 15 | 16 | LieGroup state = _state.template cast(); 17 | Tangent delta = Tangent::Zero(); 18 | 19 | LieGroup state_plus_delta; 20 | 21 | auto f = [](const auto& s, const auto& t){ 22 | return s + t; 23 | }; 24 | 25 | Jac J_so_t = autodiff::jacobian( 26 | f, autodiff::wrt(delta), autodiff::at(state, delta), state_plus_delta 27 | ); 28 | 29 | MANIF_ASSERT(state.isApprox(state_plus_delta)); 30 | MANIF_ASSERT(Derived::RepSize == J_so_t.rows()); 31 | MANIF_ASSERT(Derived::DoF == J_so_t.cols()); 32 | 33 | return J_so_t; 34 | } 35 | 36 | } // namespace manif 37 | #endif // _MANIF_MANIF_AUTODIFF_LOCAL_PARAMETRIZATION_H_ -------------------------------------------------------------------------------- /include/manif/ceres/ceres_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_CERES_UTILS_H_ 2 | #define _MANIF_MANIF_CERES_UTILS_H_ 3 | 4 | #if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 5 | #include "manif/ceres/manifold.h" 6 | #else 7 | #include "manif/ceres/local_parametrization.h" 8 | #endif 9 | #include "manif/ceres/objective.h" 10 | #include "manif/ceres/constraint.h" 11 | 12 | #if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 13 | #include 14 | #else 15 | #include 16 | #endif 17 | #include 18 | 19 | namespace manif { 20 | 21 | #if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2 22 | /** 23 | * @brief Helper function to create a Ceres Manifold parameterization wrapper. 24 | * @see CeresManifoldFunctor 25 | */ 26 | template 27 | std::shared_ptr< 28 | ceres::AutoDiffManifold, 29 | _LieGroup::RepSize, _LieGroup::DoF>> 30 | make_manifold_autodiff() 31 | { 32 | return std::make_shared< 33 | ceres::AutoDiffManifold< 34 | CeresManifoldFunctor<_LieGroup>, _LieGroup::RepSize, _LieGroup::DoF>>(); 35 | } 36 | #else 37 | /** 38 | * @brief Helper function to create a Ceres autodiff local parameterization wrapper. 39 | * @see CeresLocalParameterizationFunctor 40 | */ 41 | template 42 | std::shared_ptr< 43 | ceres::AutoDiffLocalParameterization, 44 | _LieGroup::RepSize, _LieGroup::DoF>> 45 | make_local_parameterization_autodiff() 46 | { 47 | return std::make_shared< 48 | ceres::AutoDiffLocalParameterization< 49 | CeresLocalParameterizationFunctor<_LieGroup>, _LieGroup::RepSize, _LieGroup::DoF>>(); 50 | } 51 | #endif 52 | 53 | /** 54 | * @brief Helper function to create a Ceres autodiff objective wrapper. 55 | * @see CeresObjectiveFunctor 56 | */ 57 | template 58 | std::shared_ptr< 59 | ceres::AutoDiffCostFunction< 60 | CeresObjectiveFunctor<_LieGroup>, 1, _LieGroup::RepSize>> 61 | make_objective_autodiff(Args&&... args) 62 | { 63 | return std::make_shared< 64 | ceres::AutoDiffCostFunction, 1, _LieGroup::RepSize>>( 65 | new CeresObjectiveFunctor<_LieGroup>(std::forward(args)...) 66 | ); 67 | } 68 | 69 | /** 70 | * @brief Helper function to create a Ceres autodiff constraint wrapper. 71 | * @see CeresConstraintFunctor 72 | */ 73 | template 74 | std::shared_ptr< 75 | ceres::AutoDiffCostFunction< 76 | CeresConstraintFunctor<_LieGroup>, _LieGroup::DoF, _LieGroup::RepSize, _LieGroup::RepSize>> 77 | make_constraint_autodiff(Args&&... args) 78 | { 79 | return std::make_shared< 80 | ceres::AutoDiffCostFunction< 81 | CeresConstraintFunctor<_LieGroup>, 82 | _LieGroup::DoF, 83 | _LieGroup::RepSize, 84 | _LieGroup::RepSize>>( 85 | new CeresConstraintFunctor<_LieGroup>(std::forward(args)...)); 86 | } 87 | 88 | } /* namespace manif */ 89 | 90 | #endif /* _MANIF_MANIF_CERES_UTILS_H_ */ 91 | -------------------------------------------------------------------------------- /include/manif/ceres/constants.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_CERES_CONSTANTS_H_ 2 | #define _MANIF_MANIF_CERES_CONSTANTS_H_ 3 | 4 | #include "manif/constants.h" 5 | 6 | #include 7 | #include 8 | 9 | namespace manif { 10 | 11 | /// @brief Specialize Constants traits 12 | /// for the ceres::Jet type 13 | template 14 | struct Constants> 15 | { 16 | static const ceres::Jet<_Scalar, N> eps; 17 | }; 18 | 19 | template 20 | const ceres::Jet<_Scalar, N> 21 | Constants>::eps = 22 | ceres::Jet<_Scalar, N>(Constants<_Scalar>::eps); 23 | 24 | } /* namespace manif */ 25 | 26 | #endif /* _MANIF_MANIF_CERES_CONSTANTS_H_ */ 27 | -------------------------------------------------------------------------------- /include/manif/ceres/local_parametrization.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_CERES_LOCAL_PARAMETRIZATION_H_ 2 | #define _MANIF_MANIF_CERES_LOCAL_PARAMETRIZATION_H_ 3 | 4 | #include 5 | 6 | namespace manif { 7 | 8 | /** 9 | * @brief A wrapper for Ceres autodiff local parameterization. 10 | */ 11 | template 12 | class CeresLocalParameterizationFunctor 13 | { 14 | using LieGroup = _LieGroup; 15 | using Tangent = typename _LieGroup::Tangent; 16 | 17 | template 18 | using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>; 19 | 20 | template 21 | using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>; 22 | 23 | public: 24 | 25 | CeresLocalParameterizationFunctor() = default; 26 | virtual ~CeresLocalParameterizationFunctor() = default; 27 | 28 | template 29 | bool operator()(const T* state_raw, 30 | const T* delta_raw, 31 | T* state_plus_delta_raw) const 32 | { 33 | const Eigen::Map> state(state_raw); 34 | const Eigen::Map> delta(delta_raw); 35 | 36 | Eigen::Map> state_plus_delta(state_plus_delta_raw); 37 | 38 | state_plus_delta = state + delta; 39 | 40 | return true; 41 | } 42 | }; 43 | 44 | } /* namespace manif */ 45 | 46 | #endif /* _MANIF_MANIF_CERES_LOCAL_PARAMETRIZATION_H_ */ 47 | -------------------------------------------------------------------------------- /include/manif/ceres/manifold.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_CERES_MANIFOLD_H_ 2 | #define _MANIF_MANIF_CERES_MANIFOLD_H_ 3 | 4 | #include 5 | 6 | namespace manif { 7 | 8 | /** 9 | * @brief A wrapper for Ceres autodiff local parameterization. 10 | */ 11 | template 12 | class CeresManifoldFunctor 13 | { 14 | using LieGroup = _LieGroup; 15 | using Tangent = typename _LieGroup::Tangent; 16 | 17 | template 18 | using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>; 19 | 20 | template 21 | using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>; 22 | 23 | public: 24 | 25 | CeresManifoldFunctor() = default; 26 | virtual ~CeresManifoldFunctor() = default; 27 | 28 | template 29 | bool Plus(const T* state_raw, 30 | const T* delta_raw, 31 | T* state_plus_delta_raw) const 32 | { 33 | const Eigen::Map> state(state_raw); 34 | const Eigen::Map> delta(delta_raw); 35 | 36 | Eigen::Map> state_plus_delta(state_plus_delta_raw); 37 | 38 | state_plus_delta = state + delta; 39 | 40 | return true; 41 | } 42 | 43 | template 44 | bool Minus(const T* y_raw, 45 | const T* x_raw, 46 | T* y_minus_x_raw) const 47 | { 48 | const Eigen::Map> y(y_raw); 49 | const Eigen::Map> x(x_raw); 50 | 51 | Eigen::Map> y_minus_x(y_minus_x_raw); 52 | 53 | y_minus_x = y - x; 54 | 55 | return true; 56 | } 57 | }; 58 | 59 | } /* namespace manif */ 60 | 61 | #endif /* _MANIF_MANIF_CERES_MANIFOLD_H_ */ 62 | -------------------------------------------------------------------------------- /include/manif/ceres/objective.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_CERES_OBJECTIVE_H_ 2 | #define _MANIF_MANIF_CERES_OBJECTIVE_H_ 3 | 4 | #include 5 | 6 | namespace manif { 7 | 8 | template 9 | class CeresObjectiveFunctor 10 | { 11 | using LieGroup = _LieGroup; 12 | using Tangent = typename _LieGroup::Tangent; 13 | 14 | template 15 | using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>; 16 | 17 | public: 18 | 19 | MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND_TYPE(LieGroup) 20 | 21 | template 22 | CeresObjectiveFunctor(Args&&... args) 23 | : target_state_(std::forward(args)...) 24 | { 25 | // 26 | } 27 | 28 | CeresObjectiveFunctor(const LieGroup& target_state, 29 | const double weight = 1) 30 | : weight_(weight) 31 | , target_state_(target_state) 32 | { 33 | // 34 | } 35 | 36 | virtual ~CeresObjectiveFunctor() = default; 37 | 38 | template 39 | bool operator()(const T* const state_raw, T* residuals_raw) const 40 | { 41 | const Eigen::Map> state(state_raw); 42 | 43 | residuals_raw[0] = (target_state_.template cast() - state). 44 | coeffs().norm() * T(weight_); 45 | 46 | /// @todo 47 | /// 48 | /// Jacobian G = q.rjac().transpose() * q.rjac(); 49 | /// residual = (q.coeffs().transpose() * G * q.coeffs()); 50 | /// 51 | /// or 52 | /// 53 | /// residual = (q.coeffs().transpose() * W * q.coeffs()); 54 | 55 | // std::cout << "State:" 56 | // << state_raw[0] << "," << state_raw[1] 57 | // << "\n"; 58 | 59 | // std::cout << "Target:" 60 | // << target_state_.coeffs().transpose() 61 | // << "\n"; 62 | 63 | // std::cout << "residual: " << residuals_raw[0] << "\n"; 64 | 65 | return true; 66 | } 67 | 68 | LieGroup getTargetState() const; 69 | void setTargetState(const LieGroup& target_state) const; 70 | 71 | inline void weight(const double weight) { weight_ = weight; } 72 | inline double weight() const noexcept { return weight_; } 73 | 74 | protected: 75 | 76 | double weight_ = 1; 77 | LieGroup target_state_; 78 | }; 79 | 80 | template 81 | typename CeresObjectiveFunctor<_LieGroup>::LieGroup 82 | CeresObjectiveFunctor<_LieGroup>::getTargetState() const 83 | { 84 | return target_state_; 85 | } 86 | 87 | template 88 | void CeresObjectiveFunctor<_LieGroup>::setTargetState( 89 | const LieGroup& target_state) const 90 | { 91 | target_state_ = target_state; 92 | } 93 | 94 | } /* namespace manif */ 95 | 96 | #endif /* _MANIF_MANIF_CERES_OBJECTIVE_H_ */ 97 | -------------------------------------------------------------------------------- /include/manif/constants.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_CONSTANTS_H_ 2 | #define _MANIF_MANIF_CONSTANTS_H_ 3 | 4 | #include 5 | #include 6 | 7 | #define MANIF_PI 3.141592653589793238462643383279502884 8 | #define MANIF_PI_2 1.570796326794896619231321691639751442 9 | #define MANIF_PI_4 0.785398163397448309615660845819875721 10 | 11 | namespace manif { 12 | namespace internal { 13 | 14 | /** 15 | * Constexpr Newton-Raphson iterative algorithm for the sqrt aprrox. 16 | */ 17 | template 18 | T constexpr sqrtNewtonRaphson(T x, T curr, T prev) 19 | { 20 | return curr == prev 21 | ? curr 22 | : sqrtNewtonRaphson(x, T(0.5) * (curr + x / curr), curr); 23 | } 24 | 25 | /** 26 | * Constexpr version of the square root 27 | * Return value: 28 | * - For a finite and non-negative value of "x", 29 | * returns an approximation for the square root of "x" 30 | * - Otherwise, returns NaN 31 | * 32 | * credits : https://stackoverflow.com/a/34134071/9709397 33 | */ 34 | template 35 | T constexpr csqrt(T x) 36 | { 37 | return x >= T(0) && x < std::numeric_limits::infinity() 38 | ? sqrtNewtonRaphson(x, x, T(0)) 39 | : std::numeric_limits::quiet_NaN(); 40 | } 41 | 42 | } /* namespace internal */ 43 | 44 | /** 45 | * @brief Traits to define some constant scalar. 46 | */ 47 | template 48 | struct Constants 49 | { 50 | static constexpr _Scalar eps = std::numeric_limits<_Scalar>::epsilon()*_Scalar(100); 51 | static constexpr _Scalar eps_sqrt = internal::csqrt(eps); 52 | 53 | static constexpr _Scalar to_rad = _Scalar(MANIF_PI / 180.0); 54 | static constexpr _Scalar to_deg = _Scalar(180.0 / MANIF_PI); 55 | }; 56 | 57 | template 58 | constexpr _Scalar Constants<_Scalar>::eps; 59 | template 60 | constexpr _Scalar Constants<_Scalar>::eps_sqrt; 61 | template 62 | constexpr _Scalar Constants<_Scalar>::to_rad; 63 | template 64 | constexpr _Scalar Constants<_Scalar>::to_deg; 65 | 66 | } /* namespace manif */ 67 | 68 | #endif /* _MANIF_MANIF_CONSTANTS_H_ */ 69 | -------------------------------------------------------------------------------- /include/manif/impl/assignment_assert.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_ 2 | #define _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_ 3 | 4 | namespace manif { 5 | namespace internal { 6 | 7 | template 8 | struct AssignmentEvaluatorImpl 9 | { 10 | template static void run_impl(const T&) { } 11 | }; 12 | 13 | template 14 | struct AssignmentEvaluator : AssignmentEvaluatorImpl 15 | { 16 | using Base = AssignmentEvaluatorImpl; 17 | 18 | template void run(T&& t) { Base::run_impl(std::forward(t)); } 19 | }; 20 | 21 | } // namespace internal 22 | } // namespace manif 23 | 24 | #endif // _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_ 25 | -------------------------------------------------------------------------------- /include/manif/impl/bracket.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_IMPL_BRACKET_H_ 2 | #define _MANIF_MANIF_IMPL_BRACKET_H_ 3 | 4 | namespace manif { 5 | namespace internal { 6 | 7 | template 8 | struct BracketEvaluatorImpl { 9 | template 10 | static typename Derived::Tangent run(const TL& a, const TR& b) { 11 | return a.smallAdj() * b; 12 | } 13 | }; 14 | 15 | template 16 | struct BracketEvaluator : BracketEvaluatorImpl { 17 | using Base = BracketEvaluatorImpl; 18 | 19 | BracketEvaluator(const Derived& xptr, const DerivedOther& xptr_o) 20 | : xptr_(xptr), xptr_o_(xptr_o) {} 21 | 22 | typename Derived::Tangent run() { 23 | return Base::run(xptr_, xptr_o_); 24 | } 25 | 26 | protected: 27 | 28 | const Derived& xptr_; 29 | const DerivedOther& xptr_o_; 30 | }; 31 | 32 | } // namespace internal 33 | } // namespace manif 34 | 35 | #endif // _MANIF_MANIF_IMPL_BRACKET_H_ 36 | -------------------------------------------------------------------------------- /include/manif/impl/bundle/BundleTangent_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_BUNDLETANGENT_MAP_H_ 2 | #define _MANIF_MANIF_BUNDLETANGENT_MAP_H_ 3 | 4 | #include "manif/impl/bundle/BundleTangent.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | /** 10 | * @brief traits specialization for Eigen Map 11 | */ 12 | template class ... T> 13 | struct traits, 0>> 14 | : public traits> 15 | { 16 | using typename traits>::Scalar; 17 | using traits>::DoF; 18 | using Base = BundleTangentBase, 0>>; 19 | using DataType = Eigen::Map, 0>; 20 | }; 21 | 22 | /** 23 | * @brief traits specialization for Eigen const Map 24 | */ 25 | template class ... T> 26 | struct traits, 0>> 27 | : public traits> 28 | { 29 | using typename traits>::Scalar; 30 | using traits>::DoF; 31 | using Base = BundleTangentBase, 0>>; 32 | using DataType = Eigen::Map, 0>; 33 | }; 34 | 35 | } // namespace internal 36 | } // namespace manif 37 | 38 | 39 | namespace Eigen { 40 | 41 | /** 42 | * @brief Specialization of Map for manif::Bundle 43 | */ 44 | template class ... T> 45 | class Map, 0> 46 | : public manif::BundleTangentBase, 0>> 47 | { 48 | using Base = manif::BundleTangentBase, 0>>; 49 | 50 | public: 51 | 52 | MANIF_TANGENT_TYPEDEF 53 | MANIF_INHERIT_TANGENT_API 54 | MANIF_INHERIT_TANGENT_OPERATOR 55 | 56 | using Base::BundleSize; 57 | 58 | Map(Scalar * coeffs) : data_(coeffs) { } 59 | 60 | MANIF_TANGENT_MAP_ASSIGN_OP(BundleTangent) 61 | 62 | DataType & coeffs() {return data_;} 63 | 64 | const DataType & coeffs() const {return data_;} 65 | 66 | protected: 67 | 68 | DataType data_; 69 | }; 70 | 71 | /** 72 | * @brief Specialization of Map for const manif::BundleTangent 73 | */ 74 | template class ... T> 75 | class Map, 0> 76 | : public manif::BundleTangentBase, 0>> 77 | { 78 | using Base = manif::BundleTangentBase, 0>>; 79 | 80 | public: 81 | 82 | MANIF_TANGENT_TYPEDEF 83 | MANIF_INHERIT_TANGENT_API 84 | MANIF_INHERIT_TANGENT_OPERATOR 85 | 86 | using Base::BundleSize; 87 | 88 | Map(const Scalar * coeffs) : data_(coeffs) { } 89 | 90 | const DataType & coeffs() const {return data_;} 91 | 92 | protected: 93 | 94 | const DataType data_; 95 | }; 96 | 97 | } // namespace Eigen 98 | 99 | #endif // _MANIF_MANIF_BUNDLETANGENT_MAP_H_ 100 | -------------------------------------------------------------------------------- /include/manif/impl/bundle/Bundle_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_BUNDLE_MAP_H_ 2 | #define _MANIF_MANIF_BUNDLE_MAP_H_ 3 | 4 | #include "manif/impl/bundle/Bundle.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | /** 10 | * @brief traits specialization for Eigen Map 11 | */ 12 | template class ... T> 13 | struct traits, 0>> 14 | : public traits> 15 | { 16 | using typename traits>::Scalar; 17 | using traits>::RepSize; 18 | using Base = BundleBase, 0>>; 19 | using DataType = Eigen::Map, 0>; 20 | }; 21 | 22 | /** 23 | * @brief traits specialization for Eigen const Map 24 | */ 25 | template class ... T> 26 | struct traits, 0>> 27 | : public traits> 28 | { 29 | using typename traits>::Scalar; 30 | using traits>::RepSize; 31 | using Base = BundleBase, 0>>; 32 | using DataType = Eigen::Map, 0>; 33 | }; 34 | 35 | } // namespace internal 36 | } // namespace manif 37 | 38 | 39 | namespace Eigen { 40 | 41 | /** 42 | * @brief Specialization of Map for manif::Bundle 43 | */ 44 | template class ... T> 45 | class Map, 0> 46 | : public manif::BundleBase, 0>> 47 | { 48 | using Base = manif::BundleBase, 0>>; 49 | 50 | public: 51 | 52 | MANIF_COMPLETE_GROUP_TYPEDEF 53 | MANIF_INHERIT_GROUP_API 54 | 55 | using Base::BundleSize; 56 | 57 | Map(Scalar * coeffs) : data_(coeffs) { } 58 | 59 | MANIF_GROUP_MAP_ASSIGN_OP(Bundle) 60 | 61 | DataType & coeffs() {return data_;} 62 | 63 | const DataType & coeffs() const {return data_;} 64 | 65 | protected: 66 | 67 | DataType data_; 68 | }; 69 | 70 | /** 71 | * @brief Specialization of Map for const manif::Bundle 72 | */ 73 | template class ... T> 74 | class Map, 0> 75 | : public manif::BundleBase, 0>> 76 | { 77 | using Base = manif::BundleBase, 0>>; 78 | 79 | public: 80 | 81 | MANIF_COMPLETE_GROUP_TYPEDEF 82 | MANIF_INHERIT_GROUP_API 83 | 84 | using Base::BundleSize; 85 | 86 | Map(const Scalar * coeffs) : data_(coeffs) { } 87 | 88 | const DataType & coeffs() const {return data_;} 89 | 90 | protected: 91 | 92 | const DataType data_; 93 | }; 94 | 95 | } // namespace Eigen 96 | 97 | #endif // _MANIF_MANIF_BUNDLE_MAP_H_ 98 | -------------------------------------------------------------------------------- /include/manif/impl/bundle/Bundle_properties.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_BUNDLE_PROPERTIES_H_ 2 | #define _MANIF_MANIF_BUNDLE_PROPERTIES_H_ 3 | 4 | #include "manif/impl/traits.h" 5 | 6 | namespace manif { 7 | 8 | // Forward declaration for type traits specialization 9 | template struct BundleBase; 10 | template struct BundleTangentBase; 11 | 12 | namespace internal { 13 | 14 | //! traits specialization 15 | template 16 | struct LieGroupProperties> 17 | { 18 | static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension 19 | static constexpr int DoF = traits<_Derived>::DoF; /// @brief Degrees of freedom 20 | }; 21 | 22 | //! traits specialization 23 | template 24 | struct LieGroupProperties> 25 | { 26 | static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension 27 | static constexpr int DoF = traits<_Derived>::DoF; /// @brief Degrees of freedom 28 | }; 29 | 30 | } // namespace internal 31 | } // namespace manif 32 | 33 | #endif // _MANIF_MANIF_BUNDLE_PROPERTIES_H_ 34 | -------------------------------------------------------------------------------- /include/manif/impl/cast.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_IMPL_CAST_H_ 2 | #define _MANIF_MANIF_IMPL_CAST_H_ 3 | 4 | namespace manif { 5 | namespace internal { 6 | 7 | template 8 | struct CastEvaluatorImpl { 9 | template 10 | static auto run(const T& o) -> typename T::template LieGroupTemplate { 11 | return typename T::template LieGroupTemplate( 12 | o.coeffs().template cast() 13 | ); 14 | } 15 | }; 16 | 17 | template 18 | struct CastEvaluator : CastEvaluatorImpl { 19 | using Base = CastEvaluatorImpl; 20 | 21 | CastEvaluator(const Derived& xptr) : xptr_(xptr) {} 22 | 23 | auto run() const -> typename Derived::template LieGroupTemplate { 24 | return Base::run(xptr_); 25 | } 26 | 27 | protected: 28 | 29 | const Derived& xptr_; 30 | }; 31 | 32 | } // namespace internal 33 | } // namespace manif 34 | 35 | #endif // _MANIF_MANIF_IMPL_CAST_H_ 36 | -------------------------------------------------------------------------------- /include/manif/impl/generator.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_GENERATOR_H_ 2 | #define _MANIF_MANIF_GENERATOR_H_ 3 | 4 | namespace manif { 5 | namespace internal { 6 | 7 | template 8 | struct GeneratorEvaluator 9 | { 10 | static typename Derived::LieAlg 11 | run(const unsigned int) 12 | { 13 | /// @todo print actual Derived type 14 | static_assert(constexpr_false(), 15 | "GeneratorEvaluator not overloaded for Derived type!"); 16 | } 17 | }; 18 | 19 | template 20 | struct InnerWeightsEvaluator 21 | { 22 | static typename Derived::InnerWeightsMatrix 23 | run() 24 | { 25 | using InnerWeightsMatrix = typename Derived::InnerWeightsMatrix; 26 | 27 | auto computeW = []() 28 | { 29 | InnerWeightsMatrix W = InnerWeightsMatrix::Zero(); 30 | 31 | for (int r = 0; r 8 | struct RandomEvaluatorImpl 9 | { 10 | template 11 | static void run(T&) 12 | { 13 | /// @todo print actual Derived type 14 | static_assert(constexpr_false(), 15 | "RandomEvaluator not overloaded for Derived type!"); 16 | } 17 | }; 18 | 19 | template 20 | struct RandomEvaluator : RandomEvaluatorImpl 21 | { 22 | using Base = RandomEvaluatorImpl; 23 | 24 | RandomEvaluator(Derived& xptr) : xptr_(xptr) {} 25 | 26 | void run() 27 | { 28 | Base::run(xptr_); 29 | } 30 | 31 | protected: 32 | 33 | Derived& xptr_; 34 | }; 35 | 36 | } /* namespace internal */ 37 | } /* namespace manif */ 38 | 39 | #endif /* _MANIF_MANIF_IMPL_RANDOM_H_ */ 40 | -------------------------------------------------------------------------------- /include/manif/impl/rn/RnTangent_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_RNTANGENT_MAP_H_ 2 | #define _MANIF_MANIF_RNTANGENT_MAP_H_ 3 | 4 | #include "manif/impl/rn/RnTangent.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::DoF; 16 | using DataType = ::Eigen::Map, 0>; 17 | using Base = RnTangentBase, 0>>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::DoF; 27 | using DataType = ::Eigen::Map, 0>; 28 | using Base = RnTangentBase, 0>>; 29 | }; 30 | 31 | } // namespace internal 32 | } // namespace manif 33 | 34 | namespace Eigen { 35 | 36 | //! @brief Specialization of Map for manif::RnTangent 37 | template 38 | class Map, 0> 39 | : public manif::RnTangentBase, 0> > 40 | { 41 | using Base = manif::RnTangentBase, 0> >; 42 | 43 | public: 44 | 45 | MANIF_TANGENT_TYPEDEF 46 | MANIF_INHERIT_TANGENT_API 47 | MANIF_INHERIT_TANGENT_OPERATOR 48 | 49 | Map(Scalar* coeffs) : data_(coeffs) { } 50 | 51 | MANIF_TANGENT_MAP_ASSIGN_OP(RnTangent) 52 | 53 | DataType& coeffs() { return data_; } 54 | const DataType& coeffs() const { return data_; } 55 | 56 | protected: 57 | 58 | DataType data_; 59 | }; 60 | 61 | //! @brief Specialization of Map for const manif::RnTangent 62 | template 63 | class Map, 0> 64 | : public manif::RnTangentBase, 0> > 65 | { 66 | using Base = manif::RnTangentBase, 0> >; 67 | 68 | public: 69 | 70 | MANIF_TANGENT_TYPEDEF 71 | MANIF_INHERIT_TANGENT_API 72 | MANIF_INHERIT_TANGENT_OPERATOR 73 | 74 | Map(const Scalar* coeffs) : data_(coeffs) { } 75 | 76 | const DataType& coeffs() const { return data_; } 77 | 78 | protected: 79 | 80 | const DataType data_; 81 | }; 82 | 83 | } // namespace Eigen 84 | 85 | #endif // _MANIF_MANIF_RNTANGENT_MAP_H_ 86 | -------------------------------------------------------------------------------- /include/manif/impl/rn/Rn_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_RN_MAP_H_ 2 | #define _MANIF_MANIF_RN_MAP_H_ 3 | 4 | #include "manif/impl/rn/Rn.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::RepSize; 16 | using Base = RnBase, 0>>; 17 | using DataType = Eigen::Map, 0>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::RepSize; 27 | using Base = RnBase, 0>>; 28 | using DataType = Eigen::Map, 0>; 29 | }; 30 | 31 | } // namespace internal 32 | } // namespace manif 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::Rn 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::RnBase, 0> > 42 | { 43 | using Base = manif::RnBase, 0> >; 44 | 45 | public: 46 | 47 | MANIF_COMPLETE_GROUP_TYPEDEF 48 | MANIF_INHERIT_GROUP_API 49 | 50 | Map(Scalar* coeffs) : data_(coeffs) { } 51 | 52 | MANIF_GROUP_MAP_ASSIGN_OP(Rn) 53 | 54 | DataType& coeffs() { return data_; } 55 | const DataType& coeffs() const { return data_; } 56 | 57 | protected: 58 | 59 | DataType data_; 60 | }; 61 | 62 | /** 63 | * @brief Specialization of Map for const manif::Rn 64 | */ 65 | template 66 | class Map, 0> 67 | : public manif::RnBase, 0> > 68 | { 69 | using Base = manif::RnBase, 0> >; 70 | 71 | public: 72 | 73 | MANIF_COMPLETE_GROUP_TYPEDEF 74 | MANIF_INHERIT_GROUP_API 75 | 76 | Map(const Scalar* coeffs) : data_(coeffs) { } 77 | 78 | const DataType& coeffs() const { return data_; } 79 | 80 | protected: 81 | 82 | const DataType data_; 83 | }; 84 | 85 | } // namespace Eigen 86 | 87 | #endif // _MANIF_MANIF_RN_MAP_H_ 88 | -------------------------------------------------------------------------------- /include/manif/impl/rn/Rn_properties.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_RN_PROPERTIES_H_ 2 | #define _MANIF_MANIF_RN_PROPERTIES_H_ 3 | 4 | #include "manif/impl/traits.h" 5 | 6 | namespace manif { 7 | 8 | // Forward declaration 9 | template struct RnBase; 10 | template struct RnTangentBase; 11 | 12 | namespace internal { 13 | 14 | //! traits specialization 15 | template 16 | struct LieGroupProperties> 17 | { 18 | static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension 19 | static constexpr int DoF = traits<_Derived>::Dim; /// @brief Degrees of freedom 20 | }; 21 | 22 | //! traits specialization 23 | template 24 | struct LieGroupProperties> 25 | { 26 | static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension 27 | static constexpr int DoF = traits<_Derived>::Dim; /// @brief Degrees of freedom 28 | }; 29 | 30 | } // namespace internal 31 | } // namespace manif 32 | 33 | #endif // _MANIF_MANIF_RN_PROPERTIES_H_ 34 | -------------------------------------------------------------------------------- /include/manif/impl/se2/SE2Tangent.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE2TANGENT_H_ 2 | #define _MANIF_MANIF_SE2TANGENT_H_ 3 | 4 | #include "manif/impl/se2/SE2Tangent_base.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! Traits specialization 10 | template 11 | struct traits> 12 | { 13 | using Scalar = _Scalar; 14 | 15 | using LieGroup = SE2<_Scalar>; 16 | using Tangent = SE2Tangent<_Scalar>; 17 | 18 | using Base = SE2TangentBase; 19 | 20 | static constexpr int Dim = LieGroupProperties::Dim; 21 | static constexpr int DoF = LieGroupProperties::DoF; 22 | static constexpr int RepSize = DoF; 23 | 24 | using DataType = Eigen::Matrix; 25 | 26 | using Jacobian = Eigen::Matrix; 27 | using LieAlg = Eigen::Matrix; 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | namespace manif { 34 | 35 | // 36 | // Tangent 37 | // 38 | 39 | /** 40 | * @brief Represent an element of the tangent space of SE2. 41 | */ 42 | template 43 | struct SE2Tangent : SE2TangentBase> 44 | { 45 | private: 46 | 47 | using Base = SE2TangentBase>; 48 | using Type = SE2Tangent<_Scalar>; 49 | 50 | protected: 51 | 52 | using Base::derived; 53 | 54 | public: 55 | 56 | MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND 57 | 58 | MANIF_TANGENT_TYPEDEF 59 | MANIF_INHERIT_TANGENT_API 60 | MANIF_INHERIT_TANGENT_OPERATOR 61 | 62 | SE2Tangent() = default; 63 | ~SE2Tangent() = default; 64 | 65 | MANIF_COPY_CONSTRUCTOR(SE2Tangent) 66 | MANIF_MOVE_CONSTRUCTOR(SE2Tangent) 67 | 68 | // Copy constructor given base 69 | template 70 | SE2Tangent(const TangentBase<_DerivedOther>& o); 71 | 72 | MANIF_TANGENT_ASSIGN_OP(SE2Tangent) 73 | 74 | SE2Tangent(const Scalar x, const Scalar y, const Scalar theta); 75 | 76 | // Tangent common API 77 | 78 | DataType& coeffs(); 79 | const DataType& coeffs() const; 80 | 81 | // SE2Tangent specific API 82 | 83 | using Base::angle; 84 | 85 | protected: 86 | 87 | DataType data_; 88 | }; 89 | 90 | MANIF_EXTRA_TANGENT_TYPEDEF(SE2Tangent); 91 | 92 | template 93 | template 94 | SE2Tangent<_Scalar>::SE2Tangent(const TangentBase<_DerivedOther>& o) 95 | : data_(o.coeffs()) 96 | { 97 | // 98 | } 99 | 100 | template 101 | SE2Tangent<_Scalar>::SE2Tangent(const Scalar x, 102 | const Scalar y, 103 | const Scalar theta) 104 | : SE2Tangent(DataType(x, y, theta)) 105 | { 106 | // 107 | } 108 | 109 | template 110 | typename SE2Tangent<_Scalar>::DataType& 111 | SE2Tangent<_Scalar>::coeffs() 112 | { 113 | return data_; 114 | } 115 | 116 | template 117 | const typename SE2Tangent<_Scalar>::DataType& 118 | SE2Tangent<_Scalar>::coeffs() const 119 | { 120 | return data_; 121 | } 122 | 123 | } /* namespace manif */ 124 | 125 | #endif /* _MANIF_MANIF_SE2TANGENT_H_ */ 126 | -------------------------------------------------------------------------------- /include/manif/impl/se2/SE2Tangent_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE2TANGENT_MAP_H_ 2 | #define _MANIF_MANIF_SE2TANGENT_MAP_H_ 3 | 4 | #include "manif/impl/se2/SE2Tangent.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::DoF; 16 | using DataType = ::Eigen::Map, 0>; 17 | using Base = SE2TangentBase, 0>>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::DoF; 27 | using DataType = ::Eigen::Map, 0>; 28 | using Base = SE2TangentBase, 0>>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::SE2 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::SE2TangentBase, 0> > 42 | { 43 | using Base = manif::SE2TangentBase, 0> >; 44 | 45 | public: 46 | 47 | MANIF_TANGENT_TYPEDEF 48 | MANIF_INHERIT_TANGENT_API 49 | MANIF_INHERIT_TANGENT_OPERATOR 50 | 51 | Map(Scalar* coeffs) : data_(coeffs) { } 52 | 53 | MANIF_TANGENT_MAP_ASSIGN_OP(SE2Tangent) 54 | 55 | DataType& coeffs() { return data_; } 56 | const DataType& coeffs() const { return data_; } 57 | 58 | protected: 59 | 60 | DataType data_; 61 | }; 62 | 63 | /** 64 | * @brief Specialization of Map for const manif::SE2 65 | */ 66 | template 67 | class Map, 0> 68 | : public manif::SE2TangentBase, 0> > 69 | { 70 | using Base = manif::SE2TangentBase, 0> >; 71 | 72 | public: 73 | 74 | MANIF_TANGENT_TYPEDEF 75 | MANIF_INHERIT_TANGENT_API 76 | MANIF_INHERIT_TANGENT_OPERATOR 77 | 78 | Map(const Scalar* coeffs) : data_(coeffs) { } 79 | 80 | const DataType& coeffs() const { return data_; } 81 | 82 | protected: 83 | 84 | const DataType data_; 85 | }; 86 | 87 | } /* namespace Eigen */ 88 | 89 | #endif /* _MANIF_MANIF_SE2TANGENT_MAP_H_ */ 90 | -------------------------------------------------------------------------------- /include/manif/impl/se2/SE2_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE2_MAP_H_ 2 | #define _MANIF_MANIF_SE2_MAP_H_ 3 | 4 | #include "manif/impl/se2/SE2.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::RepSize; 16 | using Base = SE2Base, 0>>; 17 | using DataType = Eigen::Map, 0>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::RepSize; 27 | using Base = SE2Base, 0>>; 28 | using DataType = Eigen::Map, 0>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::SE2 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::SE2Base, 0> > 42 | { 43 | using Base = manif::SE2Base, 0> >; 44 | 45 | public: 46 | 47 | MANIF_COMPLETE_GROUP_TYPEDEF 48 | MANIF_INHERIT_GROUP_API 49 | using Base::transform; 50 | using Base::rotation; 51 | 52 | Map(Scalar* coeffs) : data_(coeffs) { } 53 | 54 | MANIF_GROUP_MAP_ASSIGN_OP(SE2) 55 | 56 | DataType& coeffs() { return data_; } 57 | const DataType& coeffs() const { return data_; } 58 | 59 | using Base::angle; 60 | using Base::real; 61 | using Base::imag; 62 | using Base::x; 63 | using Base::y; 64 | 65 | protected: 66 | 67 | DataType data_; 68 | }; 69 | 70 | /** 71 | * @brief Specialization of Map for const manif::SE2 72 | */ 73 | template 74 | class Map, 0> 75 | : public manif::SE2Base, 0> > 76 | { 77 | using Base = manif::SE2Base, 0> >; 78 | 79 | public: 80 | 81 | MANIF_COMPLETE_GROUP_TYPEDEF 82 | MANIF_INHERIT_GROUP_API 83 | using Base::transform; 84 | using Base::rotation; 85 | 86 | Map(const Scalar* coeffs) : data_(coeffs) { } 87 | 88 | const DataType& coeffs() const { return data_; } 89 | 90 | using Base::angle; 91 | using Base::real; 92 | using Base::imag; 93 | using Base::x; 94 | using Base::y; 95 | 96 | protected: 97 | 98 | const DataType data_; 99 | }; 100 | 101 | } /* namespace Eigen */ 102 | 103 | #endif /* _MANIF_MANIF_SE2_MAP_H_ */ 104 | -------------------------------------------------------------------------------- /include/manif/impl/se2/SE2_properties.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE2_PROPERTIES_H_ 2 | #define _MANIF_MANIF_SE2_PROPERTIES_H_ 3 | 4 | #include "manif/impl/traits.h" 5 | 6 | namespace manif { 7 | 8 | // Forward declaration 9 | template struct SE2Base; 10 | template struct SE2TangentBase; 11 | 12 | namespace internal { 13 | 14 | //! traits specialization 15 | template 16 | struct LieGroupProperties> 17 | { 18 | static constexpr int Dim = 2; /// @brief Space dimension 19 | static constexpr int DoF = 3; /// @brief Degrees of freedom 20 | }; 21 | 22 | //! traits specialization 23 | template 24 | struct LieGroupProperties> 25 | { 26 | static constexpr int Dim = 2; /// @brief Space dimension 27 | static constexpr int DoF = 3; /// @brief Degrees of freedom 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | #endif /* _MANIF_MANIF_SE2_PROPERTIES_H_ */ 34 | -------------------------------------------------------------------------------- /include/manif/impl/se3/SE3Tangent.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE3TANGENT_H_ 2 | #define _MANIF_MANIF_SE3TANGENT_H_ 3 | 4 | #include "manif/impl/se3/SE3Tangent_base.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! Traits specialization 10 | template 11 | struct traits> 12 | { 13 | using Scalar = _Scalar; 14 | 15 | using LieGroup = SE3<_Scalar>; 16 | using Tangent = SE3Tangent<_Scalar>; 17 | 18 | using Base = SE3TangentBase; 19 | 20 | static constexpr int Dim = LieGroupProperties::Dim; 21 | static constexpr int DoF = LieGroupProperties::DoF; 22 | static constexpr int RepSize = DoF; 23 | 24 | using DataType = Eigen::Matrix; 25 | 26 | using Jacobian = Eigen::Matrix; 27 | using LieAlg = Eigen::Matrix; 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | namespace manif { 34 | 35 | // 36 | // Tangent 37 | // 38 | 39 | /** 40 | * @brief Represents an element of tangent space of SE3. 41 | */ 42 | template 43 | struct SE3Tangent : SE3TangentBase> 44 | { 45 | private: 46 | 47 | using Base = SE3TangentBase>; 48 | using Type = SE3Tangent<_Scalar>; 49 | 50 | protected: 51 | 52 | using Base::derived; 53 | 54 | public: 55 | 56 | MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND 57 | 58 | MANIF_TANGENT_TYPEDEF 59 | MANIF_INHERIT_TANGENT_API 60 | MANIF_INHERIT_TANGENT_OPERATOR 61 | 62 | SE3Tangent() = default; 63 | ~SE3Tangent() = default; 64 | 65 | MANIF_COPY_CONSTRUCTOR(SE3Tangent) 66 | MANIF_MOVE_CONSTRUCTOR(SE3Tangent) 67 | 68 | // Copy constructor given base 69 | template 70 | SE3Tangent(const TangentBase<_DerivedOther>& o); 71 | 72 | MANIF_TANGENT_ASSIGN_OP(SE3Tangent) 73 | 74 | // Tangent common API 75 | 76 | DataType& coeffs(); 77 | const DataType& coeffs() const; 78 | 79 | // SE3Tangent specific API 80 | 81 | protected: 82 | 83 | DataType data_; 84 | }; 85 | 86 | MANIF_EXTRA_TANGENT_TYPEDEF(SE3Tangent); 87 | 88 | template 89 | template 90 | SE3Tangent<_Scalar>::SE3Tangent( 91 | const TangentBase<_DerivedOther>& o) 92 | : data_(o.coeffs()) 93 | { 94 | // 95 | } 96 | 97 | template 98 | typename SE3Tangent<_Scalar>::DataType& 99 | SE3Tangent<_Scalar>::coeffs() 100 | { 101 | return data_; 102 | } 103 | 104 | template 105 | const typename SE3Tangent<_Scalar>::DataType& 106 | SE3Tangent<_Scalar>::coeffs() const 107 | { 108 | return data_; 109 | } 110 | 111 | } /* namespace manif */ 112 | 113 | #endif /* _MANIF_MANIF_SE3TANGENT_H_ */ 114 | -------------------------------------------------------------------------------- /include/manif/impl/se3/SE3Tangent_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE3TANGENT_MAP_H_ 2 | #define _MANIF_MANIF_SE3TANGENT_MAP_H_ 3 | 4 | #include "manif/impl/se3/SE3Tangent.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::DoF; 16 | using DataType = Eigen::Map, 0>; 17 | using Base = SE3TangentBase, 0>>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::DoF; 27 | using DataType = Eigen::Map, 0>; 28 | using Base = SE3TangentBase, 0>>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::SE3 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::SE3TangentBase, 0> > 42 | { 43 | using Base = manif::SE3TangentBase, 0> >; 44 | 45 | public: 46 | 47 | MANIF_TANGENT_TYPEDEF 48 | MANIF_INHERIT_TANGENT_API 49 | MANIF_INHERIT_TANGENT_OPERATOR 50 | 51 | Map(Scalar* coeffs) : data_(coeffs) { } 52 | 53 | MANIF_TANGENT_MAP_ASSIGN_OP(SE3Tangent) 54 | 55 | DataType& coeffs() { return data_; } 56 | const DataType& coeffs() const { return data_; } 57 | 58 | protected: 59 | 60 | DataType data_; 61 | }; 62 | 63 | /** 64 | * @brief Specialization of Map for const manif::SE3 65 | */ 66 | template 67 | class Map, 0> 68 | : public manif::SE3TangentBase, 0> > 69 | { 70 | using Base = manif::SE3TangentBase, 0> >; 71 | 72 | public: 73 | 74 | MANIF_TANGENT_TYPEDEF 75 | MANIF_INHERIT_TANGENT_API 76 | MANIF_INHERIT_TANGENT_OPERATOR 77 | 78 | Map(const Scalar* coeffs) : data_(coeffs) { } 79 | 80 | const DataType& coeffs() const { return data_; } 81 | 82 | protected: 83 | 84 | const DataType data_; 85 | }; 86 | 87 | } /* namespace Eigen */ 88 | 89 | #endif /* _MANIF_MANIF_SE3TANGENT_MAP_H_ */ 90 | -------------------------------------------------------------------------------- /include/manif/impl/se3/SE3_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE3_MAP_H_ 2 | #define _MANIF_MANIF_SE3_MAP_H_ 3 | 4 | #include "manif/impl/se3/SE3.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::RepSize; 16 | using Base = SE3Base, 0>>; 17 | using DataType = Eigen::Map, 0>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::RepSize; 27 | using Base = SE3Base, 0>>; 28 | using DataType = Eigen::Map, 0>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::SE3 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::SE3Base, 0> > 42 | { 43 | using Base = manif::SE3Base, 0> >; 44 | 45 | public: 46 | 47 | MANIF_COMPLETE_GROUP_TYPEDEF 48 | MANIF_INHERIT_GROUP_API 49 | using Base::transform; 50 | using Base::rotation; 51 | 52 | Map(Scalar* coeffs) : data_(coeffs) { } 53 | 54 | MANIF_GROUP_MAP_ASSIGN_OP(SE3) 55 | 56 | DataType& coeffs() { return data_; } 57 | const DataType& coeffs() const { return data_; } 58 | 59 | protected: 60 | 61 | DataType data_; 62 | }; 63 | 64 | /** 65 | * @brief Specialization of Map for const manif::SE3 66 | */ 67 | template 68 | class Map, 0> 69 | : public manif::SE3Base, 0> > 70 | { 71 | using Base = manif::SE3Base, 0> >; 72 | 73 | public: 74 | 75 | MANIF_COMPLETE_GROUP_TYPEDEF 76 | MANIF_INHERIT_GROUP_API 77 | using Base::transform; 78 | using Base::rotation; 79 | 80 | Map(const Scalar* coeffs) : data_(coeffs) { } 81 | 82 | const DataType& coeffs() const { return data_; } 83 | 84 | protected: 85 | 86 | const DataType data_; 87 | }; 88 | 89 | } /* namespace Eigen */ 90 | 91 | #endif /* _MANIF_MANIF_SE3_MAP_H_ */ 92 | -------------------------------------------------------------------------------- /include/manif/impl/se3/SE3_properties.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE3_PROPERTIES_H_ 2 | #define _MANIF_MANIF_SE3_PROPERTIES_H_ 3 | 4 | #include "manif/impl/traits.h" 5 | 6 | namespace manif { 7 | 8 | // Forward declaration 9 | template struct SE3Base; 10 | template struct SE3TangentBase; 11 | 12 | namespace internal { 13 | 14 | //! traits specialization 15 | template 16 | struct LieGroupProperties> 17 | { 18 | static constexpr int Dim = 3; /// @brief Space dimension 19 | static constexpr int DoF = 6; /// @brief Degrees of freedom 20 | }; 21 | 22 | //! traits specialization 23 | template 24 | struct LieGroupProperties> 25 | { 26 | static constexpr int Dim = 3; /// @brief Space dimension 27 | static constexpr int DoF = 6; /// @brief Degrees of freedom 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | #endif /* _MANIF_MANIF_SE3_PROPERTIES_H_ */ 34 | -------------------------------------------------------------------------------- /include/manif/impl/se_2_3/SE_2_3Tangent.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE_2_3TANGENT_H_ 2 | #define _MANIF_MANIF_SE_2_3TANGENT_H_ 3 | 4 | #include "manif/impl/se_2_3/SE_2_3Tangent_base.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! Traits specialization 10 | template 11 | struct traits> 12 | { 13 | using Scalar = _Scalar; 14 | 15 | using LieGroup = SE_2_3<_Scalar>; 16 | using Tangent = SE_2_3Tangent<_Scalar>; 17 | 18 | using Base = SE_2_3TangentBase; 19 | 20 | static constexpr int Dim = LieGroupProperties::Dim; 21 | static constexpr int DoF = LieGroupProperties::DoF; 22 | static constexpr int RepSize = DoF; 23 | 24 | using DataType = Eigen::Matrix; 25 | 26 | using Jacobian = Eigen::Matrix; 27 | using LieAlg = Eigen::Matrix; 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | namespace manif { 34 | 35 | // 36 | // Tangent 37 | // 38 | 39 | /** 40 | * @brief Represents an element of tangent space of SE_2_3. 41 | */ 42 | template 43 | struct SE_2_3Tangent : SE_2_3TangentBase> 44 | { 45 | private: 46 | 47 | using Base = SE_2_3TangentBase>; 48 | using Type = SE_2_3Tangent<_Scalar>; 49 | 50 | protected: 51 | 52 | using Base::derived; 53 | 54 | public: 55 | 56 | MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND 57 | 58 | MANIF_TANGENT_TYPEDEF 59 | MANIF_INHERIT_TANGENT_API 60 | MANIF_INHERIT_TANGENT_OPERATOR 61 | 62 | SE_2_3Tangent() = default; 63 | ~SE_2_3Tangent() = default; 64 | 65 | MANIF_COPY_CONSTRUCTOR(SE_2_3Tangent) 66 | MANIF_MOVE_CONSTRUCTOR(SE_2_3Tangent) 67 | 68 | template 69 | SE_2_3Tangent(const TangentBase<_DerivedOther>& o); 70 | 71 | MANIF_TANGENT_ASSIGN_OP(SE_2_3Tangent) 72 | 73 | // Tangent common API 74 | 75 | DataType& coeffs(); 76 | const DataType& coeffs() const; 77 | 78 | // SE_2_3Tangent specific API 79 | 80 | protected: 81 | 82 | DataType data_; 83 | }; 84 | 85 | MANIF_EXTRA_TANGENT_TYPEDEF(SE_2_3Tangent); 86 | 87 | template 88 | template 89 | SE_2_3Tangent<_Scalar>::SE_2_3Tangent( 90 | const TangentBase<_DerivedOther>& o) 91 | : data_(o.coeffs()) 92 | { 93 | // 94 | } 95 | 96 | template 97 | typename SE_2_3Tangent<_Scalar>::DataType& 98 | SE_2_3Tangent<_Scalar>::coeffs() 99 | { 100 | return data_; 101 | } 102 | 103 | template 104 | const typename SE_2_3Tangent<_Scalar>::DataType& 105 | SE_2_3Tangent<_Scalar>::coeffs() const 106 | { 107 | return data_; 108 | } 109 | 110 | } /* namespace manif */ 111 | 112 | #endif /* _MANIF_MANIF_SE_2_3TANGENT_H_ */ 113 | -------------------------------------------------------------------------------- /include/manif/impl/se_2_3/SE_2_3Tangent_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE_2_3TANGENT_MAP_H_ 2 | #define _MANIF_MANIF_SE_2_3TANGENT_MAP_H_ 3 | 4 | #include "manif/impl/se_2_3/SE_2_3Tangent.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::DoF; 16 | using DataType = Eigen::Map, 0>; 17 | using Base = SE_2_3TangentBase, 0>>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::DoF; 27 | using DataType = Eigen::Map, 0>; 28 | using Base = SE_2_3TangentBase, 0>>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::SE_2_3 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::SE_2_3TangentBase, 0> > 42 | { 43 | using Base = manif::SE_2_3TangentBase, 0> >; 44 | 45 | public: 46 | 47 | MANIF_TANGENT_TYPEDEF 48 | MANIF_INHERIT_TANGENT_API 49 | MANIF_INHERIT_TANGENT_OPERATOR 50 | 51 | Map(Scalar* coeffs) : data_(coeffs) { } 52 | 53 | MANIF_TANGENT_MAP_ASSIGN_OP(SE_2_3Tangent) 54 | 55 | DataType& coeffs() { return data_; } 56 | const DataType& coeffs() const { return data_; } 57 | 58 | protected: 59 | 60 | DataType data_; 61 | }; 62 | 63 | /** 64 | * @brief Specialization of Map for const manif::SE_2_3 65 | */ 66 | template 67 | class Map, 0> 68 | : public manif::SE_2_3TangentBase, 0> > 69 | { 70 | using Base = manif::SE_2_3TangentBase, 0> >; 71 | 72 | public: 73 | 74 | MANIF_TANGENT_TYPEDEF 75 | MANIF_INHERIT_TANGENT_API 76 | MANIF_INHERIT_TANGENT_OPERATOR 77 | 78 | Map(const Scalar* coeffs) : data_(coeffs) { } 79 | 80 | const DataType& coeffs() const { return data_; } 81 | 82 | protected: 83 | 84 | const DataType data_; 85 | }; 86 | 87 | } /* namespace Eigen */ 88 | 89 | #endif /* _MANIF_MANIF_SE_2_3TANGENT_MAP_H_ */ 90 | -------------------------------------------------------------------------------- /include/manif/impl/se_2_3/SE_2_3_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE_2_3_MAP_H_ 2 | #define _MANIF_MANIF_SE_2_3_MAP_H_ 3 | 4 | #include "manif/impl/se_2_3/SE_2_3.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::RepSize; 16 | using Base = SE_2_3Base, 0>>; 17 | using DataType = Eigen::Map, 0>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::RepSize; 27 | using Base = SE_2_3Base, 0>>; 28 | using DataType = Eigen::Map, 0>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::SE_2_3 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::SE_2_3Base, 0> > 42 | { 43 | using Base = manif::SE_2_3Base, 0> >; 44 | 45 | public: 46 | 47 | MANIF_COMPLETE_GROUP_TYPEDEF 48 | MANIF_INHERIT_GROUP_API 49 | using Base::rotation; 50 | 51 | Map(Scalar* coeffs) : data_(coeffs) { } 52 | 53 | MANIF_GROUP_MAP_ASSIGN_OP(SE_2_3) 54 | 55 | DataType& coeffs() { return data_; } 56 | const DataType& coeffs() const { return data_; } 57 | 58 | protected: 59 | 60 | DataType data_; 61 | }; 62 | 63 | /** 64 | * @brief Specialization of Map for const manif::SE_2_3 65 | */ 66 | template 67 | class Map, 0> 68 | : public manif::SE_2_3Base, 0> > 69 | { 70 | using Base = manif::SE_2_3Base, 0> >; 71 | 72 | public: 73 | 74 | MANIF_COMPLETE_GROUP_TYPEDEF 75 | MANIF_INHERIT_GROUP_API 76 | using Base::rotation; 77 | 78 | Map(const Scalar* coeffs) : data_(coeffs) { } 79 | 80 | const DataType& coeffs() const { return data_; } 81 | 82 | protected: 83 | 84 | const DataType data_; 85 | }; 86 | 87 | } /* namespace Eigen */ 88 | 89 | #endif /* _MANIF_MANIF_SE_2_3_MAP_H_ */ 90 | -------------------------------------------------------------------------------- /include/manif/impl/se_2_3/SE_2_3_properties.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SE_2_3_PROPERTIES_H_ 2 | #define _MANIF_MANIF_SE_2_3_PROPERTIES_H_ 3 | 4 | #include "manif/impl/traits.h" 5 | 6 | namespace manif { 7 | 8 | // Forward declaration 9 | template struct SE_2_3Base; 10 | template struct SE_2_3TangentBase; 11 | 12 | namespace internal { 13 | 14 | //! traits specialization 15 | template 16 | struct LieGroupProperties> 17 | { 18 | static constexpr int Dim = 3; /// @brief Space dimension 19 | static constexpr int DoF = 9; /// @brief Degrees of freedom 20 | }; 21 | 22 | //! traits specialization 23 | template 24 | struct LieGroupProperties> 25 | { 26 | static constexpr int Dim = 3; /// @brief Space dimension 27 | static constexpr int DoF = 9; /// @brief Degrees of freedom 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | #endif /* _MANIF_MANIF_SE_2_3_PROPERTIES_H_ */ 34 | -------------------------------------------------------------------------------- /include/manif/impl/sgal3/SGal3Tangent.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SGAL3TANGENT_H_ 2 | #define _MANIF_MANIF_SGAL3TANGENT_H_ 3 | 4 | #include "manif/impl/sgal3/SGal3Tangent_base.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! Traits specialization 10 | template 11 | struct traits> { 12 | using Scalar = _Scalar; 13 | 14 | using LieGroup = SGal3<_Scalar>; 15 | using Tangent = SGal3Tangent<_Scalar>; 16 | 17 | using Base = SGal3TangentBase; 18 | 19 | static constexpr int Dim = LieGroupProperties::Dim; 20 | static constexpr int DoF = LieGroupProperties::DoF; 21 | static constexpr int RepSize = DoF; 22 | 23 | using DataType = Eigen::Matrix; 24 | 25 | using Jacobian = Eigen::Matrix; 26 | using LieAlg = Eigen::Matrix; 27 | }; 28 | 29 | } // namespace internal 30 | } // namespace manif 31 | 32 | namespace manif { 33 | 34 | // 35 | // Tangent 36 | // 37 | 38 | /** 39 | * @brief Represents an element of tangent space of SGal3. 40 | */ 41 | template 42 | struct SGal3Tangent : SGal3TangentBase> { 43 | private: 44 | 45 | using Base = SGal3TangentBase>; 46 | using Type = SGal3Tangent<_Scalar>; 47 | 48 | protected: 49 | 50 | using Base::derived; 51 | 52 | public: 53 | 54 | MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND 55 | 56 | MANIF_TANGENT_TYPEDEF 57 | MANIF_INHERIT_TANGENT_API 58 | MANIF_INHERIT_TANGENT_OPERATOR 59 | 60 | SGal3Tangent() = default; 61 | ~SGal3Tangent() = default; 62 | 63 | MANIF_COPY_CONSTRUCTOR(SGal3Tangent) 64 | MANIF_MOVE_CONSTRUCTOR(SGal3Tangent) 65 | 66 | template 67 | SGal3Tangent(const TangentBase<_DerivedOther>& o); 68 | 69 | MANIF_TANGENT_ASSIGN_OP(SGal3Tangent) 70 | 71 | // Tangent common API 72 | 73 | DataType& coeffs(); 74 | const DataType& coeffs() const; 75 | 76 | // SGal3Tangent specific API 77 | 78 | protected: 79 | 80 | DataType data_; 81 | }; 82 | 83 | MANIF_EXTRA_TANGENT_TYPEDEF(SGal3Tangent); 84 | 85 | template 86 | template 87 | SGal3Tangent<_Scalar>::SGal3Tangent(const TangentBase<_DerivedOther>& o) 88 | : data_(o.coeffs()) { 89 | // 90 | } 91 | 92 | template 93 | typename SGal3Tangent<_Scalar>::DataType& 94 | SGal3Tangent<_Scalar>::coeffs() { 95 | return data_; 96 | } 97 | 98 | template 99 | const typename SGal3Tangent<_Scalar>::DataType& 100 | SGal3Tangent<_Scalar>::coeffs() const { 101 | return data_; 102 | } 103 | 104 | } // namespace manif 105 | 106 | #endif // _MANIF_MANIF_SGAL3TANGENT_H_ 107 | -------------------------------------------------------------------------------- /include/manif/impl/sgal3/SGal3Tangent_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SGAL3TANGENT_MAP_H_ 2 | #define _MANIF_MANIF_SGAL3TANGENT_MAP_H_ 3 | 4 | #include "manif/impl/sgal3/SGal3Tangent.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits, 0> > 12 | : public traits> { 13 | using typename traits>::Scalar; 14 | using traits>::DoF; 15 | using DataType = Eigen::Map, 0>; 16 | using Base = SGal3TangentBase, 0>>; 17 | }; 18 | 19 | //! @brief traits specialization for Eigen Map 20 | template 21 | struct traits, 0> > 22 | : public traits> { 23 | using typename traits>::Scalar; 24 | using traits>::DoF; 25 | using DataType = Eigen::Map, 0>; 26 | using Base = SGal3TangentBase, 0>>; 27 | }; 28 | 29 | } // namespace internal 30 | } // namespace manif 31 | 32 | namespace Eigen { 33 | 34 | /** 35 | * @brief Specialization of Map for manif::SGal3 36 | */ 37 | template 38 | class Map, 0> 39 | : public manif::SGal3TangentBase, 0> > { 40 | using Base = manif::SGal3TangentBase, 0> >; 41 | 42 | public: 43 | 44 | MANIF_TANGENT_TYPEDEF 45 | MANIF_INHERIT_TANGENT_API 46 | MANIF_INHERIT_TANGENT_OPERATOR 47 | 48 | Map(Scalar* coeffs) : data_(coeffs) { } 49 | 50 | MANIF_TANGENT_MAP_ASSIGN_OP(SGal3Tangent) 51 | 52 | DataType& coeffs() { return data_; } 53 | const DataType& coeffs() const { return data_; } 54 | 55 | protected: 56 | 57 | DataType data_; 58 | }; 59 | 60 | /** 61 | * @brief Specialization of Map for const manif::SGal3 62 | */ 63 | template 64 | class Map, 0> 65 | : public manif::SGal3TangentBase, 0> > { 66 | using Base = manif::SGal3TangentBase, 0> >; 67 | 68 | public: 69 | 70 | MANIF_TANGENT_TYPEDEF 71 | MANIF_INHERIT_TANGENT_API 72 | MANIF_INHERIT_TANGENT_OPERATOR 73 | 74 | Map(const Scalar* coeffs) : data_(coeffs) { } 75 | 76 | const DataType& coeffs() const { return data_; } 77 | 78 | protected: 79 | 80 | const DataType data_; 81 | }; 82 | 83 | } // namespace Eigen 84 | 85 | #endif // _MANIF_MANIF_SGAL3TANGENT_MAP_H_ 86 | -------------------------------------------------------------------------------- /include/manif/impl/sgal3/SGal3_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SGAL3_MAP_H_ 2 | #define _MANIF_MANIF_SGAL3_MAP_H_ 3 | 4 | #include "manif/impl/sgal3/SGal3.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits, 0> > : public traits> { 12 | using typename traits>::Scalar; 13 | using traits>::RepSize; 14 | using Base = SGal3Base, 0>>; 15 | using DataType = Eigen::Map, 0>; 16 | }; 17 | 18 | //! @brief traits specialization for Eigen Map const 19 | template 20 | struct traits,0> > 21 | : public traits> { 22 | using typename traits>::Scalar; 23 | using traits>::RepSize; 24 | using Base = SGal3Base, 0>>; 25 | using DataType = Eigen::Map, 0>; 26 | }; 27 | 28 | } // namespace internal 29 | } // namespace manif 30 | 31 | namespace Eigen { 32 | 33 | /** 34 | * @brief Specialization of Map for manif::SGal3 35 | */ 36 | template 37 | class Map, 0> 38 | : public manif::SGal3Base, 0> > { 39 | using Base = manif::SGal3Base, 0> >; 40 | 41 | public: 42 | 43 | MANIF_COMPLETE_GROUP_TYPEDEF 44 | MANIF_INHERIT_GROUP_API 45 | using Base::rotation; 46 | 47 | Map(Scalar* coeffs) : data_(coeffs) { } 48 | 49 | MANIF_GROUP_MAP_ASSIGN_OP(SGal3) 50 | 51 | DataType& coeffs() { return data_; } 52 | const DataType& coeffs() const { return data_; } 53 | 54 | protected: 55 | 56 | DataType data_; 57 | }; 58 | 59 | /** 60 | * @brief Specialization of Map for const manif::SGal3 61 | */ 62 | template 63 | class Map, 0> 64 | : public manif::SGal3Base, 0> > { 65 | using Base = manif::SGal3Base, 0> >; 66 | 67 | public: 68 | 69 | MANIF_COMPLETE_GROUP_TYPEDEF 70 | MANIF_INHERIT_GROUP_API 71 | // using Base::rotation; 72 | 73 | Map(const Scalar* coeffs) : data_(coeffs) { } 74 | 75 | const DataType& coeffs() const { return data_; } 76 | 77 | protected: 78 | 79 | const DataType data_; 80 | }; 81 | 82 | } // namespace Eigen 83 | 84 | #endif // _MANIF_MANIF_SGAL3_MAP_H_ 85 | -------------------------------------------------------------------------------- /include/manif/impl/sgal3/SGal3_properties.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SGAL3_PROPERTIES_H_ 2 | #define _MANIF_MANIF_SGAL3_PROPERTIES_H_ 3 | 4 | #include "manif/impl/traits.h" 5 | 6 | namespace manif { 7 | 8 | // Forward declaration 9 | template struct SGal3Base; 10 | template struct SGal3TangentBase; 11 | 12 | namespace internal { 13 | 14 | //! traits specialization 15 | template 16 | struct LieGroupProperties> { 17 | static constexpr int Dim = 3; /// @brief Space dimension 18 | static constexpr int DoF = 10; /// @brief Degrees of freedom 19 | }; 20 | 21 | //! traits specialization 22 | template 23 | struct LieGroupProperties> { 24 | static constexpr int Dim = 3; /// @brief Space dimension 25 | static constexpr int DoF = 10; /// @brief Degrees of freedom 26 | }; 27 | 28 | } // namespace internal 29 | } // namespace manif 30 | 31 | #endif // _MANIF_MANIF_SGAL3_PROPERTIES_H_ 32 | -------------------------------------------------------------------------------- /include/manif/impl/so2/SO2Tangent.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SO2TANGENT_H_ 2 | #define _MANIF_MANIF_SO2TANGENT_H_ 3 | 4 | #include "manif/impl/so2/SO2Tangent_base.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! Traits specialization 10 | template 11 | struct traits> 12 | { 13 | using Scalar = _Scalar; 14 | 15 | using LieGroup = SO2<_Scalar>; 16 | using Tangent = SO2Tangent<_Scalar>; 17 | 18 | using Base = SO2TangentBase; 19 | 20 | static constexpr int Dim = LieGroupProperties::Dim; 21 | static constexpr int DoF = LieGroupProperties::DoF; 22 | static constexpr int RepSize = DoF; 23 | 24 | using DataType = Eigen::Matrix; 25 | 26 | using Jacobian = Eigen::Matrix; 27 | using LieAlg = Eigen::Matrix; 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | namespace manif { 34 | 35 | // 36 | // Tangent 37 | // 38 | 39 | /** 40 | * @brief Represents an element of tangent space of SO2. 41 | */ 42 | template 43 | struct SO2Tangent : SO2TangentBase> 44 | { 45 | private: 46 | 47 | using Base = SO2TangentBase>; 48 | using Type = SO2Tangent<_Scalar>; 49 | 50 | protected: 51 | 52 | using Base::derived; 53 | 54 | public: 55 | 56 | MANIF_TANGENT_TYPEDEF 57 | MANIF_INHERIT_TANGENT_API 58 | MANIF_INHERIT_TANGENT_OPERATOR 59 | 60 | SO2Tangent() = default; 61 | ~SO2Tangent() = default; 62 | 63 | MANIF_COPY_CONSTRUCTOR(SO2Tangent) 64 | MANIF_MOVE_CONSTRUCTOR(SO2Tangent) 65 | 66 | // Copy constructor given base 67 | template 68 | SO2Tangent(const TangentBase<_DerivedOther>& o); 69 | 70 | MANIF_TANGENT_ASSIGN_OP(SO2Tangent) 71 | 72 | //! @brief Constructor given an angle (rad.). 73 | SO2Tangent(const Scalar theta); 74 | 75 | // Tangent common API 76 | 77 | DataType& coeffs(); 78 | const DataType& coeffs() const; 79 | 80 | // SO2Tangent specific API 81 | 82 | using Base::angle; 83 | 84 | protected: 85 | 86 | DataType data_; 87 | }; 88 | 89 | MANIF_EXTRA_TANGENT_TYPEDEF(SO2Tangent); 90 | 91 | template 92 | template 93 | SO2Tangent<_Scalar>::SO2Tangent(const TangentBase<_DerivedOther>& o) 94 | : data_(o.coeffs()) 95 | { 96 | // 97 | } 98 | 99 | template 100 | SO2Tangent<_Scalar>::SO2Tangent(const Scalar theta) 101 | : data_(theta) 102 | { 103 | // 104 | } 105 | 106 | template 107 | typename SO2Tangent<_Scalar>::DataType& 108 | SO2Tangent<_Scalar>::coeffs() 109 | { 110 | return data_; 111 | } 112 | 113 | template 114 | const typename SO2Tangent<_Scalar>::DataType& 115 | SO2Tangent<_Scalar>::coeffs() const 116 | { 117 | return data_; 118 | } 119 | 120 | } /* namespace manif */ 121 | 122 | #endif /* _MANIF_MANIF_SO2TANGENT_H_ */ 123 | -------------------------------------------------------------------------------- /include/manif/impl/so2/SO2Tangent_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SO2TANGENT_MAP_H_ 2 | #define _MANIF_MANIF_SO2TANGENT_MAP_H_ 3 | 4 | #include "manif/impl/so2/SO2Tangent.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::DoF; 16 | using DataType = ::Eigen::Map, 0>; 17 | using Base = SO2TangentBase, 0>>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::DoF; 27 | using DataType = ::Eigen::Map, 0>; 28 | using Base = SO2TangentBase, 0>>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | //! @brief Specialization of Map for manif::SO2Tangent 37 | template 38 | class Map, 0> 39 | : public manif::SO2TangentBase, 0> > 40 | { 41 | using Base = manif::SO2TangentBase, 0> >; 42 | 43 | public: 44 | 45 | MANIF_TANGENT_TYPEDEF 46 | MANIF_INHERIT_TANGENT_API 47 | MANIF_INHERIT_TANGENT_OPERATOR 48 | 49 | Map(Scalar* coeffs) : data_(coeffs) { } 50 | 51 | MANIF_TANGENT_MAP_ASSIGN_OP(SO2Tangent) 52 | 53 | DataType& coeffs() { return data_; } 54 | const DataType& coeffs() const { return data_; } 55 | 56 | protected: 57 | 58 | DataType data_; 59 | }; 60 | 61 | //! @brief Specialization of Map for const manif::SO2Tangent 62 | template 63 | class Map, 0> 64 | : public manif::SO2TangentBase, 0> > 65 | { 66 | using Base = manif::SO2TangentBase, 0> >; 67 | 68 | public: 69 | 70 | MANIF_TANGENT_TYPEDEF 71 | MANIF_INHERIT_TANGENT_API 72 | MANIF_INHERIT_TANGENT_OPERATOR 73 | 74 | Map(const Scalar* coeffs) : data_(coeffs) { } 75 | 76 | const DataType& coeffs() const { return data_; } 77 | 78 | protected: 79 | 80 | const DataType data_; 81 | }; 82 | 83 | } /* namespace Eigen */ 84 | 85 | #endif /* _MANIF_MANIF_SO2TANGENT_MAP_H_ */ 86 | -------------------------------------------------------------------------------- /include/manif/impl/so2/SO2_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SO2_MAP_H_ 2 | #define _MANIF_MANIF_SO2_MAP_H_ 3 | 4 | #include "manif/impl/so2/SO2.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::RepSize; 16 | using Base = SO2Base, 0>>; 17 | using DataType = Eigen::Map, 0>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::RepSize; 27 | using Base = SO2Base, 0>>; 28 | using DataType = Eigen::Map, 0>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::SO2 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::SO2Base, 0> > 42 | { 43 | using Base = manif::SO2Base, 0> >; 44 | 45 | public: 46 | 47 | MANIF_COMPLETE_GROUP_TYPEDEF 48 | MANIF_INHERIT_GROUP_API 49 | using Base::transform; 50 | using Base::rotation; 51 | 52 | Map(Scalar* coeffs) : data_(coeffs) { } 53 | 54 | MANIF_GROUP_MAP_ASSIGN_OP(SO2) 55 | 56 | DataType& coeffs() { return data_; } 57 | const DataType& coeffs() const { return data_; } 58 | 59 | protected: 60 | 61 | DataType data_; 62 | }; 63 | 64 | /** 65 | * @brief Specialization of Map for const manif::SO2 66 | */ 67 | template 68 | class Map, 0> 69 | : public manif::SO2Base, 0> > 70 | { 71 | using Base = manif::SO2Base, 0> >; 72 | 73 | public: 74 | 75 | MANIF_COMPLETE_GROUP_TYPEDEF 76 | MANIF_INHERIT_GROUP_API 77 | using Base::transform; 78 | using Base::rotation; 79 | 80 | Map(const Scalar* coeffs) : data_(coeffs) { } 81 | 82 | const DataType& coeffs() const { return data_; } 83 | 84 | protected: 85 | 86 | const DataType data_; 87 | }; 88 | 89 | } /* namespace Eigen */ 90 | 91 | #endif /* _MANIF_MANIF_SO2_MAP_H_ */ 92 | -------------------------------------------------------------------------------- /include/manif/impl/so2/SO2_properties.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SO2_PROPERTIES_H_ 2 | #define _MANIF_MANIF_SO2_PROPERTIES_H_ 3 | 4 | #include "manif/impl/traits.h" 5 | 6 | namespace manif { 7 | 8 | // Forward declaration 9 | template struct SO2Base; 10 | template struct SO2TangentBase; 11 | 12 | namespace internal { 13 | 14 | //! traits specialization 15 | template 16 | struct LieGroupProperties> 17 | { 18 | static constexpr int Dim = 2; /// @brief Space dimension 19 | static constexpr int DoF = 1; /// @brief Degrees of freedom 20 | }; 21 | 22 | //! traits specialization 23 | template 24 | struct LieGroupProperties> 25 | { 26 | static constexpr int Dim = 2; /// @brief Space dimension 27 | static constexpr int DoF = 1; /// @brief Degrees of freedom 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | #endif /* _MANIF_MANIF_SO2_PROPERTIES_H_ */ 34 | -------------------------------------------------------------------------------- /include/manif/impl/so3/SO3Tangent.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SO3TANGENT_H_ 2 | #define _MANIF_MANIF_SO3TANGENT_H_ 3 | 4 | #include "manif/impl/so3/SO3Tangent_base.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! Traits specialization 10 | template 11 | struct traits> 12 | { 13 | using Scalar = _Scalar; 14 | 15 | using LieGroup = SO3<_Scalar>; 16 | using Tangent = SO3Tangent<_Scalar>; 17 | 18 | using Base = SO3TangentBase; 19 | 20 | static constexpr int Dim = LieGroupProperties::Dim; 21 | static constexpr int DoF = LieGroupProperties::DoF; 22 | static constexpr int RepSize = DoF; 23 | 24 | using DataType = Eigen::Matrix; 25 | 26 | using Jacobian = Eigen::Matrix; 27 | using LieAlg = Eigen::Matrix; 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | namespace manif { 34 | 35 | // 36 | // Tangent 37 | // 38 | 39 | /** 40 | * @brief Represents an element of tangent space of SO3. 41 | */ 42 | template 43 | struct SO3Tangent : SO3TangentBase> 44 | { 45 | private: 46 | 47 | using Base = SO3TangentBase>; 48 | using Type = SO3Tangent<_Scalar>; 49 | 50 | protected: 51 | 52 | using Base::derived; 53 | 54 | public: 55 | 56 | MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND 57 | 58 | MANIF_TANGENT_TYPEDEF 59 | MANIF_INHERIT_TANGENT_API 60 | MANIF_INHERIT_TANGENT_OPERATOR 61 | 62 | SO3Tangent() = default; 63 | ~SO3Tangent() = default; 64 | 65 | MANIF_COPY_CONSTRUCTOR(SO3Tangent) 66 | MANIF_MOVE_CONSTRUCTOR(SO3Tangent) 67 | 68 | // Copy constructor given base 69 | template 70 | SO3Tangent(const TangentBase<_DerivedOther>& o); 71 | 72 | MANIF_TANGENT_ASSIGN_OP(SO3Tangent) 73 | 74 | // Tangent common API 75 | 76 | DataType& coeffs(); 77 | const DataType& coeffs() const; 78 | 79 | // SO3Tangent specific API 80 | 81 | protected: 82 | 83 | DataType data_; 84 | }; 85 | 86 | MANIF_EXTRA_TANGENT_TYPEDEF(SO3Tangent); 87 | 88 | template 89 | template 90 | SO3Tangent<_Scalar>::SO3Tangent(const TangentBase<_DerivedOther>& o) 91 | : data_(o.coeffs()) 92 | { 93 | // 94 | } 95 | 96 | template 97 | typename SO3Tangent<_Scalar>::DataType& 98 | SO3Tangent<_Scalar>::coeffs() 99 | { 100 | return data_; 101 | } 102 | 103 | template 104 | const typename SO3Tangent<_Scalar>::DataType& 105 | SO3Tangent<_Scalar>::coeffs() const 106 | { 107 | return data_; 108 | } 109 | 110 | } /* namespace manif */ 111 | 112 | #endif /* _MANIF_MANIF_SO3TANGENT_H_ */ 113 | -------------------------------------------------------------------------------- /include/manif/impl/so3/SO3Tangent_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SO3TANGENT_MAP_H_ 2 | #define _MANIF_MANIF_SO3TANGENT_MAP_H_ 3 | 4 | #include "manif/impl/so3/SO3Tangent.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::DoF; 16 | using DataType = ::Eigen::Map, 0>; 17 | using Base = SO3TangentBase, 0>>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::DoF; 27 | using DataType = ::Eigen::Map, 0>; 28 | using Base = SO3TangentBase, 0>>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::SO3Tangent 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::SO3TangentBase, 0> > 42 | { 43 | using Base = manif::SO3TangentBase, 0> >; 44 | 45 | public: 46 | 47 | MANIF_TANGENT_TYPEDEF 48 | MANIF_INHERIT_TANGENT_API 49 | MANIF_INHERIT_TANGENT_OPERATOR 50 | 51 | Map(Scalar* coeffs) : data_(coeffs) { } 52 | 53 | MANIF_TANGENT_MAP_ASSIGN_OP(SO3Tangent) 54 | 55 | DataType& coeffs() { return data_; } 56 | const DataType& coeffs() const { return data_; } 57 | 58 | protected: 59 | 60 | DataType data_; 61 | }; 62 | 63 | /** 64 | * @brief Specialization of Map for const manif::SO3Tangent 65 | */ 66 | template 67 | class Map, 0> 68 | : public manif::SO3TangentBase, 0> > 69 | { 70 | using Base = manif::SO3TangentBase, 0> >; 71 | 72 | public: 73 | 74 | MANIF_TANGENT_TYPEDEF 75 | MANIF_INHERIT_TANGENT_API 76 | MANIF_INHERIT_TANGENT_OPERATOR 77 | 78 | Map(const Scalar* coeffs) : data_(coeffs) { } 79 | 80 | const DataType& coeffs() const { return data_; } 81 | 82 | protected: 83 | 84 | const DataType data_; 85 | }; 86 | 87 | } /* namespace Eigen */ 88 | 89 | #endif /* _MANIF_MANIF_SO3TANGENT_MAP_H_ */ 90 | -------------------------------------------------------------------------------- /include/manif/impl/so3/SO3_map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SO3_MAP_H_ 2 | #define _MANIF_MANIF_SO3_MAP_H_ 3 | 4 | #include "manif/impl/so3/SO3.h" 5 | 6 | namespace manif { 7 | namespace internal { 8 | 9 | //! @brief traits specialization for Eigen Map 10 | template 11 | struct traits< Eigen::Map,0> > 12 | : public traits> 13 | { 14 | using typename traits>::Scalar; 15 | using traits>::RepSize; 16 | using Base = SO3Base, 0>>; 17 | using DataType = Eigen::Map, 0>; 18 | }; 19 | 20 | //! @brief traits specialization for Eigen Map const 21 | template 22 | struct traits< Eigen::Map,0> > 23 | : public traits> 24 | { 25 | using typename traits>::Scalar; 26 | using traits>::RepSize; 27 | using Base = SO3Base, 0>>; 28 | using DataType = Eigen::Map, 0>; 29 | }; 30 | 31 | } /* namespace internal */ 32 | } /* namespace manif */ 33 | 34 | namespace Eigen { 35 | 36 | /** 37 | * @brief Specialization of Map for manif::SO3 38 | */ 39 | template 40 | class Map, 0> 41 | : public manif::SO3Base, 0> > 42 | { 43 | using Base = manif::SO3Base, 0> >; 44 | 45 | public: 46 | 47 | MANIF_COMPLETE_GROUP_TYPEDEF 48 | MANIF_INHERIT_GROUP_API 49 | using Base::transform; 50 | using Base::rotation; 51 | 52 | Map(Scalar* coeffs) : data_(coeffs) { } 53 | 54 | Map(Map&&) = default; 55 | 56 | MANIF_GROUP_MAP_ASSIGN_OP(SO3) 57 | 58 | DataType& coeffs() { return data_; } 59 | const DataType& coeffs() const { return data_; } 60 | 61 | protected: 62 | 63 | DataType data_; 64 | }; 65 | 66 | /** 67 | * @brief Specialization of Map for const manif::SO3 68 | */ 69 | template 70 | class Map, 0> 71 | : public manif::SO3Base, 0> > 72 | { 73 | using Base = manif::SO3Base, 0> >; 74 | 75 | public: 76 | 77 | MANIF_COMPLETE_GROUP_TYPEDEF 78 | MANIF_INHERIT_GROUP_API 79 | using Base::transform; 80 | using Base::rotation; 81 | 82 | Map(const Scalar* coeffs) : data_(coeffs) { } 83 | 84 | Map(Map&&) = default; 85 | 86 | const DataType& coeffs() const { return data_; } 87 | 88 | protected: 89 | 90 | const DataType data_; 91 | }; 92 | 93 | } /* namespace Eigen */ 94 | 95 | #endif /* _MANIF_MANIF_SO3_MAP_H_ */ 96 | -------------------------------------------------------------------------------- /include/manif/impl/so3/SO3_properties.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_SO3_PROPERTIES_H_ 2 | #define _MANIF_MANIF_SO3_PROPERTIES_H_ 3 | 4 | #include "manif/impl/traits.h" 5 | 6 | namespace manif { 7 | 8 | // Forward declaration 9 | template struct SO3Base; 10 | template struct SO3TangentBase; 11 | 12 | namespace internal { 13 | 14 | //! traits specialization 15 | template 16 | struct LieGroupProperties> 17 | { 18 | static constexpr int Dim = 3; /// @brief Space dimension 19 | static constexpr int DoF = 3; /// @brief Degrees of freedom 20 | }; 21 | 22 | //! traits specialization 23 | template 24 | struct LieGroupProperties> 25 | { 26 | static constexpr int Dim = 3; /// @brief Space dimension 27 | static constexpr int DoF = 3; /// @brief Degrees of freedom 28 | }; 29 | 30 | } /* namespace internal */ 31 | } /* namespace manif */ 32 | 33 | #endif /* _MANIF_MANIF_SO3_PROPERTIES_H_ */ 34 | -------------------------------------------------------------------------------- /include/manif/impl/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_UTILS_H_ 2 | #define _MANIF_MANIF_UTILS_H_ 3 | 4 | #include "manif/constants.h" 5 | 6 | namespace manif { 7 | 8 | /** 9 | * @brief Wrap an angle in -pi,pi. 10 | * @param[in] angle The angle to be wrapped in radians 11 | * @return The wrapped angle. 12 | */ 13 | template 14 | T pi2pi(T angle) 15 | { 16 | while (angle > T(MANIF_PI)) angle -= T(2. * MANIF_PI); 17 | while (angle <= T(-MANIF_PI)) angle += T(2. * MANIF_PI); 18 | 19 | return angle; 20 | } 21 | 22 | /** 23 | * @brief Conversion to radians 24 | * @param[in] deg angle in degrees 25 | * @return angle in radians 26 | */ 27 | template 28 | constexpr T toRad(const T deg) 29 | { 30 | return deg * Constants::to_rad; 31 | } 32 | 33 | /** 34 | * @brief Conversion to degrees 35 | * @param[in] rad angle in radians 36 | * @return angle in degrees 37 | */ 38 | template 39 | constexpr T toDeg(const T rad) 40 | { 41 | return rad * Constants::to_deg; 42 | } 43 | 44 | /** 45 | * @brief Degree 2 polynomial approximation of 1/sqrt(x) (reciprocal sqrt). 46 | * @param[in] x 47 | * @return ~1/sqrt(x) 48 | */ 49 | template 50 | constexpr T approxSqrtInv(const T x) 51 | { 52 | return (T(15) / T(8)) - (T(5) / T(4)) * x + (T(3) / T(8)) * x * x; 53 | } 54 | 55 | } /* namespace manif */ 56 | 57 | #endif /* _MANIF_MANIF_UTILS_H_ */ 58 | -------------------------------------------------------------------------------- /include/manif/impl/vee.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_IMPL_VEE_H_ 2 | #define _MANIF_MANIF_IMPL_VEE_H_ 3 | 4 | namespace manif { 5 | namespace internal { 6 | 7 | template 8 | struct VeeEvaluatorImpl { 9 | template 10 | static void run(TL& t, const TR&) { 11 | static_assert( 12 | constexpr_false(), "VeeEvaluator not overloaded for Derived type!" 13 | ); 14 | // t.setRandom(); 15 | } 16 | }; 17 | 18 | template 19 | struct VeeEvaluator : VeeEvaluatorImpl { 20 | using Base = VeeEvaluatorImpl; 21 | 22 | VeeEvaluator(Derived& xptr) : xptr_(xptr) {} 23 | 24 | template 25 | void run(const T& t) { 26 | Base::run(xptr_, t); 27 | } 28 | 29 | protected: 30 | 31 | Derived& xptr_; 32 | }; 33 | 34 | } // namespace internal 35 | } // namespace manif 36 | 37 | #endif // _MANIF_MANIF_IMPL_VEE_H_ 38 | -------------------------------------------------------------------------------- /include/manif/manif.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_H_ 2 | #define _MANIF_MANIF_H_ 3 | 4 | #include "manif/constants.h" 5 | #include "manif/functions.h" 6 | 7 | #include "manif/SO2.h" 8 | #include "manif/SE2.h" 9 | #include "manif/SO3.h" 10 | #include "manif/SE3.h" 11 | #include "manif/Rn.h" 12 | #include "manif/SE_2_3.h" 13 | #include "manif/SGal3.h" 14 | 15 | #include "manif/Bundle.h" 16 | 17 | #include "manif/algorithms/average.h" 18 | #include "manif/algorithms/decasteljau.h" 19 | #include "manif/algorithms/interpolation.h" 20 | 21 | #endif /* _MANIF_MANIF_H_ */ 22 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | manif 4 | 0.0.3 5 | The manif package 6 | 7 | Jeremie Deray 8 | Joan Sola 9 | 10 | Jeremie Deray 11 | 12 | 13 | https://github.com/artivis/manif.git 14 | https://github.com/artivis/manif/issues 15 | 16 | MIT 17 | 18 | cmake 19 | 20 | eigen 21 | 22 | gtest 23 | eigen 24 | libceres-dev 25 | 26 | 27 | cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /paper/Lie_theory_cheat_sheet.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artivis/manif/ad00494251d8ee2d16c3d3b82a65757c94fc48ec/paper/Lie_theory_cheat_sheet.pdf -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = [ 3 | "wheel", 4 | "setuptools>=45", 5 | "setuptools_scm[toml]>=6.0", 6 | "ninja", 7 | "cmake>=3.18.2", 8 | "cmake-build-extension", 9 | "pybind11", 10 | ] 11 | build-backend = "setuptools.build_meta" 12 | 13 | [tool.setuptools_scm] 14 | local_scheme = "dirty-tag" 15 | -------------------------------------------------------------------------------- /python/bindings_manif.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void wrap_Rn(pybind11::module &m); 4 | 5 | void wrap_SO2(pybind11::module &m); 6 | void wrap_SO3(pybind11::module &m); 7 | 8 | void wrap_SE2(pybind11::module &m); 9 | void wrap_SE3(pybind11::module &m); 10 | 11 | void wrap_SE_2_3(pybind11::module &m); 12 | 13 | void wrap_SGal3(pybind11::module &m); 14 | 15 | PYBIND11_MODULE(_bindings, m) { 16 | m.doc() = "Python bindings for the manif library, " 17 | "a small library for Lie theory."; 18 | 19 | wrap_Rn(m); 20 | 21 | wrap_SO2(m); 22 | wrap_SO3(m); 23 | 24 | wrap_SE2(m); 25 | wrap_SE3(m); 26 | 27 | wrap_SE_2_3(m); 28 | 29 | wrap_SGal3(m); 30 | } 31 | -------------------------------------------------------------------------------- /python/bindings_optional.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_PYTHON_BINDINGS_OPTIONAL_H_ 2 | #define _MANIF_PYTHON_BINDINGS_OPTIONAL_H_ 3 | 4 | #include 5 | 6 | namespace pybind11 { namespace detail { 7 | template 8 | struct type_caster> : optional_caster> {}; 9 | }} 10 | 11 | #endif // _MANIF_PYTHON_BINDINGS_OPTIONAL_H_ 12 | -------------------------------------------------------------------------------- /python/bindings_se2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "manif/SE2.h" 8 | 9 | #include "bindings_optional.h" 10 | #include "bindings_lie_group_base.h" 11 | #include "bindings_tangent_base.h" 12 | 13 | namespace py = pybind11; 14 | 15 | void wrap_SE2(py::module &m) 16 | { 17 | using Scalar = manif::SE2d::Scalar; 18 | using Translation = manif::SE2d::Translation; 19 | 20 | // group declaration and constructors 21 | 22 | py::class_, std::unique_ptr, py::nodelete>> SE2_base(m, "_SE2Base"); 23 | py::class_, std::unique_ptr, py::nodelete>> SE2_tan_base(m, "_SE2TangentBase"); 24 | 25 | py::class_> SE2(m, "SE2"); 26 | py::class_> SE2_tan(m, "SE2Tangent"); 27 | 28 | // group 29 | 30 | SE2.def(py::init()); 31 | SE2.def(py::init()); 32 | SE2.def(py::init&>()); 33 | SE2.def(py::init&>()); 34 | // SE2.def(py::init&>()); 35 | 36 | wrap_lie_group_base>(SE2); 37 | 38 | SE2.def("transform", &manif::SE2d::transform); 39 | // SE2.def("isometry", &manif::SE2d::isometry); 40 | SE2.def("rotation", &manif::SE2d::rotation); 41 | SE2.def("translation", &manif::SE2d::translation); 42 | SE2.def("real", &manif::SE2d::real); 43 | SE2.def("imag", &manif::SE2d::imag); 44 | SE2.def("angle", &manif::SE2d::angle); 45 | SE2.def("x", &manif::SE2d::x); 46 | SE2.def("y", &manif::SE2d::y); 47 | SE2.def("normalize", &manif::SE2d::normalize); 48 | 49 | // tangent 50 | 51 | wrap_tangent_base>(SE2_tan); 52 | SE2_tan.def(py::init()); 53 | 54 | SE2_tan.def("x", &manif::SE2Tangentd::x); 55 | SE2_tan.def("y", &manif::SE2Tangentd::y); 56 | SE2_tan.def("angle", &manif::SE2Tangentd::angle); 57 | } 58 | -------------------------------------------------------------------------------- /python/bindings_se3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "manif/SE3.h" 7 | 8 | #include "bindings_optional.h" 9 | #include "bindings_lie_group_base.h" 10 | #include "bindings_tangent_base.h" 11 | 12 | namespace py = pybind11; 13 | 14 | void wrap_SE3(py::module &m) 15 | { 16 | using SE3d = manif::SE3d; 17 | using Scalar = SE3d::Scalar; 18 | using Quaternion = Eigen::Quaternion; 19 | 20 | py::class_, std::unique_ptr, py::nodelete>> SE3_base(m, "_SE3Base"); 21 | py::class_, std::unique_ptr, py::nodelete>> SE3_tan_base(m, "_SE3TangentBase"); 22 | 23 | py::class_> SE3(m, "SE3"); 24 | py::class_> SE3_tan(m, "SE3Tangent"); 25 | 26 | // group 27 | 28 | wrap_lie_group_base>(SE3); 29 | 30 | SE3.def(py::init()); 32 | SE3.def(py::init([](const SE3d::Translation& pos, const Eigen::Matrix& quat) { 33 | if(abs(quat.norm() - Scalar(1)) >= manif::Constants::eps) { 34 | throw pybind11::value_error("The quaternion is not normalized!"); 35 | } 36 | return manif::SE3d(pos, quat); 37 | }), 38 | py::arg("position"), 39 | py::arg("quaternion")); 40 | 41 | // SE3.def(py::init&>()); 42 | // SE3.def(py::init&>()); 43 | // SE3.def(py::init&>()); 44 | 45 | SE3.def("transform", &SE3d::transform); 46 | // SE3.def("isometry", &SE3d::isometry); 47 | SE3.def("rotation", &SE3d::rotation); 48 | 49 | SE3.def( 50 | "translation", 51 | static_cast(&SE3d::translation)); 52 | SE3.def( 53 | "translation", 54 | static_cast(&SE3d::translation), 55 | py::arg("translation")); 56 | 57 | SE3.def( 58 | "quat", 59 | [](const manif::SE3d& se3) -> Eigen::Matrix { return se3.coeffs().segment<4>(3); }); 60 | 61 | SE3.def( 62 | "quat", 63 | [](manif::SE3d& se3, const Eigen::Matrix& quaternion) { 64 | if(abs(quaternion.norm() - Scalar(1)) >= manif::Constants::eps) { 65 | throw pybind11::value_error("The quaternion is not normalized!"); 66 | } 67 | se3.quat(quaternion); 68 | }, 69 | py::arg("quaternion")); 70 | 71 | SE3.def("x", &SE3d::x); 72 | SE3.def("y", &SE3d::y); 73 | SE3.def("z", &SE3d::z); 74 | SE3.def("normalize", &SE3d::normalize); 75 | 76 | //tangent 77 | 78 | wrap_tangent_base>(SE3_tan); 79 | 80 | // SE3_tan.def("v", &manif::SE3Tangentd::v); 81 | // SE3_tan.def("w", &manif::SE3Tangentd::w); 82 | } 83 | -------------------------------------------------------------------------------- /python/bindings_se_2_3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "manif/SE_2_3.h" 7 | 8 | #include "bindings_optional.h" 9 | #include "bindings_lie_group_base.h" 10 | #include "bindings_tangent_base.h" 11 | 12 | namespace py = pybind11; 13 | 14 | void wrap_SE_2_3(py::module &m) 15 | { 16 | using Scalar = manif::SE_2_3d::Scalar; 17 | using Translation = manif::SE_2_3d::Translation; 18 | using Quaternion = Eigen::Quaternion; 19 | using LinearVelocity = manif::SE_2_3d::LinearVelocity; 20 | 21 | py::class_, std::unique_ptr, py::nodelete>> SE_2_3_base(m, "_SE_2_3Base"); 22 | py::class_, std::unique_ptr, py::nodelete>> SE_2_3_tan_base(m, "_SE_2_3TangentBase"); 23 | 24 | py::class_> SE_2_3(m, "SE_2_3"); 25 | py::class_> SE_2_3_tan(m, "SE_2_3Tangent"); 26 | 27 | //group 28 | 29 | wrap_lie_group_base>(SE_2_3); 30 | 31 | // SE_2_3.def(py::init()); 32 | // SE_2_3.def(py::init&, const LinearVelocity&>()); 33 | // SE_2_3.def(py::init&, const LinearVelocity&>()); 34 | SE_2_3.def(py::init()); 37 | // SE_2_3.def(py::init&, const LinearVelocity&>()); 38 | 39 | // SE_2_3.def("isometry", &manif::SE_2_3d::isometry); 40 | SE_2_3.def("rotation", &manif::SE_2_3d::rotation); 41 | // SE_2_3.def("quat", &manif::SE_2_3d::quat); 42 | SE_2_3.def("translation", &manif::SE_2_3d::translation); 43 | SE_2_3.def("x", &manif::SE_2_3d::x); 44 | SE_2_3.def("y", &manif::SE_2_3d::y); 45 | SE_2_3.def("z", &manif::SE_2_3d::z); 46 | SE_2_3.def("linearVelocity", &manif::SE_2_3d::linearVelocity); 47 | SE_2_3.def("vx", &manif::SE_2_3d::vx); 48 | SE_2_3.def("vy", &manif::SE_2_3d::vy); 49 | SE_2_3.def("vz", &manif::SE_2_3d::vz); 50 | SE_2_3.def("normalize", &manif::SE_2_3d::normalize); 51 | 52 | // tangent 53 | wrap_tangent_base>(SE_2_3_tan); 54 | 55 | // SE_2_3_tan.def("v", &manif::SE3Tangentd::v); 56 | // SE_2_3_tan.def("w", &manif::SE3Tangentd::w); 57 | // SE_2_3_tan.def("a", &manif::SE3Tangentd::a); 58 | } 59 | -------------------------------------------------------------------------------- /python/bindings_sgal3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "manif/SGal3.h" 7 | 8 | #include "bindings_optional.h" 9 | #include "bindings_lie_group_base.h" 10 | #include "bindings_tangent_base.h" 11 | 12 | namespace py = pybind11; 13 | 14 | void wrap_SGal3(py::module &m) { 15 | using Scalar = manif::SGal3d::Scalar; 16 | using Translation = manif::SGal3d::Translation; 17 | using Quaternion = Eigen::Quaternion; 18 | using LinearVelocity = manif::SGal3d::LinearVelocity; 19 | 20 | py::class_< 21 | manif::LieGroupBase, 22 | std::unique_ptr, py::nodelete> 23 | > SGal3_base(m, "_SGal3Base"); 24 | py::class_< 25 | manif::TangentBase, 26 | std::unique_ptr, py::nodelete> 27 | > SGal3_tan_base(m, "_SGal3TangentBase"); 28 | 29 | py::class_> SGal3(m, "SGal3"); 30 | py::class_< 31 | manif::SGal3Tangentd, manif::TangentBase 32 | > SGal3_tan(m, "SGal3Tangent"); 33 | 34 | //group 35 | 36 | wrap_lie_group_base>(SGal3); 37 | 38 | // SGal3.def(py::init()); 39 | // SGal3.def(py::init&, const LinearVelocity&>()); 40 | // SGal3.def(py::init&, const LinearVelocity&>()); 41 | SGal3.def( 42 | py::init< 43 | const Scalar, const Scalar, const Scalar, 44 | const Scalar, const Scalar, const Scalar, 45 | const Scalar, const Scalar, const Scalar, 46 | const Scalar 47 | >() 48 | ); 49 | // SGal3.def(py::init&, const LinearVelocity&>()); 50 | 51 | // SGal3.def("isometry", &manif::SGal3d::isometry); 52 | SGal3.def("rotation", &manif::SGal3d::rotation); 53 | // SGal3.def("quat", &manif::SGal3d::quat); 54 | SGal3.def("translation", &manif::SGal3d::translation); 55 | SGal3.def("x", &manif::SGal3d::x); 56 | SGal3.def("y", &manif::SGal3d::y); 57 | SGal3.def("z", &manif::SGal3d::z); 58 | SGal3.def("linearVelocity", &manif::SGal3d::linearVelocity); 59 | SGal3.def("vx", &manif::SGal3d::vx); 60 | SGal3.def("vy", &manif::SGal3d::vy); 61 | SGal3.def("vz", &manif::SGal3d::vz); 62 | SGal3.def("t", &manif::SGal3d::t); 63 | SGal3.def("normalize", &manif::SGal3d::normalize); 64 | 65 | // tangent 66 | wrap_tangent_base< 67 | manif::SGal3Tangentd, manif::TangentBase 68 | >(SGal3_tan); 69 | 70 | // SGal3_tan.def("v", &manif::SE3Tangentd::v); 71 | // SGal3_tan.def("w", &manif::SE3Tangentd::w); 72 | // SGal3_tan.def("a", &manif::SE3Tangentd::a); 73 | } 74 | -------------------------------------------------------------------------------- /python/bindings_so2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "manif/SO2.h" 7 | 8 | #include "bindings_optional.h" 9 | #include "bindings_lie_group_base.h" 10 | #include "bindings_tangent_base.h" 11 | 12 | namespace py = pybind11; 13 | 14 | void wrap_SO2(py::module &m) 15 | { 16 | using Scalar = manif::SO2d::Scalar; 17 | 18 | py::class_, std::unique_ptr, py::nodelete>> SO2_base(m, "_SO2Base"); 19 | py::class_, std::unique_ptr, py::nodelete>> SO2_tan_base(m, "_SO2TangentBase"); 20 | 21 | py::class_> SO2(m, "SO2", "The SO2 class"); 22 | py::class_> SO2_tan(m, "SO2Tangent"); 23 | 24 | //group 25 | 26 | wrap_lie_group_base>(SO2); 27 | 28 | SO2.def(py::init()); 29 | SO2.def(py::init()); 30 | 31 | SO2.def("transform", &manif::SO2d::transform, "Get the transformation matrix"); 32 | SO2.def("rotation", &manif::SO2d::rotation, "Get the rotation matrix"); 33 | SO2.def("real", &manif::SO2d::real); 34 | SO2.def("imag", &manif::SO2d::imag); 35 | SO2.def("angle", &manif::SO2d::angle); 36 | SO2.def("normalize", &manif::SO2d::normalize); 37 | 38 | // tangent 39 | 40 | wrap_tangent_base>(SO2_tan); 41 | 42 | SO2_tan.def(py::init()); 43 | SO2_tan.def("angle", &manif::SO2Tangentd::angle); 44 | } 45 | -------------------------------------------------------------------------------- /python/bindings_so3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "manif/SO3.h" 7 | 8 | #include "bindings_optional.h" 9 | #include "bindings_lie_group_base.h" 10 | #include "bindings_tangent_base.h" 11 | 12 | namespace py = pybind11; 13 | 14 | void wrap_SO3(py::module &m) 15 | { 16 | using Scalar = manif::SO3d::Scalar; 17 | using Quaternion = Eigen::Quaternion; 18 | 19 | py::class_, std::unique_ptr, py::nodelete>> SO3_base(m, "_SO3Base"); 20 | py::class_, std::unique_ptr, py::nodelete>> SO3_tan_base(m, "_SO3TangentBase"); 21 | 22 | py::class_> SO3(m, "SO3"); 23 | py::class_> SO3_tan(m, "SO3Tangent"); 24 | 25 | // group 26 | 27 | SO3.def(py::init()); 28 | SO3.def(py::init()); 29 | SO3.def(py::init([](const Eigen::Matrix& quat) { 30 | if(abs(quat.norm() - Scalar(1)) >= manif::Constants::eps) { 31 | throw pybind11::value_error("The quaternion is not normalized!"); 32 | } 33 | 34 | return manif::SO3d(quat); 35 | }), 36 | py::arg("quaternion")); 37 | 38 | // SO3.def(py::init()); 39 | // SO3.def(py::init&>()); 40 | 41 | wrap_lie_group_base>(SO3); 42 | 43 | SO3.def("transform", &manif::SO3d::transform); 44 | SO3.def("rotation", &manif::SO3d::rotation); 45 | SO3.def("x", &manif::SO3d::x); 46 | SO3.def("y", &manif::SO3d::y); 47 | SO3.def("z", &manif::SO3d::z); 48 | SO3.def("w", &manif::SO3d::w); 49 | SO3.def( 50 | "quat", 51 | [](const manif::SO3d& so3) -> Eigen::Matrix { return so3.coeffs(); }); 52 | 53 | SO3.def( 54 | "quat", 55 | [](manif::SO3d& so3, const Eigen::Matrix& quaternion) { 56 | if(abs(quaternion.norm() - Scalar(1)) >= manif::Constants::eps) { 57 | throw pybind11::value_error("The quaternion is not normalized!"); 58 | } 59 | so3.quat(quaternion); 60 | }, 61 | py::arg("quaternion")); 62 | 63 | SO3.def("normalize", &manif::SO3d::normalize); 64 | 65 | // tangent 66 | 67 | wrap_tangent_base>(SO3_tan); 68 | 69 | SO3_tan.def("x", &manif::SO3Tangentd::x); 70 | SO3_tan.def("y", &manif::SO3Tangentd::y); 71 | SO3_tan.def("z", &manif::SO3Tangentd::z); 72 | } 73 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | name = manifpy 3 | description = A small library for Lie theory. 4 | long_description = file: README.md; charset=UTF-8 5 | long_description_content_type = text/markdown 6 | author = Jeremie Deray 7 | author_email = deray.jeremie@gmail.com 8 | license = MIT 9 | platforms = any 10 | url = https://github.com/artivis/manif 11 | project_urls = 12 | Source = https://github.com/artivis/manif 13 | Tracker = https://github.com/artivis/manif/issues 14 | keywords = geometry lie-theory state-estimation slam robotics computer-vision 15 | classifiers = 16 | Development Status :: 5 - Production/Stable 17 | Operating System :: OS Independent 18 | Operating System :: POSIX :: Linux 19 | Operating System :: MacOS 20 | Operating System :: Microsoft :: Windows 21 | Framework :: Robot Framework 22 | Intended Audience :: Science/Research 23 | Intended Audience :: Developers 24 | Intended Audience :: Education 25 | Programming Language :: C++ 26 | Programming Language :: Python :: 3 :: Only 27 | Programming Language :: Python :: 3 28 | Programming Language :: Python :: 3.6 29 | Programming Language :: Python :: 3.7 30 | Programming Language :: Python :: 3.8 31 | Programming Language :: Python :: 3.9 32 | License :: OSI Approved :: MIT License 33 | 34 | [options] 35 | zip_safe = False 36 | python_requires = >=3.6 37 | 38 | [options.extras_require] 39 | testing = 40 | pytest 41 | numpy 42 | all = 43 | %(testing)s 44 | 45 | [tool:pytest] 46 | testpaths = test/python 47 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | from cmake_build_extension import BuildExtension, CMakeExtension 4 | from setuptools import setup 5 | 6 | setup( 7 | ext_modules=[ 8 | CMakeExtension( 9 | name="CMakeProject", 10 | install_prefix="manifpy", 11 | cmake_depends_on=["pybind11"], 12 | disable_editable=True, 13 | cmake_configure_options=[ 14 | "-DCALL_FROM_SETUP_PY:BOOL=ON", 15 | "-DBUILD_PYTHON_BINDINGS:BOOL=ON", 16 | ], 17 | ) 18 | ], 19 | cmdclass=dict(build_ext=BuildExtension), 20 | ) 21 | -------------------------------------------------------------------------------- /test/autodiff/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # autodiff tests 2 | 3 | if(NOT MSVC) 4 | include(CheckCXXCompilerFlag) 5 | CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17) 6 | if(COMPILER_SUPPORTS_CXX17) 7 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++17 support.") 8 | else() 9 | message(FATAL_ERROR 10 | "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support." 11 | "The library 'autodiff' requires a C++17 compliant compiler." 12 | ) 13 | endif() 14 | endif() 15 | 16 | manif_add_gtest(gtest_autodiff gtest_autodiff.cpp) 17 | 18 | set(CXX_17_TEST_TARGETS_AUTODIFF 19 | gtest_autodiff 20 | ) 21 | 22 | foreach(target ${CXX_17_TEST_TARGETS_AUTODIFF}) 23 | target_link_libraries(${target} autodiff::autodiff) 24 | endforeach() 25 | 26 | # Set required C++17 flag 27 | set_property(TARGET ${CXX_17_TEST_TARGETS_AUTODIFF} PROPERTY CXX_STANDARD 17) 28 | set_property(TARGET ${CXX_17_TEST_TARGETS_AUTODIFF} PROPERTY CXX_STANDARD_REQUIRED ON) 29 | set_property(TARGET ${CXX_17_TEST_TARGETS_AUTODIFF} PROPERTY CXX_EXTENSIONS OFF) 30 | -------------------------------------------------------------------------------- /test/autodiff/gtest_autodiff.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/manif.h" 2 | 3 | #include "gtest_common_tester_autodiff.h" 4 | 5 | MANIF_TEST_AUTODIFF_ALL; 6 | 7 | MANIF_RUN_ALL_TEST; 8 | -------------------------------------------------------------------------------- /test/autodiff/gtest_autodiff_reverse.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/manif.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "gtest_common_tester_autodiff.h" 7 | 8 | // TEST(autodiff, autodiff_atan2) { 9 | 10 | // auto f = [](const dual& x, const dual& y) -> dual { 11 | // return atan2(x, y); 12 | // }; 13 | 14 | // dual x{1}, y{0}; 15 | // dual t = f(x, y); 16 | 17 | // double dtdx = derivative(f, wrt(x), at(x, y)); 18 | // double dtdy = derivative(f, wrt(y), at(x, y)); 19 | 20 | // std::cout << "x = " << x << "\n"; 21 | // std::cout << "y = " << y << "\n"; 22 | // std::cout << "dtdx = " << dtdx << "\n"; 23 | // std::cout << "dtdy = " << dtdy << "\n"; 24 | // } 25 | 26 | // TEST(autodiff, autodiff_atan2_real) { 27 | 28 | // auto f = [](const real& x, const real& y) -> real { 29 | // return atan2(x, y); 30 | // }; 31 | 32 | // real x{1}, y{0}; 33 | // real t = f(x, y); 34 | 35 | // double dtdx = derivative(f, wrt(x), at(x, y)); 36 | // double dtdy = derivative(f, wrt(y), at(x, y)); 37 | 38 | // std::cout << "x = " << x << "\n"; 39 | // std::cout << "y = " << y << "\n"; 40 | // std::cout << "dtdx = " << dtdx << "\n"; 41 | // std::cout << "dtdy = " << dtdy << "\n"; 42 | // } 43 | 44 | 45 | // template using rreal = autodiff::Real<1, T>; 46 | // template using ddual = autodiff::HigherOrderDual<1, T>; 47 | template using vvar = autodiff::Variable; 48 | 49 | #define __MANIF_MAKE_TEST_AUTODIFF(manifold, type, ad) \ 50 | using manifold##type = manifold; \ 51 | using ad##type##manifold = ad; \ 52 | MANIF_TEST_AUTODIFF(manifold##type, ad##type##manifold) 53 | 54 | #define __MANIF_MAKE_TEST_AUTODIFF_ALL_AD(manifold, type) \ 55 | __MANIF_MAKE_TEST_AUTODIFF(manifold, type, vvar) 56 | 57 | // @todo real seem to have a few issues 58 | //__MANIF_MAKE_TEST_AUTODIFF(manifold, type, rreal) 59 | 60 | #define __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(manifold) \ 61 | __MANIF_MAKE_TEST_AUTODIFF_ALL_AD(manifold, double) 62 | 63 | // @todo float test is too flaky (in [1e-1, 1e-6]) 64 | //__MANIF_MAKE_TEST_AUTODIFF_ALL_AD(manifold, float) 65 | 66 | #define MANIF_TEST_AUTODIFF_ALL \ 67 | using namespace autodiff; \ 68 | using namespace manif; \ 69 | __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(R2) \ 70 | __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(R5) \ 71 | __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SO2) \ 72 | __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SO3) \ 73 | __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SE2) \ 74 | __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SE3) \ 75 | __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SE_2_3) 76 | 77 | MANIF_TEST_AUTODIFF_ALL; 78 | 79 | MANIF_RUN_ALL_TEST; 80 | -------------------------------------------------------------------------------- /test/bundle/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Bundle tests 2 | 3 | manif_add_gtest(gtest_bundle gtest_bundle.cpp) 4 | manif_add_gtest(gtest_bundle_single_group gtest_bundle_single_group.cpp) 5 | manif_add_gtest(gtest_bundle_large gtest_bundle_large.cpp) 6 | 7 | set(CXX_TEST_TARGETS 8 | 9 | ${CXX_TEST_TARGETS} 10 | 11 | gtest_bundle 12 | gtest_bundle_single_group 13 | gtest_bundle_large 14 | 15 | PARENT_SCOPE 16 | ) 17 | -------------------------------------------------------------------------------- /test/bundle/gtest_bundle_large.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "manif/manif.h" 4 | 5 | #include "../common_tester.h" 6 | 7 | using namespace manif; 8 | 9 | using GroupC = Bundle; 10 | using GroupD = Bundle; 11 | 12 | MANIF_TEST(GroupC); 13 | MANIF_TEST(GroupD); 14 | 15 | MANIF_TEST_MAP(GroupC); 16 | MANIF_TEST_MAP(GroupD); 17 | 18 | MANIF_TEST_JACOBIANS(GroupC); 19 | MANIF_TEST_JACOBIANS(GroupD); 20 | 21 | MANIF_RUN_ALL_TEST; 22 | -------------------------------------------------------------------------------- /test/bundle/gtest_bundle_single_group.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "manif/manif.h" 4 | 5 | #include "../common_tester.h" 6 | 7 | using namespace manif; 8 | 9 | using GroupB1 = Bundle; 10 | using GroupB2 = Bundle; 11 | using GroupB3 = Bundle; 12 | using GroupB4 = Bundle; 13 | using GroupB5 = Bundle; 14 | 15 | MANIF_TEST(GroupB1); 16 | MANIF_TEST(GroupB2); 17 | MANIF_TEST(GroupB3); 18 | MANIF_TEST(GroupB4); 19 | MANIF_TEST(GroupB5); 20 | 21 | MANIF_TEST_MAP(GroupB1); 22 | MANIF_TEST_MAP(GroupB2); 23 | MANIF_TEST_MAP(GroupB3); 24 | MANIF_TEST_MAP(GroupB4); 25 | MANIF_TEST_MAP(GroupB5); 26 | 27 | MANIF_TEST_JACOBIANS(GroupB1); 28 | MANIF_TEST_JACOBIANS(GroupB2); 29 | MANIF_TEST_JACOBIANS(GroupB3); 30 | MANIF_TEST_JACOBIANS(GroupB4); 31 | MANIF_TEST_JACOBIANS(GroupB5); 32 | 33 | MANIF_RUN_ALL_TEST; 34 | -------------------------------------------------------------------------------- /test/ceres/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Ceres tests 2 | 3 | manif_add_gtest(gtest_rn_ceres gtest_rn_ceres.cpp) 4 | 5 | manif_add_gtest(gtest_so2_ceres gtest_so2_ceres.cpp) 6 | 7 | manif_add_gtest(gtest_se2_ceres_autodiff gtest_se2_ceres_autodiff.cpp) 8 | manif_add_gtest(gtest_se2_ceres gtest_se2_ceres.cpp) 9 | 10 | manif_add_gtest(gtest_so3_ceres gtest_so3_ceres.cpp) 11 | 12 | manif_add_gtest(gtest_se3_ceres gtest_se3_ceres.cpp) 13 | 14 | manif_add_gtest(gtest_se23_ceres gtest_se23_ceres.cpp) 15 | 16 | manif_add_gtest(gtest_sgal3_ceres gtest_sgal3_ceres.cpp) 17 | 18 | manif_add_gtest(gtest_bundle_ceres gtest_bundle_ceres.cpp) 19 | 20 | set(CXX_TEST_TARGETS_CERES 21 | # Rn 22 | gtest_rn_ceres 23 | 24 | # SO2 25 | gtest_so2_ceres 26 | 27 | # SO3 28 | gtest_so3_ceres 29 | 30 | # SE2 31 | gtest_se2_ceres_autodiff 32 | gtest_se2_ceres 33 | 34 | # SE3 35 | gtest_se3_ceres 36 | 37 | # SE23 38 | gtest_se23_ceres 39 | 40 | # SGal3 41 | gtest_sgal3_ceres 42 | 43 | # Bundle 44 | gtest_bundle_ceres 45 | ) 46 | 47 | foreach(target ${CXX_TEST_TARGETS_CERES}) 48 | target_link_libraries(${target} ${CERES_LIBRARIES}) 49 | target_include_directories(${target} SYSTEM PRIVATE ${CERES_INCLUDE_DIRS}) 50 | endforeach() 51 | 52 | set(CXX_TEST_TARGETS 53 | ${CXX_TEST_TARGETS} 54 | ${CXX_TEST_TARGETS_CERES} 55 | 56 | PARENT_SCOPE 57 | ) 58 | -------------------------------------------------------------------------------- /test/ceres/gtest_bundle_ceres.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/Bundle.h" 2 | #include "manif/Rn.h" 3 | #include "manif/SO2.h" 4 | #include "manif/SO3.h" 5 | 6 | #include "ceres_test_utils.h" 7 | 8 | #include 9 | 10 | using namespace manif; 11 | 12 | using Group = Bundle; 13 | 14 | MANIF_TEST_JACOBIANS_CERES(Group); 15 | 16 | MANIF_RUN_ALL_TEST; 17 | -------------------------------------------------------------------------------- /test/ceres/gtest_rn_ceres.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/Rn.h" 2 | #include "ceres_test_utils.h" 3 | 4 | #include 5 | 6 | using namespace manif; 7 | 8 | MANIF_TEST_JACOBIANS_CERES(R1d); 9 | MANIF_TEST_JACOBIANS_CERES(R3d); 10 | MANIF_TEST_JACOBIANS_CERES(R5d); 11 | MANIF_TEST_JACOBIANS_CERES(R7d); 12 | MANIF_TEST_JACOBIANS_CERES(R9d); 13 | 14 | MANIF_RUN_ALL_TEST; 15 | -------------------------------------------------------------------------------- /test/ceres/gtest_se23_ceres.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/SE_2_3.h" 2 | #include "ceres_test_utils.h" 3 | 4 | #include 5 | 6 | using namespace manif; 7 | 8 | MANIF_TEST_JACOBIANS_CERES(SE_2_3d); 9 | 10 | MANIF_RUN_ALL_TEST; 11 | -------------------------------------------------------------------------------- /test/ceres/gtest_se2_ceres.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/SE2.h" 2 | #include "ceres_test_utils.h" 3 | 4 | #include 5 | 6 | using namespace manif; 7 | 8 | MANIF_TEST_JACOBIANS_CERES(SE2d); 9 | 10 | MANIF_RUN_ALL_TEST; 11 | -------------------------------------------------------------------------------- /test/ceres/gtest_se3_ceres.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/SE3.h" 2 | #include "ceres_test_utils.h" 3 | 4 | #include 5 | 6 | using namespace manif; 7 | 8 | MANIF_TEST_JACOBIANS_CERES(SE3d); 9 | 10 | MANIF_RUN_ALL_TEST; 11 | -------------------------------------------------------------------------------- /test/ceres/gtest_sgal3_ceres.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/SGal3.h" 2 | #include "ceres_test_utils.h" 3 | 4 | #include 5 | 6 | using namespace manif; 7 | 8 | MANIF_TEST_JACOBIANS_CERES(SGal3d); 9 | 10 | MANIF_RUN_ALL_TEST; 11 | -------------------------------------------------------------------------------- /test/ceres/gtest_so3_ceres.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/SO3.h" 2 | #include "ceres_test_utils.h" 3 | 4 | #include 5 | 6 | using namespace manif; 7 | 8 | MANIF_TEST_JACOBIANS_CERES(SO3d); 9 | 10 | MANIF_RUN_ALL_TEST; 11 | -------------------------------------------------------------------------------- /test/gtest/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | 3 | project(googletest-download NONE) 4 | 5 | include(ExternalProject) 6 | ExternalProject_Add(googletest 7 | GIT_REPOSITORY https://github.com/google/googletest.git 8 | GIT_TAG v1.12.0 9 | SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src" 10 | BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build" 11 | CONFIGURE_COMMAND "" 12 | BUILD_COMMAND "" 13 | INSTALL_COMMAND "" 14 | TEST_COMMAND "" 15 | ) 16 | -------------------------------------------------------------------------------- /test/gtest_misc.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "manif/manif.h" 4 | #include "gtest_manif_utils.h" 5 | #include "gtest_eigen_utils.h" 6 | 7 | using namespace manif; 8 | 9 | TEST(TEST_MISC, TEST_SKEW) 10 | { 11 | constexpr double s = 1.5; 12 | 13 | EXPECT_EIGEN_NEAR((Eigen::Matrix2d() << 0., -s, s, 0.).finished(), skew(s)); 14 | } 15 | 16 | TEST(TEST_MISC, TEST_SKEW3) 17 | { 18 | EXPECT_EIGEN_NEAR( 19 | (Eigen::Matrix3d() << 20 | 0., -3.7, +2.6, 21 | +3.7, 0., -1.5, 22 | -2.6, +1.5, 0.).finished(), 23 | skew(Eigen::Vector3d(1.5, 2.6, 3.7)) 24 | ); 25 | } 26 | 27 | TEST(TEST_MISC, TEST_SKEWd) 28 | { 29 | Eigen::VectorXd s(1, 1); 30 | s << 1.5; 31 | 32 | EXPECT_EIGEN_NEAR((Eigen::Matrix2d() << 0., -1.5, 1.5, 0.).finished(), skew(s)); 33 | 34 | s.resize(3, 1); 35 | s << 1.5, 2.6, 3.7; 36 | 37 | EXPECT_EIGEN_NEAR((Eigen::Matrix3d() << 38 | 0., -3.7, +2.6, 39 | +3.7, 0., -1.5, 40 | -2.6, +1.5, 0.).finished(), skew(s)); 41 | 42 | s.resize(5, 1); 43 | s << 1.5, 2.6, 3.7, 1., 1.; 44 | 45 | EXPECT_THROW(skew(s), manif::runtime_error); 46 | } 47 | 48 | MANIF_RUN_ALL_TEST; 49 | -------------------------------------------------------------------------------- /test/python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # small helper function 2 | function(manif_add_pytest target) 3 | add_test( 4 | NAME ${target} 5 | COMMAND pytest-3 ${target}.py 6 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 7 | ) 8 | set_tests_properties(${target} 9 | PROPERTIES ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}:$ENV{PYTHONPATH}" 10 | ) 11 | endfunction() 12 | 13 | manif_add_pytest(test_manif) 14 | manif_add_pytest(test_so2) 15 | manif_add_pytest(test_se2) 16 | manif_add_pytest(test_so3) 17 | manif_add_pytest(test_se3) 18 | manif_add_pytest(test_se_2_3) 19 | -------------------------------------------------------------------------------- /test/python/test_se2.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytest 3 | 4 | from manifpy import SE2, SE2Tangent 5 | 6 | 7 | def test_constructor(): 8 | state = SE2(4, 2, 1, 0) 9 | assert 4 == state.x() 10 | assert 2 == state.y() 11 | assert 1 == state.real() 12 | assert 0 == state.imag() 13 | assert 0 == state.angle() 14 | 15 | state = SE2(2, 4, 0.17) 16 | assert 2 == state.x() 17 | assert 4 == state.y() 18 | assert 0.17 == state.angle() 19 | 20 | state = SE2(4, 2, 1+0j) 21 | assert 4 == state.x() 22 | assert 2 == state.y() 23 | assert 0 == state.angle() 24 | 25 | state = SE2(np.array([2, 4]), 1+0j) 26 | assert 2 == state.x() 27 | assert 4 == state.y() 28 | assert 0 == state.angle() 29 | 30 | delta = SE2Tangent(4, 2, 0.17) 31 | assert 4 == delta.x() 32 | assert 2 == delta.y() 33 | assert 0.17 == delta.angle() 34 | 35 | def test_accessors(): 36 | state = SE2.Identity() 37 | 38 | assert 0 == state.x() 39 | assert 0 == state.y() 40 | assert 1 == state.real() 41 | assert 0 == state.imag() 42 | assert 0 == state.angle() 43 | 44 | delta = SE2Tangent.Zero() 45 | 46 | assert 0 == delta.x() 47 | assert 0 == delta.y() 48 | assert 0 == delta.angle() 49 | 50 | def test_transform(): 51 | state = SE2.Identity() 52 | transform = state.transform() 53 | 54 | assert (3, 3) == transform.shape 55 | assert (np.identity(3) == transform).all() 56 | 57 | # def test_isometry(): 58 | # state = SE2.Identity() 59 | # isometry = state.isometry() 60 | # 61 | # assert (3, 3) == isometry.shape 62 | # assert (np.identity(3) == isometry).all() 63 | 64 | def test_rotation(): 65 | state = SE2.Identity() 66 | rotation = state.rotation() 67 | 68 | assert (2, 2) == rotation.shape 69 | assert (np.identity(2) == rotation).all() 70 | 71 | def test_translation(): 72 | state = SE2.Identity() 73 | translation = state.translation() 74 | 75 | assert (2,) == translation.shape 76 | assert (np.zeros(2,) == translation).all() 77 | -------------------------------------------------------------------------------- /test/python/test_se3.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytest 3 | 4 | from manifpy import SE3, SE3Tangent 5 | 6 | 7 | def test_constructor(): 8 | state = SE3(0,0,0,0,0,0) 9 | assert 0 == state.x() 10 | assert 0 == state.y() 11 | assert 0 == state.z() 12 | # assert 1 == state.quat() 13 | 14 | state = SE3(position=np.array([1,2,3]), quaternion=np.array([0,0,0,1])) 15 | assert 1 == state.x() 16 | assert 2 == state.y() 17 | assert 3 == state.z() 18 | assert ([0, 0, 0, 1] == state.quat()).all() 19 | 20 | with pytest.raises(ValueError): 21 | state = SE3(position=np.array([1,2,3]), quaternion=np.array([1, 0, 0, 1])) 22 | 23 | # state = SE3(np.array([1,2,3]), AngleAxis(0, UnitX())) 24 | # assert 0 == state.x() 25 | # assert 0 == state.y() 26 | # assert 0 == state.z() 27 | 28 | # delta = SE3Tangent(1,2,3,4,5,6) 29 | # assert 1 == delta.x() 30 | # assert 2 == delta.y() 31 | # assert 3 == delta.z() 32 | 33 | def test_accessors(): 34 | state = SE3.Identity() 35 | 36 | assert 0 == state.x() 37 | assert 0 == state.y() 38 | assert 0 == state.z() 39 | 40 | assert (np.zeros([1,3]) == state.translation()).all() 41 | assert ([0, 0, 0, 1] == state.quat()).all() 42 | 43 | state.translation([1, 2, 3]) 44 | assert ([1, 2, 3] == state.translation()).all() 45 | 46 | state.quat([0, 1, 0, 0]) 47 | assert ([0, 1, 0, 0] == state.quat()).all() 48 | 49 | with pytest.raises(ValueError): 50 | state.quat([0, 1, 0, 1]) 51 | 52 | delta = SE3Tangent.Zero() 53 | 54 | # assert 0 == delta.x() 55 | # assert 0 == delta.y() 56 | # assert 0 == delta.z() 57 | 58 | def test_transform(): 59 | state = SE3.Identity() 60 | transform = state.transform() 61 | 62 | assert (4, 4) == transform.shape 63 | assert (np.identity(4) == transform).all() 64 | 65 | def test_rotation(): 66 | state = SE3.Identity() 67 | rotation = state.rotation() 68 | 69 | assert (3, 3) == rotation.shape 70 | assert (np.identity(3) == rotation).all() 71 | 72 | # def test_quaternion(): 73 | # state = SO3.Identity() 74 | # quaternion = state.quaternion() 75 | # 76 | # assert (4,) == quaternion.shape 77 | # assert (np.zeros(4,) == quaternion).all() 78 | 79 | # def test_translation(): 80 | # state = SE3.Identity() 81 | # translation = state.translation() 82 | # 83 | # assert (3,) == translation.shape 84 | # assert (np.zeros(3,) == translation).all() 85 | -------------------------------------------------------------------------------- /test/python/test_se_2_3.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytest 3 | 4 | from manifpy import SE_2_3, SE_2_3Tangent 5 | 6 | 7 | def test_constructor(): 8 | state = SE_2_3(0,0,0, 0,0,0, 0,0,0) 9 | assert 0 == state.x() 10 | assert 0 == state.y() 11 | assert 0 == state.z() 12 | # assert 0 == state.quat() 13 | assert 0 == state.vx() 14 | assert 0 == state.vy() 15 | assert 0 == state.vz() 16 | 17 | # state = SE_2_3(np.array([1,2,3]), Quaternion(1,0,0,0), np.array([4,5,6])) 18 | # assert 1 == state.x() 19 | # assert 2 == state.y() 20 | # assert 3 == state.z() 21 | # assert 1 == state.quat() 22 | # assert 4 == state.vx() 23 | # assert 5 == state.vy() 24 | # assert 6 == state.vz() 25 | 26 | # state = SE_2_3(np.array([1,2,3]), AngleAxis(0, UnitX()), np.array([4,5,6])) 27 | # assert 1 == state.x() 28 | # assert 2 == state.y() 29 | # assert 3 == state.z() 30 | # assert 1 == state.w() 31 | # assert 4 == state.vx() 32 | # assert 5 == state.vy() 33 | # assert 6 == state.vz() 34 | 35 | # delta = SE_2_3Tangent(1,2,3) 36 | # assert 1 == delta.x() 37 | # assert 2 == delta.y() 38 | # assert 3 == delta.z() 39 | 40 | def test_accessors(): 41 | state = SE_2_3.Identity() 42 | assert 0 == state.x() 43 | assert 0 == state.y() 44 | assert 0 == state.z() 45 | # assert 0 == state.quat() 46 | assert 0 == state.vx() 47 | assert 0 == state.vy() 48 | assert 0 == state.vz() 49 | 50 | delta = SE_2_3Tangent.Zero() 51 | 52 | # assert 0 == delta.x() 53 | # assert 0 == delta.y() 54 | # assert 0 == delta.z() 55 | 56 | # def test_isometry(): 57 | # state = SE_2_3.Identity() 58 | # isometry = state.isometry() 59 | # 60 | # assert (4, 4) == isometry.shape 61 | # assert (np.identity(4) == isometry).all() 62 | 63 | def test_rotation(): 64 | state = SE_2_3.Identity() 65 | rotation = state.rotation() 66 | 67 | assert (3, 3) == rotation.shape 68 | assert (np.identity(3) == rotation).all() 69 | 70 | # def test_quaternion(): 71 | # state = SO3.Identity() 72 | # quaternion = state.quaternion() 73 | # 74 | # assert (4,) == quaternion.shape 75 | # assert (np.zeros(4,) == quaternion).all() 76 | 77 | def test_translation(): 78 | state = SE_2_3.Identity() 79 | translation = state.translation() 80 | 81 | assert (3,) == translation.shape 82 | assert (np.zeros(3,) == translation).all() 83 | -------------------------------------------------------------------------------- /test/python/test_sgal3.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytest 3 | from manifpy import SGal3, SGal3Tangent 4 | 5 | 6 | def test_constructor(): 7 | state = SGal3(0,0,0, 0,0,0, 0,0,0, 0) 8 | assert 0 == state.x() 9 | assert 0 == state.y() 10 | assert 0 == state.z() 11 | # assert 0 == state.quat() 12 | assert 0 == state.vx() 13 | assert 0 == state.vy() 14 | assert 0 == state.vz() 15 | 16 | # state = SE_2_3(np.array([1,2,3]), Quaternion(1,0,0,0), np.array([4,5,6])) 17 | # assert 1 == state.x() 18 | # assert 2 == state.y() 19 | # assert 3 == state.z() 20 | # assert 1 == state.quat() 21 | # assert 4 == state.vx() 22 | # assert 5 == state.vy() 23 | # assert 6 == state.vz() 24 | 25 | # state = SE_2_3(np.array([1,2,3]), AngleAxis(0, UnitX()), np.array([4,5,6])) 26 | # assert 1 == state.x() 27 | # assert 2 == state.y() 28 | # assert 3 == state.z() 29 | # assert 1 == state.w() 30 | # assert 4 == state.vx() 31 | # assert 5 == state.vy() 32 | # assert 6 == state.vz() 33 | 34 | # delta = SE_2_3Tangent(1,2,3) 35 | # assert 1 == delta.x() 36 | # assert 2 == delta.y() 37 | # assert 3 == delta.z() 38 | 39 | def test_accessors(): 40 | state = SGal3.Identity() 41 | assert 0 == state.x() 42 | assert 0 == state.y() 43 | assert 0 == state.z() 44 | # assert 0 == state.quat() 45 | assert 0 == state.vx() 46 | assert 0 == state.vy() 47 | assert 0 == state.vz() 48 | 49 | delta = SGal3Tangent.Zero() 50 | 51 | # assert 0 == delta.x() 52 | # assert 0 == delta.y() 53 | # assert 0 == delta.z() 54 | 55 | # def test_isometry(): 56 | # state = SE_2_3.Identity() 57 | # isometry = state.isometry() 58 | # 59 | # assert (4, 4) == isometry.shape 60 | # assert (np.identity(4) == isometry).all() 61 | 62 | def test_rotation(): 63 | state = SGal3.Identity() 64 | rotation = state.rotation() 65 | 66 | assert (3, 3) == rotation.shape 67 | assert (np.identity(3) == rotation).all() 68 | 69 | # def test_quaternion(): 70 | # state = SO3.Identity() 71 | # quaternion = state.quaternion() 72 | # 73 | # assert (4,) == quaternion.shape 74 | # assert (np.zeros(4,) == quaternion).all() 75 | 76 | def test_translation(): 77 | state = SGal3.Identity() 78 | translation = state.translation() 79 | 80 | assert (3,) == translation.shape 81 | assert (np.zeros(3,) == translation).all() 82 | -------------------------------------------------------------------------------- /test/python/test_so2.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytest 3 | 4 | from manifpy import SO2, SO2Tangent 5 | 6 | 7 | def test_constructor(): 8 | state = SO2(0.17) 9 | assert 0.17 == state.angle() 10 | 11 | state = SO2(1, 0) 12 | assert 0 == state.angle() 13 | 14 | delta = SO2Tangent(0.17) 15 | assert 0.17 == delta.angle() 16 | 17 | def test_accessors(): 18 | state = SO2.Identity() 19 | 20 | assert 1 == state.real() 21 | assert 0 == state.imag() 22 | assert 0 == state.angle() 23 | 24 | delta = SO2Tangent.Zero() 25 | 26 | assert 0 == delta.angle() 27 | 28 | def test_transform(): 29 | state = SO2.Identity() 30 | transform = state.transform() 31 | 32 | assert (3, 3) == transform.shape 33 | assert (np.identity(3) == transform).all() 34 | 35 | def test_rotation(): 36 | state = SO2.Identity() 37 | rotation = state.rotation() 38 | 39 | assert (2, 2) == rotation.shape 40 | assert (np.identity(2) == rotation).all() 41 | -------------------------------------------------------------------------------- /test/python/test_so3.py: -------------------------------------------------------------------------------- 1 | from manifpy import SO3, SO3Tangent 2 | 3 | import numpy as np 4 | import pytest 5 | 6 | def test_constructor(): 7 | state = SO3(0, 0, 0, 1) 8 | assert 0 == state.x() 9 | assert 0 == state.y() 10 | assert 0 == state.z() 11 | assert 1 == state.w() 12 | 13 | state = SO3(np.array([0, 0, 0, 1])) 14 | assert 0 == state.x() 15 | assert 0 == state.y() 16 | assert 0 == state.z() 17 | assert 1 == state.w() 18 | 19 | state = SO3(0, 0, 0) 20 | assert 0 == state.x() 21 | assert 0 == state.y() 22 | assert 0 == state.z() 23 | assert 1 == state.w() 24 | 25 | state = SO3(quaternion=np.array([0, 0, 0, 1])) 26 | assert 0 == state.x() 27 | assert 0 == state.y() 28 | assert 0 == state.z() 29 | assert 1 == state.w() 30 | 31 | with pytest.raises(ValueError): 32 | state = SO3(quaternion=np.array([1, 0, 0, 1])) 33 | 34 | # state = SO3(AngleAxis(0, UnitX())) 35 | # assert 0 == state.x() 36 | # assert 0 == state.y() 37 | # assert 0 == state.z() 38 | # assert 1 == state.w() 39 | 40 | # delta = SO3Tangent(1,2,3) 41 | # assert 1 == delta.x() 42 | # assert 2 == delta.y() 43 | # assert 3 == delta.z() 44 | 45 | 46 | def test_accessors(): 47 | state = SO3.Identity() 48 | 49 | assert 0 == state.x() 50 | assert 0 == state.y() 51 | assert 0 == state.z() 52 | assert 1 == state.w() 53 | 54 | assert ([0, 0, 0, 1] == state.quat()).all() 55 | 56 | state.quat([0, 1, 0, 0]) 57 | assert ([0, 1, 0, 0] == state.quat()).all() 58 | 59 | with pytest.raises(ValueError): 60 | state.quat([0, 1, 0, 1]) 61 | 62 | delta = SO3Tangent.Zero() 63 | 64 | assert 0 == delta.x() 65 | assert 0 == delta.y() 66 | assert 0 == delta.z() 67 | 68 | 69 | def test_transform(): 70 | state = SO3.Identity() 71 | transform = state.transform() 72 | 73 | assert (4, 4) == transform.shape 74 | assert (np.identity(4) == transform).all() 75 | 76 | 77 | def test_rotation(): 78 | state = SO3.Identity() 79 | rotation = state.rotation() 80 | 81 | assert (3, 3) == rotation.shape 82 | assert (np.identity(3) == rotation).all() 83 | 84 | 85 | # def test_quaternion(): 86 | # state = SO3.Identity() 87 | # quaternion = state.quat() 88 | 89 | # assert Quaternion.Identity().isApprox(quaternion) 90 | -------------------------------------------------------------------------------- /test/rn/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SO3 tests 2 | 3 | manif_add_gtest(gtest_rn gtest_rn.cpp) 4 | 5 | set(CXX_TEST_TARGETS 6 | 7 | ${CXX_TEST_TARGETS} 8 | 9 | # R^n 10 | gtest_rn 11 | 12 | PARENT_SCOPE 13 | ) 14 | -------------------------------------------------------------------------------- /test/se2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SE2 tests 2 | 3 | manif_add_gtest(gtest_se2 gtest_se2.cpp) 4 | manif_add_gtest(gtest_se2_map gtest_se2_map.cpp) 5 | manif_add_gtest(gtest_se2_tangent gtest_se2_tangent.cpp) 6 | manif_add_gtest(gtest_se2_tangent_map gtest_se2_tangent_map.cpp) 7 | 8 | set(CXX_TEST_TARGETS 9 | 10 | ${CXX_TEST_TARGETS} 11 | 12 | # SE2 13 | gtest_se2 14 | gtest_se2_map 15 | gtest_se2_tangent 16 | gtest_se2_tangent_map 17 | 18 | PARENT_SCOPE 19 | ) 20 | -------------------------------------------------------------------------------- /test/se2/gtest_se2_tangent_map.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/SE2.h" 2 | 3 | #include "../gtest_manif_utils.h" 4 | 5 | using namespace manif; 6 | 7 | TEST(TEST_SE2, TEST_SE2_TANGENT_MAP_0) 8 | { 9 | double data[3] = {4,2,MANIF_PI}; 10 | Eigen::Map so2tan(data); 11 | 12 | EXPECT_DOUBLE_EQ(4, so2tan.x()); 13 | EXPECT_DOUBLE_EQ(2, so2tan.y()); 14 | EXPECT_DOUBLE_EQ(MANIF_PI, so2tan.angle()); 15 | } 16 | 17 | TEST(TEST_SE2, TEST_SE2_TANGENT_MAP_DATA) 18 | { 19 | /// @todo without specifying const 20 | /// it calls non-const data() 21 | 22 | double data[3] = {4,2,MANIF_PI}; 23 | Eigen::Map so2tan(data); 24 | 25 | EXPECT_NE(nullptr, so2tan.data()); 26 | EXPECT_EQ(data, so2tan.data()); 27 | } 28 | 29 | TEST(TEST_SE2, TEST_SE2_TANGENT_MAP_ZERO) 30 | { 31 | double data[3] = {4,2,MANIF_PI}; 32 | Eigen::Map so2tan(data); 33 | 34 | so2tan.setZero(); 35 | 36 | EXPECT_DOUBLE_EQ(0, so2tan.x()); 37 | EXPECT_DOUBLE_EQ(0, so2tan.y()); 38 | EXPECT_DOUBLE_EQ(0, so2tan.angle()); 39 | } 40 | 41 | TEST(TEST_SE2, TEST_SE2_TANGENT_MAP_ZERO2) 42 | { 43 | double data[3] = {4,2,MANIF_PI}; 44 | Eigen::Map so2tan(data); 45 | so2tan = SE2Tangentd::Zero(); 46 | 47 | EXPECT_DOUBLE_EQ(0, so2tan.x()); 48 | EXPECT_DOUBLE_EQ(0, so2tan.y()); 49 | EXPECT_DOUBLE_EQ(0, so2tan.angle()); 50 | } 51 | 52 | //TEST(TEST_SE2, TEST_SE2_RANDOM) 53 | //{ 54 | // SE2d so2; 55 | 56 | // so2.setRandom(); 57 | 58 | // EXPECT_DOUBLE_EQ(0, so2.angle()); 59 | //} 60 | 61 | //TEST(TEST_SE2, TEST_SE2_RANDOM2) 62 | //{ 63 | // SE2d so2 = SE2d::Random(); 64 | 65 | // EXPECT_DOUBLE_EQ(0, so2.angle()); 66 | //} 67 | 68 | TEST(TEST_SE2, TEST_SE2_TANGENT_MAP_RETRACT) 69 | { 70 | double data[3] = {4,2,MANIF_PI}; 71 | Eigen::Map so2tan(data); 72 | 73 | EXPECT_DOUBLE_EQ(4, so2tan.x()); 74 | EXPECT_DOUBLE_EQ(2, so2tan.y()); 75 | EXPECT_DOUBLE_EQ(MANIF_PI, so2tan.angle()); 76 | 77 | auto so2_exp = so2tan.exp(); 78 | 79 | EXPECT_DOUBLE_EQ(std::cos(MANIF_PI), so2_exp.real()); 80 | EXPECT_DOUBLE_EQ(std::sin(MANIF_PI), so2_exp.imag()); 81 | EXPECT_DOUBLE_EQ(MANIF_PI, so2_exp.angle()); 82 | 83 | /// @todo what to expect ? :S 84 | // EXPECT_DOUBLE_EQ(0, so2_exp.x()); 85 | // EXPECT_DOUBLE_EQ(0, so2_exp.y()); 86 | } 87 | 88 | /// with Jacs 89 | 90 | TEST(TEST_SE2, TEST_SE2_TANGENT_MAP_RETRACT_JAC) 91 | { 92 | double data[3] = {4,2,MANIF_PI}; 93 | Eigen::Map so2tan(data); 94 | 95 | EXPECT_DOUBLE_EQ(4, so2tan.x()); 96 | EXPECT_DOUBLE_EQ(2, so2tan.y()); 97 | EXPECT_DOUBLE_EQ(MANIF_PI, so2tan.angle()); 98 | 99 | SE2d::Jacobian J_ret; 100 | SE2d so2_exp = so2tan.exp(J_ret); 101 | 102 | EXPECT_DOUBLE_EQ(std::cos(MANIF_PI), so2_exp.real()); 103 | EXPECT_DOUBLE_EQ(std::sin(MANIF_PI), so2_exp.imag()); 104 | EXPECT_DOUBLE_EQ(MANIF_PI, so2_exp.angle()); 105 | 106 | /// @todo what to expect ? :S 107 | // EXPECT_DOUBLE_EQ(0, so2_exp.x()); 108 | // EXPECT_DOUBLE_EQ(0, so2_exp.y()); 109 | 110 | /// @todo check this J 111 | EXPECT_EQ(3, J_ret.rows()); 112 | EXPECT_EQ(3, J_ret.cols()); 113 | // EXPECT_DOUBLE_EQ(1, J_ret(0)); 114 | } 115 | 116 | MANIF_RUN_ALL_TEST; 117 | -------------------------------------------------------------------------------- /test/se3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SE3 tests 2 | 3 | manif_add_gtest(gtest_se3 gtest_se3.cpp) 4 | 5 | set(CXX_TEST_TARGETS 6 | 7 | ${CXX_TEST_TARGETS} 8 | 9 | # SE3 10 | gtest_se3 11 | 12 | PARENT_SCOPE 13 | ) 14 | -------------------------------------------------------------------------------- /test/se_2_3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SE_2_3 tests 2 | 3 | manif_add_gtest(gtest_se_2_3 gtest_se_2_3.cpp) 4 | 5 | set(CXX_TEST_TARGETS 6 | 7 | ${CXX_TEST_TARGETS} 8 | 9 | # SE_2_3 10 | gtest_se_2_3 11 | 12 | PARENT_SCOPE 13 | ) 14 | -------------------------------------------------------------------------------- /test/sgal3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SGal3 tests 2 | 3 | manif_add_gtest(gtest_sgal3 gtest_sgal3.cpp) 4 | 5 | set(CXX_TEST_TARGETS 6 | 7 | ${CXX_TEST_TARGETS} 8 | 9 | # SGal3 10 | gtest_sgal3 11 | 12 | PARENT_SCOPE 13 | ) 14 | -------------------------------------------------------------------------------- /test/so2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SO2 tests 2 | 3 | # so2 tests 4 | manif_add_gtest(gtest_so2 gtest_so2.cpp) 5 | 6 | # so2 Eigen::Map tests 7 | manif_add_gtest(gtest_so2_map gtest_so2_map.cpp) 8 | 9 | # so2 tangent tests 10 | manif_add_gtest(gtest_so2_tangent gtest_so2_tangent.cpp) 11 | 12 | # so2 tangent Eigen::Map tests 13 | manif_add_gtest(gtest_so2_tangent_map gtest_so2_tangent_map.cpp) 14 | 15 | set(CXX_TEST_TARGETS 16 | 17 | ${CXX_TEST_TARGETS} 18 | 19 | # SO2 20 | gtest_so2 21 | gtest_so2_map 22 | gtest_so2_tangent 23 | gtest_so2_tangent_map 24 | 25 | PARENT_SCOPE 26 | ) 27 | -------------------------------------------------------------------------------- /test/so2/gtest_so2_tangent.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/SO2.h" 2 | 3 | #include "../gtest_manif_utils.h" 4 | 5 | using namespace manif; 6 | 7 | TEST(TEST_SO2, TEST_SO2_TANGENT_0) 8 | { 9 | SO2Tangentd so2tan(SO2Tangentd::DataType(MANIF_PI)); 10 | 11 | EXPECT_DOUBLE_EQ(MANIF_PI, so2tan.angle()); 12 | } 13 | 14 | TEST(TEST_SO2, TEST_SO2_TANGENT_1) 15 | { 16 | SO2Tangentd so2tan(MANIF_PI); 17 | 18 | EXPECT_DOUBLE_EQ(MANIF_PI, so2tan.angle()); 19 | } 20 | 21 | TEST(TEST_SO2, TEST_SO2_TANGENT_DATA) 22 | { 23 | const SO2Tangentd so2tan(MANIF_PI); 24 | 25 | EXPECT_DOUBLE_EQ(MANIF_PI, so2tan.coeffs()(0)); 26 | } 27 | 28 | TEST(TEST_SO2, TEST_SO2_TANGENT_ZERO) 29 | { 30 | SO2Tangentd so2tan; 31 | 32 | so2tan.setZero(); 33 | 34 | EXPECT_DOUBLE_EQ(0, so2tan.angle()); 35 | } 36 | 37 | TEST(TEST_SO2, TEST_SO2_TANGENT_ZERO2) 38 | { 39 | SO2Tangentd so2tan = SO2Tangentd::Zero(); 40 | 41 | EXPECT_DOUBLE_EQ(0, so2tan.angle()); 42 | } 43 | 44 | //TEST(TEST_SO2, TEST_SO2_RANDOM) 45 | //{ 46 | // SO2d so2; 47 | 48 | // so2.setRandom(); 49 | 50 | // EXPECT_DOUBLE_EQ(0, so2.angle()); 51 | //} 52 | 53 | //TEST(TEST_SO2, TEST_SO2_RANDOM2) 54 | //{ 55 | // SO2d so2 = SO2d::Random(); 56 | 57 | // EXPECT_DOUBLE_EQ(0, so2.angle()); 58 | //} 59 | 60 | TEST(TEST_SO2, TEST_SO2_TANGENT_RETRACT) 61 | { 62 | SO2Tangentd so2_tan(MANIF_PI); 63 | 64 | EXPECT_DOUBLE_EQ(MANIF_PI, so2_tan.angle()); 65 | 66 | auto so2_exp = so2_tan.exp(); 67 | 68 | EXPECT_DOUBLE_EQ(std::cos(MANIF_PI), so2_exp.real()); 69 | EXPECT_DOUBLE_EQ(std::sin(MANIF_PI), so2_exp.imag()); 70 | EXPECT_DOUBLE_EQ(MANIF_PI, so2_exp.angle()); 71 | } 72 | 73 | TEST(TEST_SO2, TEST_SO2_TANGENT_SKEW) 74 | { 75 | SO2Tangentd so2_tan(MANIF_PI); 76 | 77 | EXPECT_DOUBLE_EQ(MANIF_PI, so2_tan.angle()); 78 | 79 | SO2Tangentd::LieAlg so2_lie = so2_tan.hat(); 80 | 81 | EXPECT_DOUBLE_EQ( 0, so2_lie(0,0)); 82 | EXPECT_DOUBLE_EQ(-MANIF_PI, so2_lie(0,1)); 83 | EXPECT_DOUBLE_EQ( MANIF_PI, so2_lie(1,0)); 84 | EXPECT_DOUBLE_EQ( 0, so2_lie(1,1)); 85 | } 86 | 87 | /// with Jacs 88 | 89 | //TEST(TEST_SO2, TEST_SO2_TANGENT_RETRACT_JAC) 90 | //{ 91 | // SO2Tangentd so2_tan(MANIF_PI); 92 | 93 | // EXPECT_DOUBLE_EQ(MANIF_PI, so2_tan.angle()); 94 | 95 | // SO2d so2_exp; 96 | // SO2d::Jacobian J_ret; 97 | 98 | // so2_tan.exp(so2_exp, J_ret); 99 | 100 | // EXPECT_DOUBLE_EQ(std::cos(MANIF_PI), so2_exp.real()); 101 | // EXPECT_DOUBLE_EQ(std::sin(MANIF_PI), so2_exp.imag()); 102 | // EXPECT_DOUBLE_EQ(MANIF_PI, so2_exp.angle()); 103 | 104 | // /// @todo check this J 105 | // EXPECT_EQ(1, J_ret.rows()); 106 | // EXPECT_EQ(1, J_ret.cols()); 107 | // EXPECT_DOUBLE_EQ(1, J_ret(0)); 108 | //} 109 | 110 | TEST(TEST_SO2, TEST_SO2_TANGENT_RETRACT_OPTJAC) 111 | { 112 | SO2Tangentd so2_tan(MANIF_PI); 113 | 114 | EXPECT_DOUBLE_EQ(MANIF_PI, so2_tan.angle()); 115 | 116 | SO2d so2_exp; 117 | SO2d::Jacobian J_ret; 118 | 119 | so2_exp = so2_tan.exp(J_ret); 120 | 121 | EXPECT_DOUBLE_EQ(std::cos(MANIF_PI), so2_exp.real()); 122 | EXPECT_DOUBLE_EQ(std::sin(MANIF_PI), so2_exp.imag()); 123 | EXPECT_DOUBLE_EQ(MANIF_PI, so2_exp.angle()); 124 | 125 | /// @todo check this J 126 | EXPECT_EQ(1, J_ret.rows()); 127 | EXPECT_EQ(1, J_ret.cols()); 128 | EXPECT_DOUBLE_EQ(1, J_ret(0)); 129 | } 130 | 131 | MANIF_RUN_ALL_TEST; 132 | -------------------------------------------------------------------------------- /test/so2/gtest_so2_tangent_map.cpp: -------------------------------------------------------------------------------- 1 | #include "manif/SO2.h" 2 | 3 | #include "../gtest_manif_utils.h" 4 | 5 | using namespace manif; 6 | 7 | TEST(TEST_SO2, TEST_SO2_TANGENT_MAP_0) 8 | { 9 | double data(MANIF_PI); 10 | Eigen::Map so2tan(&data); 11 | 12 | EXPECT_DOUBLE_EQ(MANIF_PI, so2tan.angle()); 13 | } 14 | 15 | TEST(TEST_SO2, TEST_SO2_TANGENT_MAP_DATA) 16 | { 17 | /// @todo without specifying const 18 | /// it calls non-const data() 19 | 20 | double data(MANIF_PI); 21 | const Eigen::Map so2tan(&data); 22 | 23 | // EXPECT_NE(nullptr, so2tan.data()); 24 | // EXPECT_EQ(&data, so2tan.data()->data()); 25 | 26 | // EXPECT_DOUBLE_EQ(MANIF_PI, (*so2tan.data())(0)); 27 | } 28 | 29 | TEST(TEST_SO2, TEST_SO2_TANGENT_MAP_ZERO) 30 | { 31 | double data(1); 32 | Eigen::Map so2tan(&data); 33 | 34 | so2tan.setZero(); 35 | 36 | EXPECT_DOUBLE_EQ(0, so2tan.angle()); 37 | } 38 | 39 | TEST(TEST_SO2, TEST_SO2_TANGENT_MAP_ZERO2) 40 | { 41 | double data(1); 42 | Eigen::Map so2tan(&data); 43 | so2tan = SO2Tangentd::Zero(); 44 | 45 | EXPECT_DOUBLE_EQ(0, so2tan.angle()); 46 | } 47 | 48 | //TEST(TEST_SO2, TEST_SO2_RANDOM) 49 | //{ 50 | // SO2d so2; 51 | 52 | // so2.setRandom(); 53 | 54 | // EXPECT_DOUBLE_EQ(0, so2.angle()); 55 | //} 56 | 57 | //TEST(TEST_SO2, TEST_SO2_RANDOM2) 58 | //{ 59 | // SO2d so2 = SO2d::Random(); 60 | 61 | // EXPECT_DOUBLE_EQ(0, so2.angle()); 62 | //} 63 | 64 | TEST(TEST_SO2, TEST_SO2_TANGENT_MAP_RETRACT) 65 | { 66 | double data(MANIF_PI); 67 | Eigen::Map so2tan(&data); 68 | 69 | EXPECT_DOUBLE_EQ(MANIF_PI, so2tan.angle()); 70 | 71 | auto so2_exp = so2tan.exp(); 72 | 73 | EXPECT_DOUBLE_EQ(std::cos(MANIF_PI), so2_exp.real()); 74 | EXPECT_DOUBLE_EQ(std::sin(MANIF_PI), so2_exp.imag()); 75 | EXPECT_DOUBLE_EQ(MANIF_PI, so2_exp.angle()); 76 | } 77 | 78 | /// with Jacs 79 | 80 | TEST(TEST_SO2, TEST_SO2_TANGENT_MAP_RETRACT_JAC) 81 | { 82 | double data(MANIF_PI); 83 | Eigen::Map so2tan(&data); 84 | 85 | EXPECT_DOUBLE_EQ(MANIF_PI, so2tan.angle()); 86 | 87 | SO2d::Jacobian J_ret; 88 | 89 | SO2d so2_exp = so2tan.exp(J_ret); 90 | 91 | EXPECT_DOUBLE_EQ(std::cos(MANIF_PI), so2_exp.real()); 92 | EXPECT_DOUBLE_EQ(std::sin(MANIF_PI), so2_exp.imag()); 93 | EXPECT_DOUBLE_EQ(MANIF_PI, so2_exp.angle()); 94 | 95 | /// @todo check this J 96 | EXPECT_EQ(1, J_ret.rows()); 97 | EXPECT_EQ(1, J_ret.cols()); 98 | EXPECT_DOUBLE_EQ(1, J_ret(0)); 99 | } 100 | 101 | MANIF_RUN_ALL_TEST; 102 | -------------------------------------------------------------------------------- /test/so3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SO3 tests 2 | 3 | manif_add_gtest(gtest_so3 gtest_so3.cpp) 4 | 5 | set(CXX_TEST_TARGETS 6 | 7 | ${CXX_TEST_TARGETS} 8 | 9 | # SO3 10 | gtest_so3 11 | 12 | PARENT_SCOPE 13 | ) 14 | -------------------------------------------------------------------------------- /test/test_func.h: -------------------------------------------------------------------------------- 1 | #ifndef _MANIF_MANIF_TEST_FUNCTIONS_H_ 2 | #define _MANIF_MANIF_TEST_FUNCTIONS_H_ 3 | 4 | #include "manif/impl/lie_group_base.h" 5 | 6 | namespace manif { 7 | 8 | template 9 | void copy_assign(LieGroupBase<_Derived>& state, 10 | const _Other& state_other) 11 | { 12 | state = state_other; 13 | } 14 | 15 | template 16 | void copy_assign_base(LieGroupBase<_Derived>& state, 17 | const LieGroupBase<_DerivedOther>& state_other) 18 | { 19 | state = state_other; 20 | } 21 | 22 | template 23 | void move_assign(LieGroupBase<_Derived>& state, 24 | _Other& state_other) 25 | { 26 | state = std::move(state_other); 27 | } 28 | 29 | template 30 | void move_assign_base(LieGroupBase<_Derived>& state, 31 | LieGroupBase<_DerivedOther>& state_other) 32 | { 33 | state = std::move(state_other); 34 | } 35 | 36 | } // namespace manif 37 | 38 | #endif // _MANIF_MANIF_TEST_FUNCTIONS_H_ 39 | --------------------------------------------------------------------------------