├── .github └── workflows │ └── cmake-build-and-test.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include ├── groups │ ├── Gal3.hpp │ ├── In.hpp │ ├── SDB.hpp │ ├── SEn3.hpp │ ├── SO3.hpp │ ├── SOT3.hpp │ └── TG.hpp └── utils │ └── tools.hpp ├── resources └── lie-plusplus-logo.png └── tests ├── test_common.hpp ├── test_groups.hpp └── tests.cpp /.github/workflows/cmake-build-and-test.yml: -------------------------------------------------------------------------------- 1 | name: Build and test 2 | 3 | on: 4 | push: 5 | branches: [ "main" ] 6 | pull_request: 7 | branches: [ "main" ] 8 | 9 | env: 10 | BUILD_TYPE: Release 11 | 12 | jobs: 13 | build: 14 | runs-on: ubuntu-latest 15 | 16 | steps: 17 | - uses: actions/checkout@v4 18 | 19 | - name: Configure CMake 20 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DLIEPLUSPLUS_TESTS=ON 21 | 22 | - name: Build 23 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} --target lieplusplus_tests 24 | 25 | - name: Test 26 | run: ${{github.workspace}}/build/lieplusplus_tests 27 | 28 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # VSCode 2 | .vscode 3 | .vscode/* 4 | !.vscode/settings.json 5 | !.vscode/tasks.json 6 | !.vscode/launch.json 7 | !.vscode/extensions.json 8 | !.vscode/*.code-snippets 9 | 10 | # Local History for Visual Studio Code 11 | .history/ 12 | 13 | # Built Visual Studio Code Extensions 14 | *.vsix 15 | 16 | # Build 17 | **/build/* 18 | 19 | # Prerequisites 20 | *.d 21 | 22 | # Compiled Object files 23 | *.slo 24 | *.lo 25 | *.o 26 | *.obj 27 | 28 | # Precompiled Headers 29 | *.gch 30 | *.pch 31 | 32 | # Compiled Dynamic libraries 33 | *.so 34 | *.dylib 35 | *.dll 36 | 37 | # Fortran module files 38 | *.mod 39 | *.smod 40 | 41 | # Compiled Static libraries 42 | *.lai 43 | *.la 44 | *.a 45 | *.lib 46 | 47 | # Executables 48 | *.exe 49 | *.out 50 | *.app 51 | 52 | # Format 53 | .clang-format 54 | 55 | # Linux 56 | *~ 57 | 58 | # temporary files which can be created if a process still has a handle open of a deleted file 59 | .fuse_hidden* 60 | 61 | # KDE directory preferences 62 | .directory 63 | 64 | # Linux trash folder which might appear on any partition or disk 65 | .Trash-* 66 | 67 | # .nfs files are created when an open file is removed but is still being accessed 68 | .nfs* 69 | 70 | # Qt Creator settings 71 | *.creator 72 | 73 | # User project settings 74 | *.creator.user* 75 | 76 | # Qt Creator backups 77 | *.autosave 78 | 79 | # Matlab 80 | **/Matlab 81 | 82 | # Pycharm 83 | **/.idea 84 | 85 | # Codechecker 86 | .codechecker 87 | 88 | # Docs 89 | **/docs/* 90 | 91 | # Other 92 | CMakeFiles 93 | TODO 94 | _deps 95 | **/*.csv 96 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.13) 2 | 3 | project( 4 | LiePlusPlus 5 | VERSION 1.0 6 | DESCRIPTION "Lie++ A header-only Eigen-based C++ library for Lie group operations" 7 | LANGUAGES CXX 8 | ) 9 | 10 | # Set build type, Release by default 11 | if (NOT CMAKE_BUILD_TYPE) 12 | set(CMAKE_BUILD_TYPE "Release") 13 | endif() 14 | 15 | # Set options 16 | option(LIEPLUSPLUS_TESTS "Build Lie++ tests" OFF) 17 | option(ENABLE_ADDRESS_SANITIZER "Enable address sanitizer" OFF) 18 | option(ENABLE_UNDEFINED_SANITIZER "Enable undefined behavior sanitizer" ON) 19 | 20 | ## Include and set up external libraries 21 | include(FetchContent) 22 | 23 | # Googletest 24 | if(LIEPLUSPLUS_TESTS) 25 | FetchContent_Declare( 26 | googletest 27 | GIT_REPOSITORY https://github.com/google/googletest.git 28 | GIT_TAG release-1.12.1 29 | GIT_SHALLOW TRUE 30 | GIT_PROGRESS TRUE 31 | ) 32 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) 33 | list(APPEND external googletest) 34 | list(APPEND include_dirs ${googletest_INCLUDE_DIR}) 35 | list(APPEND libs ${googletest_LIBRARIES}) 36 | endif() 37 | 38 | # Eigen 39 | FetchContent_Declare( 40 | Eigen 41 | GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git 42 | GIT_TAG 3.4.0 43 | GIT_SHALLOW TRUE 44 | GIT_PROGRESS TRUE 45 | ) 46 | set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) 47 | set(EIGEN_BUILD_DOC OFF) 48 | set(EIGEN_BUILD_PKGCONFIG OFF) 49 | set(EIGEN_TEST_CXX11 ON) 50 | set(EIGEN_HAS_CXX11_MATH ON) 51 | list(APPEND external Eigen) 52 | list(APPEND libs Eigen3::Eigen) 53 | 54 | FetchContent_MakeAvailable(${external}) 55 | 56 | include(CheckCXXCompilerFlag) 57 | 58 | # Set compiler flags 59 | set(RELEASE_FLAGS "-DNDEBUG" "-O3" "-fsee" "-fomit-frame-pointer" "-fno-signed-zeros" "-fno-math-errno" "-funroll-loops" "-fno-unsafe-math-optimizations" "-flto" "-march=native") 60 | set(DEBUG_FLAGS "-O0" "-g3" "-Wall" "-Wextra" "-Werror" "-Wuninitialized" "-Wmaybe-uninitialized" "-pedantic" "-fno-omit-frame-pointer") 61 | 62 | CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17) 63 | if(COMPILER_SUPPORTS_CXX17) 64 | set(CMAKE_CXX_STANDARD 17) 65 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 66 | else() 67 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no c++17 support. Please use a different C++ compiler.") 68 | endif() 69 | if (CMAKE_BUILD_TYPE STREQUAL "Debug") 70 | foreach(FLAG ${DEBUG_FLAGS}) 71 | string(REPLACE "-" "" FLAG_NAME ${FLAG}) 72 | string(TOUPPER ${FLAG_NAME} FLAG_VAR_NAME) 73 | CHECK_CXX_COMPILER_FLAG("${FLAG}" COMPILER_SUPPORTS_${FLAG_VAR_NAME}) 74 | if (COMPILER_SUPPORTS_${FLAG_VAR_NAME}) 75 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${FLAG}") 76 | else() 77 | message("${FLAG} was requested but is not supported.") 78 | endif() 79 | endforeach() 80 | if (ENABLE_ADDRESS_SANITIZER) 81 | CHECK_CXX_COMPILER_FLAG("-fsanitize=address" COMPILER_SUPPORTS_ADDRESS_SANITIZER) 82 | if (COMPILER_SUPPORTS_ADDRESS_SANITIZER) 83 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=address -fsanitize-recover=address") 84 | else() 85 | message("-fsanitize=address -fsanitize-recover=address were requested but is not supported.") 86 | endif() 87 | elseif (ENABLE_UNDEFINED_SANITIZER) 88 | CHECK_CXX_COMPILER_FLAG("-fsanitize=undefined" COMPILER_SUPPORTS_UNDEFINED_SANITIZER) 89 | if (COMPILER_SUPPORTS_UNDEFINED_SANITIZER) 90 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=undefined") 91 | else() 92 | message("-fsanitize=undefined was requested but is not supported.") 93 | endif() 94 | endif() 95 | else() 96 | foreach(FLAG ${RELEASE_FLAGS}) 97 | string(REPLACE "-" "" FLAG_NAME ${FLAG}) 98 | string(TOUPPER ${FLAG_NAME} FLAG_VAR_NAME) 99 | CHECK_CXX_COMPILER_FLAG("${FLAG}" COMPILER_SUPPORTS_${FLAG_VAR_NAME}) 100 | if (COMPILER_SUPPORTS_${FLAG_VAR_NAME}) 101 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${FLAG}") 102 | else() 103 | message("${FLAG} was requested but is not supported.") 104 | endif() 105 | endforeach() 106 | endif() 107 | 108 | message(STATUS "COMPILER: " ${CMAKE_CXX_COMPILER}) 109 | message(STATUS "CMAKE_BUILD_TYPE: " ${CMAKE_BUILD_TYPE}) 110 | 111 | ## Define includes 112 | list(APPEND include_dirs ${CMAKE_CURRENT_SOURCE_DIR}/include) 113 | include_directories(${include_dirs}) 114 | 115 | # Add the library 116 | add_library(LiePlusPlus INTERFACE) 117 | target_include_directories(LiePlusPlus INTERFACE ${include_dirs}) 118 | 119 | if (LIEPLUSPLUS_TESTS) 120 | message(STATUS "Building Lie++ tests") 121 | enable_testing() 122 | add_executable(lieplusplus_tests tests/tests.cpp) 123 | target_link_libraries(lieplusplus_tests gtest_main LiePlusPlus ${libs}) 124 | include(GoogleTest) 125 | gtest_discover_tests(lieplusplus_tests) 126 | endif() -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2023 University of Klagenfurt - Control of Networked Systems (CNS) 2 | 3 | BSD-2 with additional conditions 4 | 5 | The Software is provided to you by the Licensor under the License, as defined 6 | below, subject to the following condition. 7 | 8 | Without limiting other conditions in the License, the grant of rights under the 9 | License will not include, and the License does not grant to you, the right to 10 | Sell the Software. 11 | 12 | For purposes of the foregoing, “Sell” means practicing any or all of the rights 13 | granted to you under the License to provide to third parties, for a fee or other 14 | consideration (including without limitation fees for hosting or consulting/ 15 | support services related to the Software), a product or service whose value 16 | derives from, or exploits the functionality of the Software in any way. Any 17 | license notice or attribution required by the License must also include this 18 | License Condition notice. 19 | 20 | IF THE SOFTWARE IS USED IN AN ACADEMIC SETTING, THE AUTHORS OF THE SOFTWARE MUST 21 | BE CITED. The proper citation format is listed in the README.md file. 22 | 23 | No express or implied license in any of the Licensor's patents is granted nor 24 | shall any of the provisions of the License be construed in such way. Any license 25 | notice or attribution required by the License must also include this notice. 26 | 27 | Software: Lie++ 28 | License: BSD-2-Clause 29 | Licensor: University of Klagenfurt - Control of Networked Systems (CNS) 30 | 31 | 32 | BSD-2-Clause-License: 33 | 34 | Redistribution and use in source and binary forms, with or without modification, 35 | are permitted provided that the following conditions are met: 36 | 37 | 1. Redistributions of source code must retain the above copyright notice, this 38 | list of conditions and the following disclaimer. 39 | 40 | 2. Redistributions in binary form must reproduce the above copyright notice, 41 | this list of conditions and the following disclaimer in the documentation and/or 42 | other materials provided with the distribution. 43 | 44 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 45 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 46 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 47 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 48 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 49 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 50 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 51 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 52 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 53 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 54 | 55 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Lie++ 2 | 3 | [![License](https://img.shields.io/badge/License-AAUCNS-336B81.svg)](./LICENSE) 4 | ![GitHub Actions Workflow Status](https://img.shields.io/github/actions/workflow/status/aau-cns/Lie-plusplus/cmake-build-and-test.yml?label=Test) 5 | 6 | 7 | ![Lie-plusplus logo](./resources/lie-plusplus-logo.png) 8 | 9 | Maintainer: [Alessandro Fornasier](mailto:alessandro.fornasier@aau.at) 10 | 11 | - [Description](#description) 12 | - [Installation](#installation) 13 | - [Usage](#usage) 14 | * [Base groups usage](#base-groups-usage) 15 | * [Composition of groups](#composition-of-groups) 16 | - [Credit](#credit) 17 | * [License](#license) 18 | * [Usage for academic purposes](#usage-for-academic-purposes) 19 | 20 | ## Description 21 | 22 | Lie++ is a header-only C++ library based on Eigen for Lie groups. It provides a set of classes and functions for working with common Lie groups encountered in robotics application. 23 | 24 | | Base groups | Applications | 25 | | ------------ | ----------------------------------| 26 | | SO(3) | Rotation | 27 | | SE(3) | Pose | 28 | | SE2(3) | Extended pose | 29 | | SEn(3) | SLAM | 30 | | Gal(3) | Discrete-time Inertial navigation | 31 | | SOT(3) | Rotation and scaling | 32 | | TG | Inertial navigation with IMU bias | 33 | | SDB | Inertial navigation with IMU bias | 34 | 35 | ## Installation 36 | 37 | This is a header-only C++ library meant to be used within other projects. Either copy the content of `include` folder within the external project's `include` folder or use cmake's `FetchContent_Declare` as follows 38 | ``` 39 | FetchContent_Declare( 40 | LiePlusPlus 41 | GIT_REPOSITORY 42 | GIT_TAG main 43 | GIT_SHALLOW TRUE 44 | GIT_PROGRESS TRUE 45 | ) 46 | list(APPEND external LiePlusPlus) 47 | list(APPEND include_dirs ${LIEPLUSPLUS_INCLUDE_DIR}) 48 | list(APPEND libs LiePlusPlus Eigen3::Eigen) 49 | ``` 50 | ## Usage 51 | 52 | Two mini examples on how to use the base groups of the library and how to compose them to create new groups. For usage of Lie++ in the context of filter design have a look at [MSCEqF](https://github.com/aau-cns/MSCEqF) 53 | 54 | ### Base groups usage 55 | 56 | ```cpp 57 | #include 58 | 59 | int main(int argc, char **argv) 60 | { 61 | using SO3d = group::SO3; 62 | using SE23d = group::SEn3; 63 | using Vector3d = Eigen::Matrix; 64 | using Vector9d = Eigen::Matrix; 65 | using Matrix9d = Eigen::Matrix; 66 | 67 | # Define random extended pose X via exponential map 68 | Vector9d x = Vector9d::Random(); 69 | SE23d X = SE23d::exp(x); 70 | 71 | # Define Extended pose Y with identity rotation, and given velocity and position 72 | Vector3d p = Vector3d(1, 2, 3); 73 | Vector3d v = Vector3d(0.1, 0.1, 0.3); 74 | SO3d R = SO3d(); 75 | SE23d Y = SE23d(R, {v, p}); 76 | 77 | # Get extended pose composition Z = XY 78 | SE23d Z = X*Y; 79 | 80 | # print Z as matrix 81 | std::cout << Z.asMatrix() << std::endl; 82 | 83 | # print Rotational component of Z as quaternion, position and velocity 84 | std::cout << Z.q() << '\n' << Z.p() << '\n'<< Z.v() << '\n' std::endl; 85 | 86 | # get Adjoint matrix of SE23 87 | Matrix9d AdZ = Z.Adjoint(); 88 | } 89 | ``` 90 | 91 | ### Composition of groups 92 | 93 | ```cpp 94 | #include 95 | 96 | /** 97 | * @brief Composition of SO(3) and SE(3) via direct product 98 | */ 99 | template 100 | class ComposedGroup 101 | { 102 | public: 103 | using SO3 = group::SO3; 104 | using SE3 = group::SEn3; 105 | using Vector3 = Eigen::Matrix; 106 | using Vector6 = Eigen::Matrix; 107 | using Vector9 = Eigen::Matrix; 108 | 109 | ComposedGroup() : C_(), P_() {}; 110 | 111 | ComposedGroup(const SO3& C, const SE3& P) : C_(C), P_(P) {}; 112 | 113 | // other constructors ... 114 | 115 | [[nodiscard]] static const ComposedGroup exp(const Vector9& u) 116 | { 117 | return ComposedGroup(SO3::exp(u.template block<3, 1>(0, 0)), SE3::exp(u.template block<6, 1>(3, 0))); 118 | } 119 | 120 | [[nodiscard]] static const Vector9 log(const ComposedGroup& X) 121 | { 122 | Vector6 u = Vector9::Zero(); 123 | u.template block<3, 1>(0, 0) = SO3::log(X.C_); 124 | u.template block<6, 1>(3, 0) = SE3::log(X.P_); 125 | return u; 126 | } 127 | 128 | [[nodiscard]] const ComposedGroup operator*(const ComposedGroup& other) const 129 | { 130 | return ComposedGroup(C_ * other.C_, P_ * other.P_); 131 | } 132 | 133 | // other methods ... 134 | 135 | private: 136 | SO3 C_; 137 | SE3 P_; 138 | 139 | } 140 | ``` 141 | 142 | ## Credit 143 | This code was written within the [Control of Networked System (CNS)](https://www.aau.at/en/smart-systems-technologies/control-of-networked-systems/), University of Klagenfurt. 144 | 145 | ### License 146 | This software is made available to the public to use (_source-available_), licensed under the terms of the BSD-2-Clause-License with no commercial use allowed, the full terms of which are made available in the `LICENSE` file. No license in patents is granted. 147 | 148 | ### Usage for academic purposes 149 | If you use this software in an academic research setting, please cite the corresponding paper. 150 | 151 | ```latex 152 | @article{fornasier2023equivariant, 153 | title={Equivariant Symmetries for Inertial Navigation Systems}, 154 | author={Fornasier, Alessandro and Ge, Yixiao and van Goor, Pieter and Mahony, Robert and Weiss, Stephan}, 155 | journal={arXiv preprint arXiv:2309.03765}, 156 | year={2023} 157 | } 158 | 159 | @article{fornasier2023msceqf, 160 | title={MSCEqF: A Multi State Constraint Equivariant Filter for Vision-aided Inertial Navigation}, 161 | author={Fornasier, Alessandro and van Goor, Pieter and Allak, Eren and Mahony, Robert and Weiss, Stephan}, 162 | journal={arXiv preprint arXiv:2311.11649}, 163 | year={2023} 164 | } 165 | ``` 166 | 167 | -------------------------------------------------------------------------------- /include/groups/Gal3.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Giulio Delama. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // 4 | // All rights reserved. 5 | // 6 | // This software is licensed under the terms of the BSD-2-Clause-License with 7 | // no commercial use allowed, the full terms of which are made available 8 | // in the LICENSE file. No license in patents is granted. 9 | // 10 | // You can contact the authors at , 11 | // . 12 | 13 | #ifndef GAL3_HPP 14 | #define GAL3_HPP 15 | 16 | #include 17 | 18 | #include "SO3.hpp" 19 | 20 | namespace group 21 | { 22 | /** 23 | * @brief the Galilean group Gal(3). This is the Lie group of 3D rotations, 24 | * translations in space and time, and transformations between frames of reference 25 | * that differ only by constant relative motion. 26 | * 27 | * @tparam FPType. Floating point type (float, double, long double) 28 | * 29 | * @note Galilei invariant theories [https://arxiv.org/abs/math-ph/0604002] 30 | * @note Constructive Equivariant Observer Design for Inertial Velocity-Aided 31 | * Attitude [https://arxiv.org/pdf/2209.03564.pdf] 32 | * @note Equivariant IMU Preintegration with Biases: a Galilean Group Approach [preprint: 33 | * https://arxiv.org/abs/2411.05548] 34 | */ 35 | template 36 | class Gal3 37 | { 38 | public: 39 | using Scalar = FPType; //!< The underlying scalar type 40 | using SO3Type = SO3; //!< The underlying SO3 type 41 | using VectorType = Eigen::Matrix; //!< R10 Vectorspace element type (isomorphic to Lie Algebra gal3) 42 | using MatrixType = Eigen::Matrix; //!< Lie Algebra / Lie Group matrix type 43 | using TMatrixType = Eigen::Matrix; //!< Transformation matrix type 44 | using IsometriesType = std::array; //!< Vector of translations (Isometries) 45 | 46 | /** 47 | * @brief Construct an identity Gal3 object 48 | */ 49 | Gal3() : C_(SO3Type()), t_(), s_(0.0) { t_.fill(SO3Type::VectorType::Zero()); } 50 | 51 | /** 52 | * @brief Construct a Gal3 object from a given normalized quaternion, an array of vectors, and a scalar factor. 53 | * 54 | * @param q Quaternion 55 | * @param t Array of R3 vectors 56 | * @param s Scalar factor 57 | */ 58 | Gal3(const typename SO3Type::QuaternionType& q, const IsometriesType& t, const Scalar& s) : C_(q), t_(t), s_(s) {} 59 | 60 | /** 61 | * @brief Construct a Gal3 object from a given rotation matrix, an array of vectors, and a scalar factor. 62 | * 63 | * @param R Rotation matrix 64 | * @param t Array of R3 vectors 65 | * @param s Scalar factor 66 | */ 67 | Gal3(const typename SO3Type::MatrixType& R, const IsometriesType& t, const Scalar& s) : C_(R), t_(t), s_(s) {} 68 | 69 | /** 70 | * @brief Construct a Gal3 object from a given SO3 object, an array of vectors, and a scalar factor. 71 | * 72 | * @param C SO3 group element 73 | * @param t Array of R3 vectors 74 | * @param s Scalar factor 75 | */ 76 | Gal3(const SO3Type& C, const IsometriesType& t, const Scalar& s) : C_(C), t_(t), s_(s) {} 77 | 78 | /** 79 | * @brief Construct a Gal3 object from a given matrix 80 | * 81 | * @param X Gal3 group element in matrix form 82 | */ 83 | Gal3(const MatrixType& X) : C_(X.template block<3, 3>(0, 0)), t_(), s_(X(3, 4)) 84 | { 85 | t_[0] = X.template block<3, 1>(0, 3); 86 | t_[1] = X.template block<3, 1>(0, 4); 87 | } 88 | 89 | /** 90 | * @brief wedge operator, transform a vector in R10 to a matrix in gal3 91 | * 92 | * @param u R10 vector 93 | * 94 | * @return gal3 Lie algebra element in matrix form 95 | */ 96 | [[nodiscard]] static const MatrixType wedge(const VectorType& u) 97 | { 98 | MatrixType U = MatrixType::Zero(); 99 | U.template block<3, 3>(0, 0) = SO3Type::wedge(u.template block<3, 1>(0, 0)); 100 | U.template block<3, 1>(0, 3) = u.template block<3, 1>(3, 0); 101 | U.template block<3, 1>(0, 4) = u.template block<3, 1>(6, 0); 102 | U(3, 4) = u(9); 103 | return U; 104 | } 105 | 106 | /** 107 | * @brief Transform a matrix in gal3 to a vector in R10 108 | * 109 | * @param U Gal3 Lie algebra element in matrix form 110 | * 111 | * @return R10 vector 112 | */ 113 | [[nodiscard]] static const VectorType vee(const MatrixType& U) 114 | { 115 | VectorType u = VectorType::Zero(); 116 | u.template block<3, 1>(0, 0) = SO3Type::vee(U.template block<3, 3>(0, 0)); 117 | u.template block<3, 1>(3, 0) = U.template block<3, 1>(0, 3); 118 | u.template block<3, 1>(6, 0) = U.template block<3, 1>(0, 4); 119 | u(9) = U(3, 4); 120 | return u; 121 | } 122 | 123 | /** 124 | * @brief gal3 adjoint matrix 125 | * 126 | * @param u R10 vector 127 | * 128 | * @return Gal3 Lie algebra adjoint matrix 129 | */ 130 | [[nodiscard]] static const TMatrixType adjoint(const VectorType& u) 131 | { 132 | TMatrixType ad = TMatrixType::Zero(); 133 | typename SO3Type::MatrixType W = SO3Type::wedge(u.template block<3, 1>(0, 0)); 134 | ad.template block<3, 3>(0, 0) = W; 135 | ad.template block<3, 3>(3, 0) = SO3Type::wedge(u.template block<3, 1>(3, 0)); 136 | ad.template block<3, 3>(3, 3) = W; 137 | ad.template block<3, 3>(6, 0) = SO3Type::wedge(u.template block<3, 1>(6, 0)); 138 | ad.template block<3, 3>(6, 3) = -u(9) * SO3Type::MatrixType::Identity(); 139 | ad.template block<3, 3>(6, 6) = W; 140 | ad.template block<3, 1>(6, 9) = u.template block<3, 1>(3, 0); 141 | return ad; 142 | } 143 | 144 | /** 145 | * @brief Gal3 left Jacobian matrix 146 | * 147 | * @param u R10 vector 148 | * 149 | * @return Gal3 left Jacobian matrix 150 | */ 151 | [[nodiscard]] static const TMatrixType leftJacobian(const VectorType& u) 152 | { 153 | typename SO3Type::VectorType w = u.template block<3, 1>(0, 0); 154 | typename SO3Type::VectorType v = u.template block<3, 1>(3, 0); 155 | typename SO3Type::VectorType p = u.template block<3, 1>(6, 0); 156 | Scalar s = u(9); 157 | FPType ang = w.norm(); 158 | if (ang < eps_) 159 | { 160 | return TMatrixType::Identity() + 0.5 * adjoint(u); 161 | } 162 | typename SO3Type::MatrixType SO3JL = SO3Type::leftJacobian(w); 163 | TMatrixType J = TMatrixType::Identity(); 164 | J.template block<3, 3>(0, 0) = SO3JL; 165 | J.template block<3, 3>(3, 0) = Gal3leftJacobianQ1(w, v); 166 | J.template block<3, 3>(3, 3) = SO3JL; 167 | J.template block<3, 3>(6, 0) = Gal3leftJacobianQ1(w, p) - s * Gal3leftJacobianQ2(w, v); 168 | J.template block<3, 3>(6, 3) = -s * Gal3leftJacobianU1(w); 169 | J.template block<3, 3>(6, 6) = SO3JL; 170 | J.template block<3, 1>(6, 9) = SO3Type::Gamma2(w) * v; 171 | return J; 172 | } 173 | 174 | /** 175 | * @brief Gal3 inverse left Jacobian matrix 176 | * 177 | * @param u R10 vector 178 | * 179 | * @return Gal3 inverse left Jacobian matrix 180 | */ 181 | [[nodiscard]] static const TMatrixType invLeftJacobian(const VectorType& u) 182 | { 183 | typename SO3Type::VectorType w = u.template block<3, 1>(0, 0); 184 | typename SO3Type::VectorType v = u.template block<3, 1>(3, 0); 185 | typename SO3Type::VectorType p = u.template block<3, 1>(6, 0); 186 | Scalar s = u(9); 187 | FPType ang = w.norm(); 188 | if (ang < eps_) 189 | { 190 | return TMatrixType::Identity() - 0.5 * adjoint(u); 191 | } 192 | typename SO3Type::MatrixType invSO3JL = SO3Type::invLeftJacobian(w); 193 | typename SO3Type::MatrixType SO3Gamma2 = SO3Type::Gamma2(w); 194 | typename SO3Type::MatrixType U1 = Gal3leftJacobianU1(w); 195 | typename SO3Type::MatrixType Q1p = Gal3leftJacobianQ1(w, p); 196 | typename SO3Type::MatrixType Q1v = Gal3leftJacobianQ1(w, v); 197 | typename SO3Type::MatrixType Q2v = Gal3leftJacobianQ2(w, v); 198 | typename SO3Type::MatrixType JiQJiv = invSO3JL * Q1v * invSO3JL; 199 | TMatrixType J = TMatrixType::Identity(); 200 | J.template block<3, 3>(0, 0) = invSO3JL; 201 | J.template block<3, 3>(3, 0) = -JiQJiv; 202 | J.template block<3, 3>(3, 3) = invSO3JL; 203 | J.template block<3, 3>(6, 0) = -invSO3JL * (Q1p * invSO3JL - s * (Q2v * invSO3JL - U1 * JiQJiv)); 204 | J.template block<3, 3>(6, 3) = s * invSO3JL * U1 * invSO3JL; 205 | J.template block<3, 3>(6, 6) = invSO3JL; 206 | J.template block<3, 1>(6, 9) = -invSO3JL * SO3Gamma2 * v; 207 | return J; 208 | } 209 | 210 | /** 211 | * @brief Gal3 right Jacobian matrix 212 | * 213 | * @param u R10 vector 214 | * 215 | * @return Gal3 right Jacobian matrix 216 | */ 217 | [[nodiscard]] static const TMatrixType rightJacobian(const VectorType& u) { return leftJacobian(-u); } 218 | 219 | /** 220 | * @brief Gal3 inverse right Jacobian matrix 221 | * 222 | * @param u R10 vector 223 | * 224 | * @return Gal3 inverse right Jacobian matrix 225 | */ 226 | [[nodiscard]] static const TMatrixType invRightJacobian(const VectorType& u) { return invLeftJacobian(-u); } 227 | 228 | /** 229 | * @brief The exponential map for Gal3. 230 | * Returns a Gal3 object given a vector u in R10 (equivalent to exp(wedge(u))) 231 | * 232 | * @param u R10 vector 233 | * 234 | * @return Gal3 group element 235 | */ 236 | [[nodiscard]] static const Gal3 exp(const VectorType& u) 237 | { 238 | typename SO3Type::VectorType w = u.template block<3, 1>(0, 0); 239 | typename SO3Type::VectorType v = u.template block<3, 1>(3, 0); 240 | typename SO3Type::VectorType p = u.template block<3, 1>(6, 0); 241 | Scalar s = u(9); 242 | typename SO3Type::MatrixType SO3JL = SO3Type::leftJacobian(w); 243 | IsometriesType t; 244 | t[0] = SO3JL * v; 245 | t[1] = SO3JL * p + s * SO3Type::Gamma2(w) * v; 246 | return Gal3(SO3Type::exp(w), t, s); 247 | } 248 | 249 | /** 250 | * @brief The logarithmic map for Gal3. 251 | * Return a vector given a Gal3 object (equivalent to vee(log(X))) 252 | * 253 | * @param X Gal3 group element 254 | * 255 | * @return R10 vector 256 | */ 257 | [[nodiscard]] static const VectorType log(const Gal3& X) 258 | { 259 | VectorType u = VectorType::Zero(); 260 | u.template block<3, 1>(0, 0) = SO3Type::log(X.C_); 261 | typename SO3Type::MatrixType invSO3JL = SO3Type::invLeftJacobian(u.template block<3, 1>(0, 0)); 262 | u.template block<3, 1>(3, 0) = invSO3JL * X.t_[0]; 263 | u.template block<3, 1>(6, 0) = 264 | invSO3JL * (X.t_[1] - X.s_ * SO3Type::Gamma2(u.template block<3, 1>(0, 0)) * u.template block<3, 1>(3, 0)); 265 | u(9) = X.s_; 266 | 267 | return u; 268 | } 269 | 270 | /** 271 | * @brief Operator * overloading. 272 | * Implements the Gal3 composition this * other 273 | * 274 | * @param other Gal3 group element 275 | * 276 | * @return Gal3 group element 277 | * 278 | * @note usage: z = x * y 279 | */ 280 | [[nodiscard]] const Gal3 operator*(const Gal3& other) const 281 | { 282 | IsometriesType t; 283 | t[0] = C_.R() * other.t_[0] + t_[0]; 284 | t[1] = C_.R() * other.t_[1] + t_[0] * other.s_ + t_[1]; 285 | return Gal3(C_ * other.C_, t, s_ + other.s_); 286 | } 287 | 288 | /** 289 | * @brief Operator * overloading. 290 | * Implements the Gal3 composition this * other with a Gal3 group or alegra element in matrix form 291 | * 292 | * @param other Gal3 group or algebra element in matrix form 293 | * 294 | * @return Gal3 group or algebra element in matrix form 295 | * 296 | * @note usage: z = x * y 297 | */ 298 | [[nodiscard]] const MatrixType operator*(const MatrixType& other) const 299 | { 300 | bool is_algebra = ((other(3, 3) == 0) && (other(4, 4) == 0)); 301 | bool is_group = ((other(3, 3) == 1) && (other(4, 4) == 1)); 302 | 303 | if (!(is_algebra || is_group)) 304 | { 305 | throw std::runtime_error( 306 | "Gal3: operator* is defined only for composition with matrix form of Gal3 group or algebra elements"); 307 | } 308 | 309 | MatrixType res = other; 310 | res.template block<3, 3>(0, 0) = C_.R() * other.template block<3, 3>(0, 0); 311 | if (is_algebra) 312 | { 313 | res.template block<3, 1>(0, 3) = C_.R() * other.template block<3, 1>(0, 3); 314 | res.template block<3, 1>(0, 4) = C_.R() * other.template block<3, 1>(0, 4) + t_[0] * other(3, 4); 315 | } 316 | else 317 | { 318 | res.template block<3, 1>(0, 3) = C_.R() * other.template block<3, 1>(0, 3) + t_[0]; 319 | res.template block<3, 1>(0, 4) = C_.R() * other.template block<3, 1>(0, 4) + t_[0] * other(3, 4) + t_[1]; 320 | res(3, 4) += s_; 321 | } 322 | return res; 323 | } 324 | 325 | /** 326 | * @brief Implements the Gal3 composition this = this * other 327 | * 328 | * @param other Gal3 group element 329 | * 330 | * @return Gal3 group element 331 | */ 332 | const Gal3& multiplyRight(const Gal3& other) 333 | { 334 | t_[1] = (C_.R() * other.t_[1] + t_[0] * other.s_ + t_[1]).eval(); 335 | t_[0] = (C_.R() * other.t_[0] + t_[0]).eval(); 336 | s_ += other.s_; 337 | C_.multiplyRight(other.C_); 338 | return *this; 339 | } 340 | 341 | /** 342 | * @brief Implements the Gal3 composition this = other * this 343 | * 344 | * @param other Gal3 group element 345 | * 346 | * @return Gal3 group element 347 | */ 348 | const Gal3& multiplyLeft(const Gal3& other) 349 | { 350 | t_[1] = (other.C_.R() * t_[1] + other.t_[0] * s_ + other.t_[1]).eval(); 351 | t_[0] = (other.C_.R() * t_[0] + other.t_[0]).eval(); 352 | s_ += other.s_; 353 | C_.multiplyLeft(other.C_); 354 | return *this; 355 | } 356 | 357 | /** 358 | * @brief Get a constant copy of the inverse of the Gal3 object 359 | * 360 | * @return Gal3 group element 361 | */ 362 | [[nodiscard]] const Gal3 inv() const 363 | { 364 | IsometriesType t; 365 | t[0] = -C_.R().transpose() * t_[0]; 366 | t[1] = -C_.R().transpose() * (t_[1] - s_ * t_[0]); 367 | return Gal3(C_.R().transpose(), t, -s_); 368 | } 369 | 370 | /** 371 | * @brief Get a constant reference to the SE23 rotation matrix 372 | * 373 | * @return Rotation matrix 374 | */ 375 | [[nodiscard]] const typename SO3Type::MatrixType& R() const { return C_.R(); } 376 | 377 | /** 378 | * @brief Get a constant reference to the SE23 normalized quaternion 379 | * 380 | * @return Quaternion 381 | */ 382 | [[nodiscard]] const typename SO3Type::QuaternionType& q() const { return C_.q(); } 383 | 384 | /** 385 | * @brief Get a constant reference to the SE23 translation vectors (isometries) 386 | * 387 | * @return Array of R3 vectors 388 | */ 389 | [[nodiscard]] const IsometriesType& t() const { return t_; } 390 | 391 | /** 392 | * @brief Get a constant referece to the first isometry (velocity) of SE23 393 | * 394 | * @return R3 vector 395 | */ 396 | [[nodiscard]] const typename SO3Type::VectorType& v() const { return t_[0]; } 397 | 398 | /** 399 | * @brief Get a constant referece to the second isometry (position) of SEn3 with n > 1 400 | * 401 | * @return R3 vector 402 | */ 403 | [[nodiscard]] const typename SO3Type::VectorType& p() const { return t_[1]; } 404 | 405 | /** 406 | * @brief Get a constant reference to the scalar factor 407 | * 408 | * @return Scalar factor 409 | */ 410 | [[nodiscard]] const Scalar& s() const { return s_; } 411 | 412 | /** 413 | * @brief Get a constant copy of the Gal3 object as a matrix 414 | * 415 | * @return Gal3 group element in matrix form 416 | */ 417 | [[nodiscard]] const MatrixType asMatrix() const 418 | { 419 | MatrixType X = MatrixType::Identity(); 420 | X.template block<3, 3>(0, 0) = C_.R(); 421 | X.template block<3, 1>(0, 3) = t_[0]; 422 | X.template block<3, 1>(0, 4) = t_[1]; 423 | X(3, 4) = s_; 424 | return X; 425 | } 426 | 427 | /** 428 | * @brief Set Gal3 object value from given matrix 429 | * 430 | * @param X Gal3 group element in matrix form 431 | */ 432 | void fromMatrix(const MatrixType& X) 433 | { 434 | C_.fromR(X.template block<3, 3>(0, 0)); 435 | t_[0] = X.template block<3, 1>(0, 3); 436 | t_[1] = X.template block<3, 1>(0, 4); 437 | s_ = X(3, 4); 438 | } 439 | 440 | /** 441 | * @brief Gal3 Adjoint matrix 442 | * 443 | * @return Gal3 group Adjoint matrix 444 | */ 445 | [[nodiscard]] const TMatrixType Adjoint() const 446 | { 447 | TMatrixType Ad = TMatrixType::Identity(); 448 | Ad.template block<3, 3>(0, 0) = C_.R(); 449 | Ad.template block<3, 3>(3, 0) = SO3Type::wedge(t_[0]) * C_.R(); 450 | Ad.template block<3, 3>(3, 3) = C_.R(); 451 | Ad.template block<3, 3>(6, 0) = SO3Type::wedge(t_[1] - s_ * t_[0]) * C_.R(); 452 | Ad.template block<3, 3>(6, 3) = -s_ * C_.R(); 453 | Ad.template block<3, 3>(6, 6) = C_.R(); 454 | Ad.template block<3, 1>(6, 9) = t_[0]; 455 | return Ad; 456 | } 457 | 458 | /** 459 | * @brief Gal3 Inverse Adjoint matrix 460 | * 461 | * @return Gal3 group inverse Adjoint matrix 462 | */ 463 | [[nodiscard]] const TMatrixType invAdjoint() const 464 | { 465 | TMatrixType Ad = TMatrixType::Identity(); 466 | Ad.template block<3, 3>(0, 0) = C_.R().transpose(); 467 | Ad.template block<3, 3>(3, 0) = -C_.R().transpose() * SO3Type::wedge(t_[0]); 468 | Ad.template block<3, 3>(3, 3) = C_.R().transpose(); 469 | Ad.template block<3, 3>(6, 0) = -C_.R().transpose() * SO3Type::wedge(t_[1]); 470 | Ad.template block<3, 3>(6, 3) = C_.R().transpose() * s_; 471 | Ad.template block<3, 3>(6, 6) = C_.R().transpose(); 472 | Ad.template block<3, 1>(6, 9) = -C_.R().transpose() * t_[0]; 473 | return Ad; 474 | } 475 | 476 | protected: 477 | /** 478 | * @brief Gal3 left Jacobian Q1 matrix 479 | * 480 | * @param w R3 vector 481 | * @param v R3 vector 482 | * 483 | * @return Gal3 left Jacobian Q1 matrix 484 | */ 485 | [[nodiscard]] static const typename SO3Type::MatrixType Gal3leftJacobianQ1(const typename SO3Type::VectorType& w, 486 | const typename SO3Type::VectorType& v) 487 | { 488 | typename SO3Type::MatrixType p = SO3Type::wedge(w); 489 | typename SO3Type::MatrixType r = SO3Type::wedge(v); 490 | 491 | FPType ang = w.norm(); 492 | FPType s = sin(ang); 493 | FPType c = cos(ang); 494 | 495 | FPType ang_p2 = pow(ang, 2); 496 | FPType ang_p3 = ang_p2 * ang; 497 | FPType ang_p4 = ang_p3 * ang; 498 | FPType ang_p5 = ang_p4 * ang; 499 | 500 | FPType c1 = (ang - s) / ang_p3; 501 | FPType c2 = (0.5 * ang_p2 + c - 1.0) / ang_p4; 502 | FPType c3 = (ang * (1.0 + 0.5 * c) - 1.5 * s) / ang_p5; 503 | 504 | typename SO3Type::MatrixType m1 = p * r + r * p + p * r * p; 505 | typename SO3Type::MatrixType m2 = p * p * r + r * p * p - 3.0 * p * r * p; 506 | typename SO3Type::MatrixType m3 = p * r * p * p + p * p * r * p; 507 | 508 | return 0.5 * r + c1 * m1 + c2 * m2 + c3 * m3; 509 | } 510 | 511 | /** 512 | * @brief Gal3 left Jacobian Q2 matrix 513 | * 514 | * @param w R3 vector 515 | * @param v R3 vector 516 | * 517 | * @return Gal3 left Jacobian Q2 matrix 518 | */ 519 | [[nodiscard]] static const typename SO3Type::MatrixType Gal3leftJacobianQ2(const typename SO3Type::VectorType& w, 520 | const typename SO3Type::VectorType& v) 521 | { 522 | typename SO3Type::MatrixType p = SO3Type::wedge(w); 523 | typename SO3Type::MatrixType r = SO3Type::wedge(v); 524 | 525 | FPType ang = w.norm(); 526 | FPType s = sin(ang); 527 | FPType c = cos(ang); 528 | 529 | FPType ang_p2 = pow(ang, 2); 530 | FPType ang_p3 = ang_p2 * ang; 531 | FPType ang_p4 = ang_p3 * ang; 532 | FPType ang_p5 = ang_p4 * ang; 533 | FPType ang_p6 = ang_p5 * ang; 534 | FPType ang_p7 = ang_p6 * ang; 535 | 536 | FPType c0 = 1 / 6; 537 | FPType c1 = (0.5 * ang_p2 + c - 1.0) / ang_p4; 538 | FPType c2 = (c0 * ang_p3 - ang + s) / ang_p5; 539 | FPType c3 = -(2.0 * c + ang * s - 2.0) / ang_p4; 540 | FPType c4 = (c0 * ang_p3 + ang * c + ang - 2.0 * s) / ang_p5; 541 | FPType c5 = -(0.75 * ang * c + (0.25 * ang_p2 - 0.75) * s) / ang_p5; 542 | FPType c6 = (0.25 * ang * c + 0.5 * ang - 0.75 * s) / ang_p5; 543 | FPType c7 = ((0.25 * ang_p2 - 2.0) * c - 1.25 * ang * s + 2.0) / ang_p6; 544 | FPType c8 = (c0 * ang_p3 + 1.25 * ang * c + (0.25 * ang_p2 - 1.25) * s) / ang_p7; 545 | 546 | typename SO3Type::MatrixType m1 = r * p; 547 | typename SO3Type::MatrixType m2 = r * p * p; 548 | typename SO3Type::MatrixType m3 = p * r; 549 | typename SO3Type::MatrixType m4 = p * p * r; 550 | typename SO3Type::MatrixType m5 = p * r * p; 551 | typename SO3Type::MatrixType m6 = p * p * r * p; 552 | typename SO3Type::MatrixType m7 = p * r * p * p; 553 | typename SO3Type::MatrixType m8 = p * p * r * p * p; 554 | 555 | return c0 * r + c1 * m1 + c2 * m2 + c3 * m3 + c4 * m4 + c5 * m5 + c6 * m6 + c7 * m7 + c8 * m8; 556 | } 557 | 558 | /** 559 | * @brief Gal3 left Jacobian U1 matrix 560 | * 561 | * @param w R3 vector 562 | * 563 | * @return Gal3 left Jacobian U1 matrix 564 | */ 565 | [[nodiscard]] static const typename SO3Type::MatrixType Gal3leftJacobianU1(const typename SO3Type::VectorType& u) 566 | { 567 | FPType ang = u.norm(); 568 | if (ang < eps_) 569 | { 570 | return 0.5 * SO3Type::MatrixType::Identity() + 1 / 3 * SO3Type::wedge(u); 571 | } 572 | typename SO3Type::VectorType ax = u / ang; 573 | FPType ang_p2 = pow(ang, 2); 574 | FPType s = sin(ang); 575 | FPType c = cos(ang); 576 | FPType c1 = (ang * s + c - 1) / ang_p2; 577 | FPType c2 = (s - ang * c) / ang_p2; 578 | return c1 * SO3Type::MatrixType::Identity() + c2 * SO3Type::wedge(ax) + (0.5 - c1) * ax * ax.transpose(); 579 | } 580 | 581 | SO3Type C_; //!< The SO3 element of the symmetry group 582 | IsometriesType t_; //!< The translation vectors (isometries) of the Gal3 element 583 | Scalar s_; //!< Scalar factor of the Gal3 group 584 | 585 | static constexpr FPType eps_ = std::is_same_v ? 1.0e-6 : 1.0e-9; //!< Epsilon 586 | }; 587 | 588 | using Gal3d = Gal3; //!< The Gal3 group with double precision floating point 589 | using Gal3f = Gal3; //!< The Gal3 group with single precision floating point 590 | 591 | } // namespace group 592 | 593 | #endif // GAL3_HPP 594 | -------------------------------------------------------------------------------- /include/groups/In.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Pieter van Goor. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // System Theory and Robotics Lab, Australian Centre for Robotic 4 | // Vision, Australian National University. 5 | // 6 | // All rights reserved. 7 | // 8 | // This software is licensed under the terms of the BSD-2-Clause-License with 9 | // no commercial use allowed, the full terms of which are made available 10 | // in the LICENSE file. No license in patents is granted. 11 | // 12 | // You can contact the authors at 13 | 14 | #ifndef IN_HPP 15 | #define IN_HPP 16 | 17 | #include 18 | #include 19 | 20 | namespace group 21 | { 22 | /** 23 | * @brief The Intrinsic Group. This represent the group describing camera intrinsics transformation. 24 | * 25 | * @tparam FPType. Floating point type (float, double, long double) 26 | * 27 | * @note MSCEqF: A Multi State Constraint Equivariant Filter for Vision-aided Inertial Navigation 28 | * [https://arxiv.org/abs/2311.11649] 29 | */ 30 | template 31 | class In 32 | { 33 | public: 34 | using Scalar = FPType; //!< The underlying scalar type 35 | using MatrixType = Eigen::Matrix; //!< The underlying 3x3 group matrix type 36 | using TMatrixType = Eigen::Matrix; //!< The underlying 4x4 transformation matrix type 37 | using VectorType = Eigen::Matrix; //!< The underlying R4 vector type 38 | using Vector3Type = Eigen::Matrix; //!< The underlying R3 vector type 39 | 40 | /** 41 | * @brief Construct an identity In Group object 42 | */ 43 | In() : fx_(1.0), fy_(1.0), cx_(0.0), cy_(0.0){}; 44 | 45 | /** 46 | * @brief Construct a In Group object given the intrinsics 47 | * 48 | * @param fx Focal length in x-direction 49 | * @param fy Focal length in y-direction 50 | * @param cx Center in x-direction 51 | * @param cy Center in y-direction 52 | */ 53 | In(const FPType& fx, const FPType& fy, const FPType& cx, const FPType& cy) : fx_(fx), fy_(fy), cx_(cx), cy_(cy){}; 54 | 55 | /** 56 | * @brief Construct a In Group object given a matrix (camera intrinsic matrix) 57 | * 58 | * @param L Intrinsic matrix 59 | */ 60 | In(const MatrixType& L) : fx_(1.0), fy_(1.0), cx_(0.0), cy_(0.0) 61 | { 62 | fx_ = L(0, 0); 63 | fy_ = L(1, 1); 64 | cx_ = L(0, 2); 65 | cy_ = L(1, 2); 66 | }; 67 | 68 | /** 69 | * @brief Construct a In Group object given a vector 70 | * 71 | * @param L Vector of intrinsic parameters (fx, fy, cx, cy) 72 | */ 73 | In(const VectorType& l) : fx_(1.0), fy_(1.0), cx_(0.0), cy_(0.0) 74 | { 75 | fx_ = l(0); 76 | fy_ = l(1); 77 | cx_ = l(2); 78 | cy_ = l(3); 79 | }; 80 | 81 | /** 82 | * @brief wedge operator, transform a vector in R4 to a matrix in the Lie Algebra of In 83 | * 84 | * @param u R4 vector 85 | * 86 | * @return In Lie algebra element in matrix form 87 | */ 88 | [[nodiscard]] static const MatrixType wedge(const VectorType& u) 89 | { 90 | MatrixType U; 91 | U << u(0), 0.0, u(2), 0.0, u(1), u(3), 0.0, 0.0, 0.0; 92 | return (U); 93 | } 94 | 95 | /** 96 | * @brief Transform a matrix in the Lie Algebra of In to a vector in R4 97 | * 98 | * @param U In Lie algebra element in matrix form 99 | * 100 | * @return R4 vector 101 | */ 102 | [[nodiscard]] static const VectorType vee(const MatrixType& U) 103 | { 104 | VectorType u; 105 | u << U(0, 0), U(1, 1), U(0, 2), U(1, 2); 106 | return (u); 107 | } 108 | 109 | /** 110 | * @brief so3 adjoint matrix 111 | * 112 | * @param u R4 vector 113 | * 114 | * @return In Lie algebra adjoint matrix 115 | */ 116 | [[nodiscard]] static const TMatrixType adjoint(const VectorType& u) 117 | { 118 | TMatrixType ad; 119 | ad << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -u(2), 0.0, u(0), 0.0, 0.0, -u(3), 0.0, u(1); 120 | return ad; 121 | } 122 | 123 | /** 124 | * @brief The exponential map for the In Group. 125 | * Returns a In object given a vector u in R4 (equivalent to exp(wedge(u))) 126 | * 127 | * @param u Vector in R4 128 | * 129 | * @return In group element 130 | */ 131 | [[nodiscard]] static const In exp(const VectorType& u) 132 | { 133 | MatrixType L = wedge(u).exp(); 134 | return In(L); 135 | } 136 | 137 | /** 138 | * @brief The logarithmic map for the In Group. 139 | * Return a vector given a In object (equivalent to vee(log(X))) 140 | * 141 | * @param X In group element 142 | * 143 | * @return Vector in R4 144 | */ 145 | [[nodiscard]] static const VectorType log(const In& X) { return vee(X.K().log()); } 146 | 147 | /** 148 | * @brief Get a constant copy of the inverse of the In object 149 | * 150 | * @return In group element 151 | */ 152 | [[nodiscard]] const In inv() const 153 | { 154 | return In(FPType(1.0 / fx_), FPType(1.0 / fy_), FPType(-cx_ / fx_), FPType(-cy_ / fy_)); 155 | } 156 | 157 | /** 158 | * @brief Get the matrix representation of In 159 | * 160 | * @return In group element in matrix form 161 | */ 162 | [[nodiscard]] const MatrixType K() const 163 | { 164 | MatrixType L; 165 | L << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0; 166 | return L; 167 | } 168 | 169 | /** 170 | * @brief Get the vector representation of In 171 | * 172 | * @return In group element in vector form (fx, fy, cx, cy) 173 | */ 174 | [[nodiscard]] const VectorType k() const 175 | { 176 | VectorType k; 177 | k << fx_, fy_, cx_, cy_; 178 | return k; 179 | } 180 | 181 | /** 182 | * @brief Get the matrix representation of In 183 | * 184 | * @return In group element in matrix form 185 | */ 186 | [[nodiscard]] const MatrixType asMatrix() const { return K(); } 187 | 188 | /** 189 | * @brief In Adjoint matrix 190 | * 191 | * @return In group Adjoint matrix 192 | */ 193 | [[nodiscard]] const TMatrixType Adjoint() const 194 | { 195 | TMatrixType Ad; 196 | Ad << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -cx_, 0.0, fx_, 0.0, 0.0, -cy_, 0.0, fy_; 197 | return Ad; 198 | } 199 | 200 | /** 201 | * @brief In Inverse Adjoint matrix 202 | * 203 | * @return In group inverse Adjoint matrix 204 | */ 205 | [[nodiscard]] const TMatrixType invAdjoint() const 206 | { 207 | TMatrixType invAd; 208 | invAd << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, cx_ / fx_, 0.0, 1.0 / fx_, 0.0, 0.0, cy_ / fy_, 0.0, 1.0 / fy_; 209 | return invAd; 210 | } 211 | 212 | /** 213 | * @brief Operator * overloading. 214 | * Implements the In composition this * other 215 | * 216 | * @param other In group element 217 | * 218 | * @return In group element 219 | * 220 | * @note usage: z = x * y 221 | */ 222 | [[nodiscard]] const In operator*(const In& other) const 223 | { 224 | return In(fx_ * other.fx_, fy_ * other.fy_, fx_ * other.cx_ + cx_, fy_ * other.cy_ + cy_); 225 | } 226 | 227 | /** 228 | * @brief Operator * overloading. 229 | * Implements the In composition this * other with a In group or alegra element in matrix form 230 | * 231 | * @param other In group or algebra element in matrix form 232 | * 233 | * @return In group or algebra element in matrix form 234 | */ 235 | [[nodiscard]] const MatrixType operator*(const MatrixType& other) const { return K() * other; } 236 | 237 | /** 238 | * @brief Operator * overloading. 239 | * Implements the In action on a R3 vector 240 | * 241 | * @param other R3 vector 242 | * 243 | * @return R3 vector 244 | */ 245 | [[nodiscard]] const Vector3Type operator*(const Vector3Type& other) const 246 | { 247 | Vector3Type x; 248 | x << fx_ * other(0) + cx_ * other(2), fy_ * other(1) + cy_ * other(2), other(2); 249 | return x; 250 | } 251 | 252 | /** 253 | * @brief Implements the In composition this = this * other 254 | * 255 | * @param other In group element 256 | * 257 | * @return In group element 258 | */ 259 | const In& multiplyRight(const In& other) 260 | { 261 | cx_ = fx_ * other.cx_ + cx_; 262 | cy_ = fy_ * other.cy_ + cy_; 263 | fx_ *= other.fx_; 264 | fy_ *= other.fy_; 265 | return *this; 266 | } 267 | 268 | /** 269 | * @brief Implements the In composition this = other * this 270 | * 271 | * @param other In group element 272 | * 273 | * @return In group element 274 | */ 275 | const In& multiplyLeft(const In& other) 276 | { 277 | cx_ = other.fx_ * cx_ + other.cx_; 278 | cy_ = other.fy_ * cy_ + other.cy_; 279 | fx_ *= other.fx_; 280 | fy_ *= other.fy_; 281 | return *this; 282 | } 283 | 284 | protected: 285 | FPType fx_; //!< The x-direction focal length 286 | FPType fy_; //!< The y-direction focal length 287 | FPType cx_; //!< The x-direction center 288 | FPType cy_; //!< The y-direction center 289 | 290 | static constexpr FPType eps_ = std::is_same_v ? 1.0e-6 : 1.0e-9; //!< Epsilon 291 | }; 292 | 293 | using Inf = In; //!< The Intrinsic group with single precision floating point 294 | using Ind = In; //!< The Intrinsic group with double precision floating point 295 | 296 | } // namespace group 297 | 298 | #endif // IN_HPP -------------------------------------------------------------------------------- /include/groups/SDB.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Pieter van Goor. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // System Theory and Robotics Lab, Australian Centre for Robotic 4 | // Vision, Australian National University. 5 | // 6 | // All rights reserved. 7 | // 8 | // This software is licensed under the terms of the BSD-2-Clause-License with 9 | // no commercial use allowed, the full terms of which are made available 10 | // in the LICENSE file. No license in patents is granted. 11 | // 12 | // You can contact the authors at 13 | 14 | #ifndef SDB_HPP 15 | #define SDB_HPP 16 | 17 | #include "SEn3.hpp" 18 | 19 | namespace group 20 | { 21 | /** 22 | * @brief The Semi Direct Bias group. This represent the core components of the symmetry group for Inertial Navigation 23 | * Systems (INS), including the SE23 |x se3 semi-direct product acting on the extended pose and IMU biases 24 | * 25 | * @tparam FPType. Floating point type (float, double, long double) 26 | * 27 | * @note Equivariant Symmetries for Inertial Navigation Systems [https://arxiv.org/abs/2309.03765] 28 | * @note MSCEqF: A Multi State Constraint Equivariant Filter for Vision-aided Inertial Navigation 29 | * [https://arxiv.org/abs/2311.11649] 30 | */ 31 | template 32 | class SemiDirectBias 33 | { 34 | public: 35 | using Scalar = FPType; //!< The underlying scalar type 36 | using SE3Type = group::SEn3; //!< The underlying SE3 type 37 | using SE23Type = group::SEn3; //!< The underlying SE23 type 38 | using VectorType = Eigen::Matrix; //!< The underlying R15 vector type 39 | using Vector6Type = Eigen::Matrix; //!< The underlying R6 vector type 40 | 41 | /** 42 | * @brief Construct an identity Semi Direct Bias Group object 43 | */ 44 | SemiDirectBias() : D_(), delta_(Vector6Type::Zero()){}; 45 | 46 | /** 47 | * @brief Construct a Semi Direct Bias Group object from a given SE23 object, and a R6 vector 48 | * 49 | * @param D SE23 group element 50 | * @param delta R6 vector 51 | */ 52 | SemiDirectBias(const SE23Type& D, const Vector6Type& delta) : D_(D), delta_(delta){}; 53 | 54 | /** 55 | * @brief The exponential map for the Semi Direct Bias Group. 56 | * Returns a SemiDirectBias object given a vector u in R15 (equivalent to exp(wedge(u))) 57 | * 58 | * @param u R15 vector 59 | * 60 | * @return SemiDirectBias group element 61 | */ 62 | [[nodiscard]] static const SemiDirectBias exp(const VectorType& u) 63 | { 64 | return SemiDirectBias(SE23Type::exp(u.template block<9, 1>(0, 0)), 65 | SE3Type::leftJacobian(u.template block<6, 1>(0, 0)) * u.template block<6, 1>(9, 0)); 66 | } 67 | 68 | /** 69 | * @brief The logarithmic map for the Semi Direct Bias Group. 70 | * Return a vector given a SemiDirectBias object (equivalent to vee(log(X))) 71 | * 72 | * @param X SemiDirectBias group element 73 | * 74 | * @return R15 vector 75 | */ 76 | [[nodiscard]] static const VectorType log(const SemiDirectBias& X) 77 | { 78 | VectorType u = VectorType::Zero(); 79 | u.template block<9, 1>(0, 0) = SE23Type::log(X.D_); 80 | u.template block<6, 1>(9, 0) = (SE3Type::invLeftJacobian(u.template block<6, 1>(0, 0))) * X.delta_; 81 | return u; 82 | } 83 | 84 | /** 85 | * @brief Get a constant copy of B (the SE3 subgroup of SE23 composed by the rotational component (R) and the first 86 | * isometry (v)) 87 | * 88 | * @return SE3 group element 89 | */ 90 | [[nodiscard]] const SE3Type B() const { return SE3Type(D_.q(), {D_.v()}); } 91 | 92 | /** 93 | * @brief Get a constant copy of C (the SE3 subgroup of SE23 composed by the rotational component (R) and the 94 | * second isometry (p)) 95 | * 96 | * @return SE3 group element 97 | */ 98 | [[nodiscard]] const SE3Type C() const { return SE3Type(D_.q(), {D_.p()}); } 99 | 100 | /** 101 | * @brief Get a constant reference to D (the SE23 element) 102 | * 103 | * @return SE23 group element 104 | */ 105 | [[nodiscard]] const SE23Type& D() const { return D_; } 106 | 107 | /** 108 | * @brief Get a constant reference to delta (the R6 element) 109 | * 110 | * @return R6 vector 111 | */ 112 | [[nodiscard]] const Vector6Type& delta() const { return delta_; } 113 | 114 | /** 115 | * @brief Get a constant copy of the inverse of the SemiDirectBias object 116 | * 117 | * @return SemiDirectBias group element 118 | */ 119 | [[nodiscard]] const SemiDirectBias inv() const { return SemiDirectBias(D_.inv(), -B().invAdjoint() * delta_); } 120 | 121 | /** 122 | * @brief Operator * overloading. 123 | * Implements the SemiDirectBias composition this * other 124 | * 125 | * @param other SemiDirectBias group element 126 | * 127 | * @return SemiDirectBias group element 128 | * 129 | * @note usage: z = x * y 130 | */ 131 | [[nodiscard]] const SemiDirectBias operator*(const SemiDirectBias& other) const 132 | { 133 | return SemiDirectBias(D_ * other.D_, delta_ + B().Adjoint() * other.delta_); 134 | } 135 | 136 | /** 137 | * @brief Implements the SemiDirectBias composition this = this * other 138 | * 139 | * @param other SemiDirectBias group element 140 | * 141 | * @return SemiDirectBias group element 142 | */ 143 | const SemiDirectBias& multiplyRight(const SemiDirectBias& other) 144 | { 145 | delta_ = (delta_ + B().Adjoint() * other.delta_).eval(); 146 | D_.multiplyRight(other.D_); // D_ * other.D_ 147 | return *this; 148 | } 149 | 150 | /** 151 | * @brief Implements the SemiDirectBias composition this = other * this 152 | * 153 | * @param other SemiDirectBias group element 154 | * 155 | * @return SemiDirectBias group element 156 | */ 157 | const SemiDirectBias& multiplyLeft(const SemiDirectBias& other) 158 | { 159 | delta_ = (other.delta_ + other.B().Adjoint() * delta_).eval(); 160 | D_.multiplyLeft(other.D_); // other.D_ * th.D_ 161 | return *this; 162 | } 163 | 164 | private: 165 | SE23Type D_; //!< The SE23 element of the symmetry group ancting on the extended pose 166 | Vector6Type delta_; //!< The R6 element of the symmetry group acting on the IMU biases 167 | }; 168 | 169 | using SDBf = SemiDirectBias; //!< The Semi Direct Bias group with single precision floating point 170 | using SDBd = SemiDirectBias; //!< The Semi Direct Bias group with double precision floating point 171 | 172 | } // namespace group 173 | 174 | #endif // SDB_HPP -------------------------------------------------------------------------------- /include/groups/SEn3.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Pieter van Goor. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // System Theory and Robotics Lab, Australian Centre for Robotic 4 | // Vision, Australian national University, Australia. 5 | // 6 | // All rights reserved. 7 | // 8 | // This software is licensed under the terms of the BSD-2-Clause-License with 9 | // no commercial use allowed, the full terms of which are made available 10 | // in the LICENSE file. No license in patents is granted. 11 | // 12 | // You can contact the authors at , 13 | // . 14 | 15 | #ifndef SEn3_HPP 16 | #define SEn3_HPP 17 | 18 | #include 19 | 20 | #include "SO3.hpp" 21 | 22 | namespace group 23 | { 24 | /** 25 | * @brief the Special Eucldean group with n multiple isometries (SEn3) 26 | * 27 | * @tparam FPType. Floating point type (float, double, long double) 28 | * 29 | * @note Group Formulation for Consistent Non-Linear Estiamtion 30 | * @note State Estimation for Robotics [http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf] 31 | * @note The Geometry and Kinematics of the MatrixType Lie Group SEK(3) [https://arxiv.org/abs/2012.00950] 32 | */ 33 | template 34 | class SEn3 35 | { 36 | static_assert(n > 0, "SEn3: n needs to be grater than 0"); 37 | 38 | public: 39 | using Scalar = FPType; //!< The underlying scalar type 40 | using SO3Type = SO3; //!< The underlying SO3 type 41 | using VectorType = 42 | Eigen::Matrix; //!< R6 Vectorspace element type (isomorphic to Lie Algebra se3) 43 | using MatrixType = Eigen::Matrix; //!< Lie Algebra / Lie Group matrix type 44 | using TMatrixType = Eigen::Matrix; //!< Transformation matrix type 45 | using IsometriesType = std::array; //!< Vector of translations (Isometries) 46 | 47 | /** 48 | * @brief Construct an identity SEn3 object 49 | */ 50 | SEn3() : C_(SO3Type()), t_() { t_.fill(SO3Type::VectorType::Zero()); } 51 | 52 | /** 53 | * @brief Construct a SEn3 object from a given normalized quaternion, and an array of vectors. 54 | * 55 | * @param q Quaternion 56 | * @param t Array of R3 vectors 57 | */ 58 | SEn3(const typename SO3Type::QuaternionType& q, const IsometriesType& t) : C_(q), t_(t) {} 59 | 60 | /** 61 | * @brief Construct a SEn3 object from a given rotation matrix, and an array of vectors. 62 | * 63 | * @param R Rotation matrix 64 | * @param t Array of R3 vectors 65 | */ 66 | SEn3(const typename SO3Type::MatrixType& R, const IsometriesType& t) : C_(R), t_(t) {} 67 | 68 | /** 69 | * @brief Construct a SEn3 object from a given SO3 object, and an array of vectors. 70 | * 71 | * @param C SO3 group element 72 | * @param t Array of R3 vectors 73 | */ 74 | SEn3(const SO3Type& C, const IsometriesType& t) : C_(C), t_(t) {} 75 | 76 | /** 77 | * @brief Construct a SEn3 object from a given matrix 78 | * 79 | * @param T SEn3 group element in matrix form 80 | */ 81 | SEn3(const MatrixType& T) : C_(T.template block<3, 3>(0, 0)), t_() 82 | { 83 | for (int i = 0; i < n; ++i) 84 | { 85 | t_[i] = T.template block<3, 1>(0, 3 + i); 86 | } 87 | } 88 | 89 | /** 90 | * @brief wedge operator, transform a vector in R(3+3n) to a matrix in sen3 91 | * 92 | * @param u R(3+3n) vector 93 | * 94 | * @return SEn3 Lie algebra element in matrix form 95 | */ 96 | [[nodiscard]] static const MatrixType wedge(const VectorType& u) 97 | { 98 | MatrixType U = MatrixType::Zero(); 99 | U.template block<3, 3>(0, 0) = SO3Type::wedge(u.template block<3, 1>(0, 0)); 100 | for (int i = 0; i < n; ++i) 101 | { 102 | U.template block<3, 1>(0, 3 + i) = u.template block<3, 1>(3 + 3 * i, 0); 103 | } 104 | return U; 105 | } 106 | 107 | /** 108 | * @brief Transform a matrix in sen3 to a vector in R(3+3n) 109 | * 110 | * @param U SEn3 Lie algebra element in matrix form 111 | * 112 | * @return R(3+3n) vector 113 | */ 114 | [[nodiscard]] static const VectorType vee(const MatrixType& U) 115 | { 116 | VectorType u = VectorType::Zero(); 117 | u.template block<3, 1>(0, 0) = SO3Type::vee(U.template block<3, 3>(0, 0)); 118 | for (int i = 0; i < n; ++i) 119 | { 120 | u.template block<3, 1>(3 + 3 * i, 0) = U.template block<3, 1>(0, 3 + i); 121 | } 122 | return u; 123 | } 124 | 125 | /** 126 | * @brief sen3 adjoint matrix 127 | * 128 | * @param u R(3+3n) vector 129 | * 130 | * @return SEn3 Lie algebra adjoint matrix 131 | */ 132 | [[nodiscard]] static const TMatrixType adjoint(const VectorType& u) 133 | { 134 | TMatrixType ad = TMatrixType::Zero(); 135 | typename SO3Type::MatrixType W = SO3Type::wedge(u.template block<3, 1>(0, 0)); 136 | ad.template block<3, 3>(0, 0) = W; 137 | for (int i = 0; i < n; ++i) 138 | { 139 | ad.template block<3, 3>(3 + 3 * i, 3 + 3 * i) = W; 140 | ad.template block<3, 3>(3 + 3 * i, 0) = SO3Type::wedge(u.template block<3, 1>(3 + 3 * i, 0)); 141 | } 142 | return ad; 143 | } 144 | 145 | /** 146 | * @brief SEn3 left Jacobian matrix 147 | * 148 | * @param u R(3+3n) vector 149 | * 150 | * @return SEn3 left Jacobian matrix 151 | */ 152 | [[nodiscard]] static const TMatrixType leftJacobian(const VectorType& u) 153 | { 154 | typename SO3Type::VectorType w = u.template block<3, 1>(0, 0); 155 | FPType ang = w.norm(); 156 | if (ang < eps_) 157 | { 158 | return TMatrixType::Identity() + 0.5 * adjoint(u); 159 | } 160 | typename SO3Type::MatrixType SO3JL = SO3Type::leftJacobian(w); 161 | TMatrixType J = TMatrixType::Identity(); 162 | J.template block<3, 3>(0, 0) = SO3JL; 163 | for (int i = 0; i < n; ++i) 164 | { 165 | typename SO3Type::VectorType x = u.template block<3, 1>(3 + 3 * i, 0); 166 | J.template block<3, 3>(3 + 3 * i, 0) = SE3leftJacobianQ(w, x); 167 | J.template block<3, 3>(3 + 3 * i, 3 + 3 * i) = SO3JL; 168 | } 169 | return J; 170 | } 171 | 172 | /** 173 | * @brief SEn3 inverse left Jacobian matrix 174 | * 175 | * @param u R(3+3n) vector 176 | * 177 | * @return SEn3 inverse left Jacobian matrix 178 | */ 179 | [[nodiscard]] static const TMatrixType invLeftJacobian(const VectorType& u) 180 | { 181 | typename SO3Type::VectorType w = u.template block<3, 1>(0, 0); 182 | FPType ang = w.norm(); 183 | if (ang < eps_) 184 | { 185 | return TMatrixType::Identity() - 0.5 * adjoint(u); 186 | } 187 | typename SO3Type::MatrixType invSO3JL = SO3Type::invLeftJacobian(w); 188 | TMatrixType J = TMatrixType::Identity(); 189 | J.template block<3, 3>(0, 0) = invSO3JL; 190 | for (int i = 0; i < n; ++i) 191 | { 192 | typename SO3Type::VectorType x = u.template block<3, 1>(3 + 3 * i, 0); 193 | J.template block<3, 3>(3 + 3 * i, 0) = -invSO3JL * SE3leftJacobianQ(w, x) * invSO3JL; 194 | J.template block<3, 3>(3 + 3 * i, 3 + 3 * i) = invSO3JL; 195 | } 196 | return J; 197 | } 198 | 199 | /** 200 | * @brief SEn3 right Jacobian matrix 201 | * 202 | * @param u R(3+3n) vector 203 | * 204 | * @return SEn3 right Jacobian matrix 205 | */ 206 | [[nodiscard]] static const TMatrixType rightJacobian(const VectorType& u) { return leftJacobian(-u); } 207 | 208 | /** 209 | * @brief SEn3 inverse right Jacobian matrix 210 | * 211 | * @param u R(3+3n) vector 212 | * 213 | * @return SEn3 inverse right Jacobian matrix 214 | */ 215 | [[nodiscard]] static const TMatrixType invRightJacobian(const VectorType& u) { return invLeftJacobian(-u); } 216 | 217 | /** 218 | * @brief The exponential map for SEn3. 219 | * Returns a SEn3 object given a vector u in R(3+3n) (equivalent to exp(wedge(u))) 220 | * 221 | * @param u R(3+3n) vector 222 | * 223 | * @return SEn3 group element 224 | */ 225 | [[nodiscard]] static const SEn3 exp(const VectorType& u) 226 | { 227 | typename SO3Type::VectorType w = u.template block<3, 1>(0, 0); 228 | typename SO3Type::MatrixType SO3JL = SO3Type::leftJacobian(w); 229 | IsometriesType t; 230 | for (int i = 0; i < n; ++i) 231 | { 232 | t[i] = SO3JL * u.template block<3, 1>(3 + 3 * i, 0); 233 | } 234 | return SEn3(SO3Type::exp(w), t); 235 | } 236 | 237 | // /** 238 | // * @brief The exponential map for SEn3. 239 | // * Returns a SEn3 object given a matrix U in sen3 240 | // * 241 | // * @param U SEn3 Lie algebra element in matrix form 242 | // * 243 | // * @return SEn3 group element 244 | // */ 245 | // [[nodiscard]] static SEn3 exp(const MatrixType& U) { return exp(vee(U)); } 246 | 247 | /** 248 | * @brief The logarithmic map for SEn3. 249 | * Return a vector given a SEn3 object (equivalent to vee(log(X))) 250 | * 251 | * @param X SEn3 group element 252 | * 253 | * @return R(3+3n) vector 254 | */ 255 | [[nodiscard]] static const VectorType log(const SEn3& X) 256 | { 257 | VectorType u = VectorType::Zero(); 258 | u.template block<3, 1>(0, 0) = SO3Type::log(X.C_); 259 | typename SO3Type::MatrixType invSO3JL = SO3Type::invLeftJacobian(u.template block<3, 1>(0, 0)); 260 | for (int i = 0; i < n; ++i) 261 | { 262 | u.template block<3, 1>(3 + 3 * i, 0) = invSO3JL * X.t_[i]; 263 | } 264 | return u; 265 | } 266 | 267 | // /** 268 | // * @brief The logarithmic map for SEn3. 269 | // * Return a se3 matrix given a SEn3 object 270 | // * 271 | // * @param X SEn3 group element 272 | // * 273 | // * @return SEn3 Lie algebra element in matrix form 274 | // */ 275 | // [[nodiscard]] static MatrixType log(const SEn3& X) { return wedge(log(X)); } 276 | 277 | /** 278 | * @brief Operator * overloading. 279 | * Implements the SEn3 composition this * other 280 | * 281 | * @param other SEn3 group element 282 | * 283 | * @return SEn3 group element 284 | * 285 | * @note usage: z = x * y 286 | */ 287 | [[nodiscard]] const SEn3 operator*(const SEn3& other) const 288 | { 289 | IsometriesType t; 290 | for (int i = 0; i < n; ++i) 291 | { 292 | t[i] = C_.R() * other.t_[i] + t_[i]; 293 | } 294 | return SEn3(C_ * other.C_, t); 295 | } 296 | 297 | /** 298 | * @brief Operator * overloading. 299 | * Implements the SEn3 composition this * other with a SEn3 group or alegra element in matrix form 300 | * 301 | * @param other SE3 group or algebra element in matrix form 302 | * 303 | * @return SEn3 group or algebra element in matrix form 304 | * 305 | * @note usage: z = x * y 306 | */ 307 | [[nodiscard]] const MatrixType operator*(const MatrixType& other) const 308 | { 309 | bool is_algebra = true; 310 | bool is_group = true; 311 | 312 | for (int i = 0; i < n; ++i) 313 | { 314 | is_algebra &= (other(3 + i, 3 + i) == 0); 315 | is_group &= (other(3 + i, 3 + i) == 1); 316 | } 317 | 318 | if (!(is_algebra || is_group)) 319 | { 320 | throw std::runtime_error( 321 | "SEn3: operator* is defined only for composition with matrix form of SEn3 group or algebra elements"); 322 | } 323 | 324 | MatrixType res = other; 325 | res.template block<3, 3>(0, 0) = C_.R() * other.template block<3, 3>(0, 0); 326 | for (int i = 0; i < n; ++i) 327 | { 328 | if (is_algebra) 329 | { 330 | res.template block<3, 1>(0, 3 + i) = C_.R() * other.template block<3, 1>(0, 3 + i); 331 | } 332 | else 333 | { 334 | res.template block<3, 1>(0, 3 + i) = C_.R() * other.template block<3, 1>(0, 3 + i) + t_[i]; 335 | } 336 | } 337 | return res; 338 | } 339 | 340 | /** 341 | * @brief Operator * overloading. 342 | * Implements the SE3 action on a R3 vector 343 | * 344 | * @param other R3 vector 345 | * 346 | * @return R3 vector 347 | */ 348 | [[nodiscard]] const typename SO3Type::VectorType operator*(const typename SO3Type::VectorType& other) const 349 | { 350 | static_assert(n == 1, "SEn3: SE3 action on R3 (* operator overloading) available only for n = 1"); 351 | return C_.R() * other + t_[0]; 352 | } 353 | 354 | /** 355 | * @brief Implements the SEn3 composition this = this * other 356 | * 357 | * @param other SEn3 group element 358 | * 359 | * @return SEn3 group element 360 | */ 361 | const SEn3& multiplyRight(const SEn3& other) 362 | { 363 | for (int i = 0; i < n; ++i) 364 | { 365 | t_[i] = (C_.R() * other.t_[i] + t_[i]).eval(); 366 | } 367 | C_.multiplyRight(other.C_); // C_ * other.C_ 368 | return *this; 369 | } 370 | 371 | /** 372 | * @brief Implements the SEn3 composition this = other * this 373 | * 374 | * @param other SEn3 group element 375 | * 376 | * @return SEn3 group element 377 | */ 378 | const SEn3& multiplyLeft(const SEn3& other) 379 | { 380 | for (int i = 0; i < n; ++i) 381 | { 382 | t_[i] = (other.C_.R() * t_[i] + other.t_[i]).eval(); 383 | } 384 | C_.multiplyLeft(other.C_); // other.C_ * th.C_ 385 | return *this; 386 | } 387 | 388 | /** 389 | * @brief Get a constant copy of the inverse of the SEn3 object 390 | * 391 | * @return SEn3 group element 392 | */ 393 | [[nodiscard]] const SEn3 inv() const 394 | { 395 | IsometriesType t; 396 | for (int i = 0; i < n; ++i) 397 | { 398 | t[i] = -C_.R().transpose() * t_[i]; 399 | } 400 | return SEn3(C_.R().transpose(), t); 401 | } 402 | 403 | /** 404 | * @brief Get a constant copy of the SEn3 object as a matrix 405 | * 406 | * @return SEn3 group element in matrix form 407 | */ 408 | [[nodiscard]] const MatrixType T() const 409 | { 410 | MatrixType T = MatrixType::Identity(); 411 | T.template block<3, 3>(0, 0) = C_.R(); 412 | for (int i = 0; i < n; ++i) 413 | { 414 | T.template block<3, 1>(0, 3 + i) = t_[i]; 415 | } 416 | return T; 417 | } 418 | 419 | /** 420 | * @brief Get a constant reference to the SEn3 rotation matrix 421 | * 422 | * @return Rotation matrix 423 | */ 424 | [[nodiscard]] const typename SO3Type::MatrixType& R() const { return C_.R(); } 425 | 426 | /** 427 | * @brief Get a constant reference to the SEn3 normalized quaternion 428 | * 429 | * @return Quaternion 430 | */ 431 | [[nodiscard]] const typename SO3Type::QuaternionType& q() const { return C_.q(); } 432 | 433 | /** 434 | * @brief Get a constant reference to the SEn3 translation vectors (isometries) 435 | * 436 | * @return Array of R3 vectors 437 | */ 438 | [[nodiscard]] const IsometriesType& t() const { return t_; } 439 | 440 | /** 441 | * @brief Get a constant referece to the first isometry of SE3 442 | * 443 | * @note Method available only for n = 1, thus SE3 444 | * 445 | * @return R3 vector 446 | */ 447 | [[nodiscard]] const typename SO3Type::VectorType& x() const 448 | { 449 | static_assert(n == 1, "SEn3: x() method available only for n = 1"); 450 | return t_[0]; 451 | } 452 | 453 | /** 454 | * @brief Get a constant referece to the first isometry (velocity) of SEn3 with n > 1 455 | * 456 | * @note Method available only for n > 1 457 | * 458 | * @return R3 vector 459 | */ 460 | [[nodiscard]] const typename SO3Type::VectorType& v() const 461 | { 462 | static_assert(n > 1, "SEn3: v() method available only for n > 1"); 463 | return t_[0]; 464 | } 465 | 466 | /** 467 | * @brief Get a constant referece to the second isometry (position) of SEn3 with n > 1 468 | * 469 | * @note Method available only for n > 1 470 | * 471 | * @return R3 vector 472 | */ 473 | [[nodiscard]] const typename SO3Type::VectorType& p() const 474 | { 475 | static_assert(n > 1, "SEn3: v() method available only for n > 1"); 476 | return t_[1]; 477 | } 478 | 479 | /** 480 | * @brief Get a constant copy of the SEn3 object as a matrix 481 | * 482 | * @return SEn3 group element in matrix form 483 | */ 484 | [[nodiscard]] const MatrixType asMatrix() const { return T(); } 485 | 486 | /** 487 | * @brief Set SEn3 object value from given matrix 488 | * 489 | * @param T SEn3 group element in matrix form 490 | */ 491 | void fromT(const MatrixType& T) 492 | { 493 | C_.fromR(T.template block<3, 3>(0, 0)); 494 | for (int i = 0; i < n; ++i) 495 | { 496 | t_[i] = T.template block<3, 1>(0, 3 + i); 497 | } 498 | } 499 | 500 | /** 501 | * @brief SEn3 Adjoint matrix 502 | * 503 | * @return SEn3 group Adjoint matrix 504 | */ 505 | [[nodiscard]] const TMatrixType Adjoint() const 506 | { 507 | TMatrixType Ad = TMatrixType::Identity(); 508 | Ad.template block<3, 3>(0, 0) = C_.R(); 509 | for (int i = 0; i < n; ++i) 510 | { 511 | Ad.template block<3, 3>(3 + 3 * i, 3 + 3 * i) = C_.R(); 512 | Ad.template block<3, 3>(3 + 3 * i, 0) = SO3Type::wedge(t_[i]) * C_.R(); 513 | } 514 | return Ad; 515 | } 516 | 517 | /** 518 | * @brief SEn3 Inverse Adjoint matrix 519 | * 520 | * @return SEn3 group inverse Adjoint matrix 521 | */ 522 | [[nodiscard]] const TMatrixType invAdjoint() const 523 | { 524 | TMatrixType Ad = TMatrixType::Identity(); 525 | Ad.template block<3, 3>(0, 0) = C_.R().transpose(); 526 | for (int i = 0; i < n; ++i) 527 | { 528 | Ad.template block<3, 3>(3 + 3 * i, 3 + 3 * i) = C_.R().transpose(); 529 | Ad.template block<3, 3>(3 + 3 * i, 0) = -C_.R().transpose() * SO3Type::wedge(t_[i]); 530 | } 531 | return Ad; 532 | } 533 | 534 | protected: 535 | /** 536 | * @brief SE3 left Jacobian Q matrix 537 | * 538 | * @param w R3 vector 539 | * @param v R3 vector 540 | * 541 | * @return SE3 left Jacobian Q matrix 542 | */ 543 | [[nodiscard]] static const typename SO3Type::MatrixType SE3leftJacobianQ(const typename SO3Type::VectorType& w, 544 | const typename SO3Type::VectorType& v) 545 | { 546 | typename SO3Type::MatrixType p = SO3Type::wedge(w); 547 | typename SO3Type::MatrixType r = SO3Type::wedge(v); 548 | 549 | FPType ang = w.norm(); 550 | FPType s = sin(ang); 551 | FPType c = cos(ang); 552 | 553 | FPType ang_p2 = pow(ang, 2); 554 | FPType ang_p3 = ang_p2 * ang; 555 | FPType ang_p4 = ang_p3 * ang; 556 | FPType ang_p5 = ang_p4 * ang; 557 | 558 | FPType c1 = (ang - s) / ang_p3; 559 | FPType c2 = (0.5 * ang_p2 + c - 1.0) / ang_p4; 560 | FPType c3 = (ang * (1.0 + 0.5 * c) - 1.5 * s) / ang_p5; 561 | 562 | typename SO3Type::MatrixType m1 = p * r + r * p + p * r * p; 563 | typename SO3Type::MatrixType m2 = p * p * r + r * p * p - 3.0 * p * r * p; 564 | typename SO3Type::MatrixType m3 = p * r * p * p + p * p * r * p; 565 | 566 | return 0.5 * r + c1 * m1 + c2 * m2 + c3 * m3; 567 | } 568 | 569 | SO3Type C_; //!< SO3 object the rotational component of SEn3 570 | IsometriesType t_; //!< R6 vector representing the n translational components of SEn3 571 | 572 | static constexpr FPType eps_ = std::is_same_v ? 1.0e-6 : 1.0e-9; //!< Epsilon 573 | }; 574 | 575 | using SE3d = SEn3; //!< The SE3 group with double precision floating point 576 | using SE3f = SEn3; //!< The SE3 group with single precision floating point 577 | using SE23d = SEn3; //!< The SE23 group with double precision floating point 578 | using SE23f = SEn3; //!< The SE23 group with single precision floating point 579 | 580 | } // namespace group 581 | 582 | #endif // SEn3_HPP -------------------------------------------------------------------------------- /include/groups/SO3.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Pieter van Goor. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // System Theory and Robotics Lab, Australian Centre for Robotic 4 | // Vision, Australian national University, Australia. 5 | // 6 | // All rights reserved. 7 | // 8 | // This software is licensed under the terms of the BSD-2-Clause-License with 9 | // no commercial use allowed, the full terms of which are made available 10 | // in the LICENSE file. No license in patents is granted. 11 | // 12 | // You can contact the authors at , 13 | // . 14 | 15 | #ifndef SO3_HPP 16 | #define SO3_HPP 17 | 18 | #include 19 | 20 | namespace group 21 | { 22 | /** 23 | * @brief The Special orthogonal group of dimension 3 (SO3) representing 3D rotations 24 | * 25 | * @tparam FPType. Floating point type (float, FPType, long FPType) 26 | * 27 | * @note Group Formulation for Consistent Non-Linear Estiamtion 28 | * @note State Estimation for Robotics [http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf] 29 | * @note The Geometry and Kinematics of the Matrix Lie Group SEK(3) [https://arxiv.org/abs/2012.00950] 30 | */ 31 | template 32 | class SO3 33 | { 34 | public: 35 | using Scalar = FPType; //!< The underlying scalar type 36 | using MatrixType = Eigen::Matrix; //!< Lie Algebra / Lie Group matrix type 37 | using TMatrixType = Eigen::Matrix; //!< Transformation matrix type (Linear operator on R3) 38 | using VectorType = Eigen::Matrix; //!< R3 Vectorspace element type (isomorphic to Lie Algebra so3) 39 | using QuaternionType = Eigen::Quaternion; //!< Quaternion type 40 | using AngleAxisType = Eigen::AngleAxis; //!< Angle-axis type 41 | 42 | /** 43 | * @brief Construct an identity SO3 object 44 | */ 45 | SO3() : R_(MatrixType::Identity()), q_(QuaternionType::Identity()) {} 46 | 47 | /** 48 | * @brief Construct a SO3 object from a given normalized quaternion. 49 | * 50 | * @param q Quaternion 51 | */ 52 | SO3(const QuaternionType& q) : R_(q.toRotationMatrix()), q_(q) 53 | { 54 | if (!isNormalized()) 55 | { 56 | normalize(); 57 | } 58 | } 59 | 60 | /** 61 | * @brief Construct a SO3 object from a given rotation matrix 62 | * 63 | * @param R Rotation matrix 64 | */ 65 | SO3(const MatrixType& R) : R_(R), q_(R) 66 | { 67 | if (!isNormalized()) 68 | { 69 | normalize(); 70 | } 71 | } 72 | 73 | /** 74 | * @brief Construct a SO3 object from two vector such that Ru = v 75 | * 76 | * @param u Vector in R3 77 | * @param v vector in R3 78 | */ 79 | SO3(const VectorType& u, const VectorType& v) : R_(), q_() 80 | { 81 | VectorType un = u.normalized(); 82 | VectorType vn = v.normalized(); 83 | 84 | VectorType ax = un.cross(vn); 85 | FPType s = ax.norm(); 86 | FPType c = un.dot(vn); 87 | 88 | if (s == 0) 89 | { 90 | q_ = QuaternionType::Identity(); 91 | R_ = MatrixType::Identity(); 92 | } 93 | else if (std::abs(1 + c) < eps_) 94 | { 95 | ax = un.cross(VectorType::Random()); 96 | ax.normalize(); 97 | q_ = QuaternionType(0.0, ax(0), ax(1), ax(2)); 98 | R_ = q_.toRotationMatrix(); 99 | } 100 | else 101 | { 102 | R_ = MatrixType::Identity() + wedge(ax) + ((1.0 - c) / (s * s)) * (wedge(ax) * wedge(ax)); 103 | q_ = QuaternionType(R_); 104 | } 105 | } 106 | 107 | /** 108 | * @brief wedge operator, transform a vector in R3 to a matrix in so3 109 | * 110 | * @param u R3 vector 111 | * @return SO3 Lie algebra element in matrix form 112 | */ 113 | [[nodiscard]] static const MatrixType wedge(const VectorType& u) 114 | { 115 | MatrixType U; 116 | U << 0.0, -u(2), u(1), u(2), 0.0, -u(0), -u(1), u(0), 0.0; 117 | return (U); 118 | } 119 | 120 | /** 121 | * @brief Transform a matrix in so3 to a vector in R3 122 | * 123 | * @param U SO3 Lie algebra element in matrix form 124 | * 125 | * @return R3 vector 126 | */ 127 | [[nodiscard]] static const VectorType vee(const MatrixType& U) 128 | { 129 | VectorType u; 130 | u << U(2, 1), U(0, 2), U(1, 0); 131 | return (u); 132 | } 133 | 134 | /** 135 | * @brief so3 adjoint matrix 136 | * 137 | * @param u R3 vector 138 | * 139 | * @return SO3 Lie algebra adjoint matrix 140 | */ 141 | [[nodiscard]] static const TMatrixType adjoint(const VectorType& u) { return wedge(u); } 142 | 143 | /** 144 | * @brief SO3 Gamma2 matrix 145 | * 146 | * @param u R3 vector 147 | * 148 | * @return SO3 Gamma2 matrix 149 | */ 150 | [[nodiscard]] static const TMatrixType Gamma2(const VectorType& u) 151 | { 152 | FPType ang = u.norm(); 153 | if (ang < eps_) 154 | { 155 | return 0.5 * TMatrixType::Identity() - 1 / 6 * wedge(u); 156 | } 157 | VectorType ax = u / ang; 158 | FPType ang_p2 = pow(ang, 2); 159 | FPType s = (ang - sin(ang)) / ang_p2; 160 | FPType c = (1 - cos(ang)) / ang_p2; 161 | return c * TMatrixType::Identity() + s * wedge(ax) + (0.5 - c) * ax * ax.transpose(); 162 | } 163 | 164 | /** 165 | * @brief SO3 left Jacobian matrix 166 | * 167 | * @param u R3 vector 168 | * 169 | * @return SO3 left Jacobian matrix 170 | */ 171 | [[nodiscard]] static const TMatrixType leftJacobian(const VectorType& u) 172 | { 173 | FPType ang = u.norm(); 174 | if (ang < eps_) 175 | { 176 | return TMatrixType::Identity() + 0.5 * wedge(u); 177 | } 178 | VectorType ax = u / ang; 179 | FPType s = sin(ang) / ang; 180 | FPType c = cos(ang); 181 | return s * TMatrixType::Identity() + ((1.0 - c) / ang) * wedge(ax) + (1.0 - s) * ax * ax.transpose(); 182 | } 183 | 184 | /** 185 | * @brief SO3 inverse left Jacobian matrix 186 | * 187 | * @param u R3 vector 188 | * 189 | * @return SO3 inverse left Jacobian matrix 190 | */ 191 | [[nodiscard]] static const TMatrixType invLeftJacobian(const VectorType& u) 192 | { 193 | FPType ang = u.norm(); 194 | if (ang < eps_) 195 | { 196 | return TMatrixType::Identity() - 0.5 * wedge(u); 197 | } 198 | VectorType ax = u / ang; 199 | FPType half_ang = 0.5 * ang; 200 | FPType cot = 1.0 / tan(half_ang); 201 | return (half_ang * cot) * TMatrixType::Identity() + (1.0 - half_ang * cot) * ax * ax.transpose() - 202 | half_ang * wedge(ax); 203 | } 204 | 205 | /** 206 | * @brief SO3 right Jacobian matrix 207 | * 208 | * @param u R3 vector 209 | * 210 | * @return SO3 right Jacobian matrix 211 | */ 212 | [[nodiscard]] static const TMatrixType rightJacobian(const VectorType& u) { return leftJacobian(-u); } 213 | 214 | /** 215 | * @brief SO3 inverse right Jacobian matrix 216 | * 217 | * @param u R3 vector 218 | * 219 | * @return SO3 inverse right Jacobian matrix 220 | */ 221 | [[nodiscard]] static const TMatrixType invRightJacobian(const VectorType& u) { return invLeftJacobian(-u); } 222 | 223 | /** 224 | * @brief The exponential map for SO3. 225 | * Returns a SO3 object given a vector u in R3 (equivalent to exp(wedge(u))) 226 | * 227 | * @param u R3 vector 228 | * 229 | * @return SO3 group element 230 | */ 231 | [[nodiscard]] static const SO3 exp(const VectorType& u) 232 | { 233 | FPType ang = 0.5 * u.norm(); 234 | QuaternionType q; 235 | if (ang < eps_) 236 | { 237 | q.w() = 1.0; 238 | q.vec() = 0.5 * u; 239 | } 240 | else 241 | { 242 | q.w() = cos(ang); 243 | q.vec() = sin(ang) * u.normalized(); 244 | } 245 | q.normalize(); 246 | return SO3(q); 247 | } 248 | 249 | // /** 250 | // * @brief The exponential map for SO3. 251 | // * Returns a SO3 object given a matrix U in so3 252 | // * 253 | // * @param U SO3 Lie algebra element in matrix form 254 | // * 255 | // * @return SO3 group element 256 | // */ 257 | // [[nodiscard]] static const SO3 exp(const MatrixType& U) { return exp(vee(U)); } 258 | 259 | /** 260 | * @brief The logarithmic map for SO3. 261 | * Return a vector given a SO3 object (equivalent to vee(log(X))) 262 | * 263 | * @param X SO3 group element 264 | * 265 | * @return R3 vector 266 | */ 267 | [[nodiscard]] static const VectorType log(const SO3& X) 268 | { 269 | FPType qw = X.q_.w(); 270 | VectorType qv = X.q_.vec(); 271 | FPType ang = qv.norm(); 272 | VectorType u; 273 | if (qw < 0) 274 | { 275 | qw = -qw; 276 | qv = -qv; 277 | } 278 | if (ang < eps_) 279 | { 280 | u = (2.0 / qw) * qv * (1.0 - (pow((ang / qw), 2) / 3)); 281 | } 282 | else 283 | { 284 | u = 2 * atan2(ang, qw) * (qv / ang); 285 | } 286 | return u; 287 | } 288 | 289 | // /** 290 | // * @brief The logarithmic map for SO3. 291 | // * Return a so3 matrix given a SO3 object 292 | // * 293 | // * @param X SO3 group element 294 | // * 295 | // * @return SO3 Lie algebra element in matrix form 296 | // */ 297 | // [[nodiscard]] static const MatrixType log(const SO3& X) { return wedge(log(X)); } 298 | 299 | /** 300 | * @brief Operator * overloading. 301 | * Implements the SO3 composition this * other 302 | * 303 | * @param other SO3 group element 304 | * 305 | * @return SO3 group element 306 | * 307 | * @note usage: z = x * y 308 | */ 309 | [[nodiscard]] const SO3 operator*(const SO3& other) const { return SO3(q_ * other.q_); } 310 | 311 | /** 312 | * @brief Operator * overloading. 313 | * Implements the SO3 composition this * other with a SO3 group or alegra element in matrix form 314 | * 315 | * @param other SO3 group or algebra element in matrix form 316 | * 317 | * @return SO3 group or algebra element in matrix form 318 | * 319 | * @note usage: z = x * y 320 | */ 321 | [[nodiscard]] const MatrixType operator*(const MatrixType& other) const { return R_ * other; } 322 | 323 | /** 324 | * @brief Operator * overloading. 325 | * Implements the SO3 action on a R3 vector 326 | * 327 | * @param other R3 vector 328 | * 329 | * @return R3 vector 330 | */ 331 | [[nodiscard]] const VectorType operator*(const VectorType& other) const 332 | { 333 | return q_ * other; 334 | // return R_ * other; 335 | } 336 | 337 | /** 338 | * @brief Implements the SO3 composition this = this * other 339 | * 340 | * @param other SO3 group element 341 | * 342 | * @return SO3 group element 343 | */ 344 | const SO3& multiplyRight(const SO3& other) 345 | { 346 | q_ = q_ * other.q_; 347 | R_ = R_ * other.R_; 348 | return *this; 349 | } 350 | 351 | /** 352 | * @brief Implements the SO3 composition this = other * this 353 | * 354 | * @param other SO3 group element 355 | * 356 | * @return SO3 group element 357 | */ 358 | const SO3& multiplyLeft(const SO3& other) 359 | { 360 | q_ = other.q_ * q_; 361 | R_ = other.R_ * R_; 362 | return *this; 363 | } 364 | 365 | /** 366 | * @brief Get a constant copy of the inverse of the SO3 object 367 | * 368 | * @return SO3 group element 369 | */ 370 | [[nodiscard]] const SO3 inv() const { return SO3(q_.inverse()); } 371 | 372 | /** 373 | * @brief Get a constant reference to the SO3 rotation matrix 374 | * 375 | * @return Rotation matrix 376 | */ 377 | [[nodiscard]] const MatrixType& R() const { return R_; } 378 | 379 | /** 380 | * @brief Get a constant reference to the SO3 normalized quaternion 381 | * 382 | * @return Quaternion 383 | */ 384 | [[nodiscard]] const QuaternionType& q() const { return q_; } 385 | 386 | /** 387 | * @brief Get a constant reference to the SO3 object as a matrix 388 | * 389 | * @return SO3 group element in matrix form 390 | */ 391 | [[nodiscard]] const MatrixType& asMatrix() const { return R_; } 392 | 393 | /** 394 | * @brief Set SO3 object value from a given normalized quaternion 395 | * 396 | * @param q quaternion 397 | */ 398 | void fromq(const QuaternionType& q) 399 | { 400 | q_ = q; 401 | R_ = q.toRotationMatrix(); 402 | 403 | if (!isNormalized()) 404 | { 405 | normalize(); 406 | } 407 | } 408 | 409 | /** 410 | * @brief Set SO3 object value from given rotation matrix 411 | * 412 | * @param R rotation matrix 413 | */ 414 | void fromR(const MatrixType& R) 415 | { 416 | q_ = R; 417 | R_ = R; 418 | 419 | if (!isNormalized()) 420 | { 421 | normalize(); 422 | } 423 | } 424 | 425 | /** 426 | * @brief SO3 Adjoint matrix 427 | * 428 | * @return SO3 group Adjoint matrix 429 | */ 430 | [[nodiscard]] const TMatrixType Adjoint() const { return R_; } 431 | 432 | /** 433 | * @brief SO3 Inverse Adjoint matrix 434 | * 435 | * @return SO3 group inverse Adjoint matrix 436 | */ 437 | [[nodiscard]] const TMatrixType invAdjoint() const { return R_.transpose(); } 438 | 439 | protected: 440 | /** 441 | * @brief Check that q_ is a normalized quaternion 442 | * 443 | * @return true if q_ is normalized 444 | */ 445 | [[nodiscard]] bool isNormalized() { return std::abs(q_.norm() - 1.0) < eps_; } 446 | 447 | /** 448 | * @brief Normalize the quaternion and update the rotation matrix 449 | */ 450 | void normalize() 451 | { 452 | q_.normalize(); 453 | R_ = q_.toRotationMatrix(); 454 | } 455 | 456 | MatrixType R_; //!< Rotation matrix 457 | QuaternionType q_; //!< Normalized quaternion 458 | 459 | static constexpr FPType eps_ = std::is_same_v ? 1.0e-6 : 1.0e-9; //!< Epsilon 460 | }; 461 | 462 | using SO3d = SO3; //!< The SO3 group with double precision floating point 463 | using SO3f = SO3; //!< The SO3 group with single precision floating point 464 | 465 | } // namespace group 466 | 467 | #endif // SO3_HPP -------------------------------------------------------------------------------- /include/groups/SOT3.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Pieter van Goor. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // System Theory and Robotics Lab, Australian Centre for Robotic 4 | // Vision, Australian national University, Australia. 5 | // 6 | // All rights reserved. 7 | // 8 | // This software is licensed under the terms of the BSD-2-Clause-License with 9 | // no commercial use allowed, the full terms of which are made available 10 | // in the LICENSE file. No license in patents is granted. 11 | // 12 | // You can contact the authors at , 13 | // . 14 | 15 | #ifndef SOT3_HPP 16 | #define SOT3_HPP 17 | 18 | #include "SO3.hpp" 19 | 20 | namespace group 21 | { 22 | /** 23 | * @brief The Scaled orthogonal transforms of dimension 3 (SOT3) 24 | * 25 | * @tparam FPType. Floating point type (float, double, long double) 26 | * 27 | * @note EqVIO: An Equivariant Filter for Visual Inertial Odometry [https://arxiv.org/abs/2205.01980] 28 | */ 29 | template 30 | class SOT3 31 | { 32 | public: 33 | using Scalar = FPType; //!< The underlying scalar type 34 | using SO3Type = SO3; //!< The underlying SO3 type 35 | using VectorType = Eigen::Matrix; //!< R4 Vectorspace element type (isomorphic to Lie Algebra sot3) 36 | using MatrixType = Eigen::Matrix; //!< Lie Algebra / Lie Group matrix type 37 | using TMatrixType = Eigen::Matrix; //!< the transformation matrix type (Linear operator on R4) 38 | using ScaleType = FPType; //!< Scale factor type 39 | 40 | /** 41 | * @brief Construct an identity SOT3 object 42 | */ 43 | SOT3() : C_(SO3Type()), s_(1.0) {} 44 | 45 | /** 46 | * @brief Construct a SOT3 object from a given normalized quaternion, and scale. 47 | * 48 | * @param q Quaternion 49 | * @param s Scale 50 | */ 51 | SOT3(const typename SO3Type::QuaternionType& q, const ScaleType& s) : C_(q), s_(s) { checkScale(); } 52 | 53 | /** 54 | * @brief Construct a SOT3 object from a given rotation matrix, and scale. 55 | * 56 | * @param R Rotation matrix 57 | * @param s Scale 58 | */ 59 | SOT3(const typename SO3Type::MatrixType& R, const ScaleType& s) : C_(R), s_(s) { checkScale(); } 60 | 61 | /** 62 | * @brief Construct a SOT3 object from a given SO3 object, and scale. 63 | * 64 | * @param C SO3 group element 65 | * @param s Scale 66 | */ 67 | SOT3(const SO3Type& C, const ScaleType& s) : C_(C), s_(s) { checkScale(); } 68 | 69 | /** 70 | * @brief Construct a SOT3 object from a given matrix. 71 | * 72 | * @param Q SOT3 group element in matrix form 73 | */ 74 | SOT3(const MatrixType& Q) : C_(Q.template block<3, 3>(0, 0)), s_(Q(3, 3)) { checkScale(); } 75 | 76 | /** 77 | * @brief Construct a SOT3 object from two vector such that Ru = v 78 | * 79 | * @param u Vector in R3 80 | * @param v vector in R3 81 | */ 82 | SOT3(const typename SO3Type::VectorType& u, const typename SO3Type::VectorType& v) : C_(u, v), s_(1.0) 83 | { 84 | if (u.norm() > eps_ && v.norm() > eps_) 85 | { 86 | s_ = v.norm() / u.norm(); 87 | } 88 | checkScale(); 89 | } 90 | 91 | /** 92 | * @brief wedge operator, transform a vector in R4 to a matrix in sot3 93 | * 94 | * @param u R4 vector 95 | * 96 | * @return SOT3 Lie algebra element in matrix form 97 | */ 98 | [[nodiscard]] static const MatrixType wedge(const VectorType& u) 99 | { 100 | MatrixType U = MatrixType::Zero(); 101 | U.template block<3, 3>(0, 0) = SO3Type::wedge(u.template block<3, 1>(0, 0)); 102 | U(3, 3) = u(3); 103 | return U; 104 | } 105 | 106 | /** 107 | * @brief transform a matrix in sot3 to a vector in R4 108 | * 109 | * @param U SOT3 Lie algebra element in matrix form 110 | * 111 | * @return R4 vector 112 | */ 113 | [[nodiscard]] static const VectorType vee(const MatrixType& U) 114 | { 115 | VectorType u = VectorType::Zero(); 116 | u.template block<3, 1>(0, 0) = SO3Type::vee(U.template block<3, 3>(0, 0)); 117 | u(3) = U(3, 3); 118 | return u; 119 | } 120 | 121 | /** 122 | * @brief sot3 adjoint matrix 123 | * 124 | * @param u R4 vector 125 | * 126 | * @return SOT3 Lie algebra adjoint matrix 127 | */ 128 | [[nodiscard]] static const TMatrixType adjoint(const VectorType& u) 129 | { 130 | TMatrixType ad = TMatrixType::Zero(); 131 | ad.template block<3, 3>(0, 0) = SO3Type::wedge(u.template block<3, 1>(0, 0)); 132 | return ad; 133 | } 134 | 135 | /** 136 | * @brief SOT3 left Jacobian matrix 137 | * 138 | * @param u R4 vector 139 | * 140 | * @return SOT3 left Jacobian matrix 141 | */ 142 | [[nodiscard]] static const TMatrixType leftJacobian(const VectorType& u) 143 | { 144 | TMatrixType J = TMatrixType::Identity(); 145 | J.template block<3, 3>(0, 0) = SO3Type::leftJacobian(u.template block<3, 1>(0, 0)); 146 | return J; 147 | } 148 | 149 | /** 150 | * @brief SOT3 inverse left Jacobian matrix 151 | * 152 | * @param u R4 vector 153 | * 154 | * @return SOT3 inverse left Jacobian matrix 155 | */ 156 | [[nodiscard]] static const TMatrixType invLeftJacobian(const VectorType& u) 157 | { 158 | TMatrixType J = TMatrixType::Identity(); 159 | J.template block<3, 3>(0, 0) = SO3Type::invLeftJacobian(u.template block<3, 1>(0, 0)); 160 | return J; 161 | } 162 | 163 | /** 164 | * @brief SOT3 right Jacobian matrix 165 | * 166 | * @param u R4 vector 167 | * 168 | * @return SOT3 right Jacobian matrix 169 | */ 170 | [[nodiscard]] static const TMatrixType rightJacobian(const VectorType& u) { return leftJacobian(-u); } 171 | 172 | /** 173 | * @brief SOT3 inverse right Jacobian matrix 174 | * 175 | * @param u R4 vector 176 | * 177 | * @return SOT3 inverse right Jacobian matrix 178 | */ 179 | [[nodiscard]] static const TMatrixType invRightJacobian(const VectorType& u) { return invLeftJacobian(-u); } 180 | 181 | /** 182 | * @brief The exponential map for SOT3. 183 | * Returns a SOT3 object given a vector u in R4 (equivalent to exp(wedge(u))) 184 | * 185 | * @param u R4 vector 186 | * 187 | * @return SOT3 group element 188 | */ 189 | [[nodiscard]] static const SOT3 exp(const VectorType& u) 190 | { 191 | if (u(3) < eps_) 192 | { 193 | return SOT3(SO3Type::exp(u.template block<3, 1>(0, 0)), 1.0 + std::expm1(u(3))); 194 | } 195 | else 196 | { 197 | return SOT3(SO3Type::exp(u.template block<3, 1>(0, 0)), std::exp(u(3))); 198 | } 199 | } 200 | 201 | // /** 202 | // * @brief The exponential map for SOT3. 203 | // * Returns a SOT3 object given a matrix U in sot3 204 | // * 205 | // * @param U SOT3 Lie algebra element in matrix form 206 | // * 207 | // * @return SOT3 group element 208 | // */ 209 | // [[nodiscard]] static const SOT3 exp(const MatrixType& U) { return exp(vee(U)); } 210 | 211 | /** 212 | * @brief The logarithmic map for SOT3. 213 | * Return a vector given a SOT3 object (equivalent to vee(log(X))) 214 | * 215 | * @param X SOT3 group element 216 | * 217 | * @return R4 vector 218 | */ 219 | [[nodiscard]] static const VectorType log(const SOT3& X) 220 | { 221 | VectorType u = VectorType::Zero(); 222 | u.template block<3, 1>(0, 0) = SO3Type::log(X.C_); 223 | u(3) = std::log(X.s_); 224 | return u; 225 | } 226 | 227 | // /** 228 | // * @brief The logarithmic map for SOT3. 229 | // * Return a sot3 matrix given a SOT3 object 230 | // * 231 | // * @param X SOT3 group element 232 | // * 233 | // * @return SO3 Lie algebra element in matrix form 234 | // */ 235 | // [[nodiscard]] static const MatrixType log(const SOT3& X) { return wedge(log(X)); } 236 | 237 | /** 238 | * @brief Operator * overloading. 239 | * Implements the SOT3 composition this * other 240 | * 241 | * @param other SOT3 group element 242 | * 243 | * @return SOT3 group element 244 | * 245 | * @note usage: z = x * y 246 | */ 247 | [[nodiscard]] const SOT3 operator*(const SOT3& other) const { return SOT3(C_ * other.C_, s_ * other.s_); } 248 | 249 | /** 250 | * @brief Operator * overloading. 251 | * Implements the SOT3 composition this * other with a SOT3 group or alegra element in matrix form 252 | * 253 | * @param other SOT3 group or algebra element in matrix form 254 | * 255 | * @return SOT3 group or algebra element in matrix form 256 | * 257 | * @note usage: z = x * y 258 | */ 259 | [[nodiscard]] const MatrixType operator*(const MatrixType& other) const 260 | { 261 | MatrixType res = MatrixType::Zero(); 262 | res.template block<3, 3>(0, 0) = C_.R() * other.template block<3, 3>(0, 0); 263 | res(3, 3) = s_ * other(3, 3); 264 | return res; 265 | } 266 | 267 | /** 268 | * @brief Operator * overloading. 269 | * Implements the SOT3 action on a R3 vector 270 | * 271 | * @param other R3 vector 272 | * 273 | * @return R3 vector 274 | */ 275 | [[nodiscard]] const typename SO3Type::VectorType operator*(const typename SO3Type::VectorType& other) const 276 | { 277 | return s_ * (C_.R() * other); 278 | } 279 | 280 | /** 281 | * @brief Implements the SOT3 composition this = this * other 282 | * 283 | * @param other SOT3 group element 284 | * 285 | * @return SOT3 group element 286 | */ 287 | const SOT3& multiplyRight(const SOT3& other) 288 | { 289 | C_.multiplyRight(other.C_); // C_ * other.C_ 290 | s_ *= other.s_; 291 | return *this; 292 | } 293 | 294 | /** 295 | * @brief Implements the SOT3 composition this = other * this 296 | * 297 | * @param other SOT3 group element 298 | * 299 | * @return SOT3 group element 300 | */ 301 | const SOT3& multiplyLeft(const SOT3& other) 302 | { 303 | C_.multiplyLeft(other.C_); // other.C_ * th.C_ 304 | s_ *= other.s_; 305 | return *this; 306 | } 307 | 308 | /** 309 | * @brief Get a constant copy of the inverse of the SOT3 object 310 | */ 311 | [[nodiscard]] const SOT3 inv() const { return SOT3(C_.R().transpose(), 1 / s_); } 312 | 313 | /** 314 | * @brief Get a constant copy of the SOT3 object as a matrix 315 | * 316 | * @return SOT3 group element in matrix form 317 | */ 318 | [[nodiscard]] const MatrixType Q() const 319 | { 320 | MatrixType Q = MatrixType::Identity(); 321 | Q.template block<3, 3>(0, 0) = C_.R(); 322 | Q(3, 3) = s_; 323 | return Q; 324 | } 325 | 326 | /** 327 | * @brief Get a constant reference to the SOT3 rotation matrix 328 | * 329 | * @return Rotation matrix 330 | */ 331 | [[nodiscard]] const typename SO3Type::MatrixType& R() const { return C_.R(); } 332 | 333 | /** 334 | * @brief Get a constant reference to the SOT3 normalized quaternion 335 | * 336 | * @return Quaternion 337 | */ 338 | [[nodiscard]] const typename SO3Type::QuaternionType& q() const { return C_.q(); } 339 | 340 | /** 341 | * @brief Get a constant reference to the SOT3 scale 342 | * 343 | * @return scale 344 | */ 345 | [[nodiscard]] const ScaleType& s() const { return s_; } 346 | 347 | /** 348 | * @brief Get a constant copy of the SOT3 object as a matrix 349 | * 350 | * @return SOT3 group element in matrix form 351 | */ 352 | [[nodiscard]] const MatrixType asMatrix() const { return Q(); } 353 | 354 | /** 355 | * @brief Set SOT3 object value from given matrix 356 | * 357 | * @param Q SOT3 group element in matrix form 358 | */ 359 | void fromQ(const MatrixType& Q) 360 | { 361 | C_.fromR(Q.template block<3, 3>(0, 0)); 362 | s_ = Q(3, 3); 363 | checkScale(); 364 | } 365 | 366 | /** 367 | * @brief SOT3 Adjoint matrix 368 | * 369 | * @return SOT3 group Adjoint matrix 370 | */ 371 | [[nodiscard]] const TMatrixType Adjoint() const 372 | { 373 | TMatrixType Ad = TMatrixType::Identity(); 374 | Ad.template block<3, 3>(0, 0) = C_.R(); 375 | return Ad; 376 | } 377 | 378 | /** 379 | * @brief SOT3 Inverse Adjoint matrix 380 | * 381 | * @return SOT3 group inverse Adjoint matrix 382 | */ 383 | [[nodiscard]] const TMatrixType invAdjoint() const 384 | { 385 | TMatrixType Ad = TMatrixType::Identity(); 386 | Ad.template block<3, 3>(0, 0) = C_.R().transpose(); 387 | return Ad; 388 | } 389 | 390 | protected: 391 | /** 392 | * @brief Check that s_ is grater than zero 393 | */ 394 | void checkScale() 395 | { 396 | if (s_ < 0) 397 | { 398 | throw std::invalid_argument("SOT3: Scale has to be grater than zero!"); 399 | } 400 | } 401 | 402 | SO3Type C_; //!< SO3 object the rotational component of SOT3 403 | ScaleType s_; //!< Scale factor 404 | 405 | static constexpr FPType eps_ = std::is_same_v ? 1.0e-6 : 1.0e-9; //!< Epsilon 406 | }; 407 | 408 | using SOT3d = SOT3; //!< The SOT3 group with double precision floating point 409 | using SOT3f = SOT3; //!< The SOT3 group with single precision floating point 410 | 411 | } // namespace group 412 | 413 | #endif // SOT3_HPP -------------------------------------------------------------------------------- /include/groups/TG.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Pieter van Goor. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // System Theory and Robotics Lab, Australian Centre for Robotic 4 | // Vision, Australian National University. 5 | // 6 | // All rights reserved. 7 | // 8 | // This software is licensed under the terms of the BSD-2-Clause-License with 9 | // no commercial use allowed, the full terms of which are made available 10 | // in the LICENSE file. No license in patents is granted. 11 | // 12 | // You can contact the authors at 13 | 14 | #ifndef TG_HPP 15 | #define TG_HPP 16 | 17 | #include "SEn3.hpp" 18 | #include "Gal3.hpp" 19 | 20 | namespace group 21 | { 22 | /** 23 | * @brief The Tangent group. The tangent group of a Lie group G is a semi-direct product group between G and 24 | * its Lie algebra g, denoted G ⋉ g. 25 | * 26 | * @tparam GroupType. The underlying Lie group type 27 | * 28 | * @note Equivariant Filter Design for Inertial Navigation Systems with Input Measurement Biases 29 | * [https://arxiv.org/abs/2202.02058] [https://ieeexplore.ieee.org/document/9811778/] 30 | * @note Equivariant Symmetries for Inertial Navigation Systems [https://arxiv.org/abs/2309.03765] 31 | */ 32 | template 33 | class Tangent 34 | { 35 | public: 36 | using Group = GroupType; //!< The underlying Lie group type 37 | using Scalar = typename Group::Scalar; //!< The underlying scalar type 38 | static constexpr int N = Group::VectorType::RowsAtCompileTime; //!< The dimension of the group Lie algebra 39 | using VectorType = Eigen::Matrix; //!< The underlying vector type 40 | 41 | /** 42 | * @brief Construct an identity Tangent Group object 43 | */ 44 | Tangent() : G_(), g_(Group::VectorType::Zero()){}; 45 | 46 | /** 47 | * @brief Construct a Tangent Group object from a given group object, and a vector 48 | * 49 | * @param G Lie group element 50 | * @param g Lie algebra vector 51 | */ 52 | Tangent(const Group& G, const typename Group::VectorType& g) : G_(G), g_(g){}; 53 | 54 | /** 55 | * @brief The exponential map for the Tangent Group TG. 56 | * Returns a Tangent object given a VetorType u (equivalent to exp(wedge(u))) 57 | * 58 | * @param u VetorType 59 | * 60 | * @return TG group element 61 | * 62 | */ 63 | [[nodiscard]] static const Tangent exp(const VectorType& u) 64 | { 65 | return Tangent(Group::exp(u.template block(0, 0)), 66 | Group::leftJacobian(u.template block(0, 0)) * u.template block(N, 0)); 67 | } 68 | 69 | /** 70 | * @brief The logarithmic map for the Tangent Group. 71 | * Return a vector given a Tangent object (equivalent to vee(log(X))) 72 | * 73 | * @param X TG element 74 | * 75 | * @return VectorType vector 76 | */ 77 | [[nodiscard]] static const VectorType log(const Tangent& X) 78 | { 79 | VectorType u = VectorType::Zero(); 80 | u.template block(0, 0) = Group::log(X.G_); 81 | u.template block(N, 0) = (Group::invLeftJacobian(u.template block(0, 0))) * X.g_; 82 | return u; 83 | } 84 | 85 | /** 86 | * @brief Get a constant reference to G (the Lie group element) 87 | * 88 | * @return Group element 89 | */ 90 | [[nodiscard]] const Group& G() const { return G_; } 91 | 92 | /** 93 | * @brief Get a constant reference to g (the Lie algebra element) 94 | * 95 | * @return Lie algebra vector 96 | */ 97 | [[nodiscard]] const typename Group::VectorType& g() const { return g_; } 98 | 99 | /** 100 | * @brief Get a constant copy of the inverse of the Tangent object 101 | * 102 | * @return TG group element 103 | */ 104 | [[nodiscard]] const Tangent inv() const { return Tangent(G_.inv(), -G_.invAdjoint() * g_); } 105 | 106 | /** 107 | * @brief Operator * overloading. 108 | * Implements the Tangent composition this * other 109 | * 110 | * @param other TG group element 111 | * 112 | * @return TG group element 113 | * 114 | * @note usage: z = x * y 115 | */ 116 | [[nodiscard]] const Tangent operator*(const Tangent& other) const 117 | { 118 | return Tangent(G_ * other.G_, g_ + G_.Adjoint() * other.g_); 119 | } 120 | 121 | /** 122 | * @brief Implements the Tangent composition this = this * other 123 | * 124 | * @param other TG group element 125 | * 126 | * @return TG group element 127 | */ 128 | const Tangent& multiplyRight(const Tangent& other) 129 | { 130 | g_ = (g_ + G_.Adjoint() * other.g_).eval(); 131 | G_.multiplyRight(other.G_); // G_ * other.G_ 132 | return *this; 133 | } 134 | 135 | /** 136 | * @brief Implements the Tangent composition this = other * this 137 | * 138 | * @param other TG group element 139 | * 140 | * @return TG group element 141 | */ 142 | const Tangent& multiplyLeft(const Tangent& other) 143 | { 144 | g_ = (other.g_ + other.G_.Adjoint() * g_).eval(); 145 | G_.multiplyLeft(other.G_); // other.G_ * G_ 146 | return *this; 147 | } 148 | 149 | private: 150 | Group G_; //!< The Lie group element of the symmetry group 151 | typename Group::VectorType g_; //!< The Lie algebra vector of the symmetry group 152 | }; 153 | 154 | /** 155 | * @brief The SEn3 Tangent group. This derived class represents the core components of the symmetry group for Inertial 156 | * Navigation Systems (INS), including the SE23 |x se23 semi-direct product acting on the extended pose and IMU biases, 157 | * including a virtual velocity bias 158 | * 159 | * @tparam FPType. Floating point type (float, double, long double) 160 | * @tparam n. The dimension of the underlying vector space 161 | * 162 | */ 163 | 164 | template 165 | class SEn3TG : public Tangent> 166 | { 167 | public: 168 | using BaseType = Tangent>; 169 | using SE3Type = SEn3; //!< The underlying SE3 type 170 | 171 | /** 172 | * @brief Default constructor 173 | */ 174 | SEn3TG() : BaseType() {} 175 | 176 | /** 177 | * @brief Construct a SEn3TG object from a given group object and vector 178 | * 179 | * @param G Lie group element 180 | * @param g Lie algebra vector 181 | */ 182 | SEn3TG(const typename BaseType::Group& G, const typename BaseType::Group::VectorType& g) : BaseType(G, g) {} 183 | 184 | /** 185 | * @brief Get a constant copy of B (the SE3 subgroup of SE23 composed by the rotational component (R) and the first 186 | * isometry (v)) 187 | * 188 | * @return SE3 group element 189 | */ 190 | [[nodiscard]] const SE3Type B() const { return SE3Type(this->G().q(), {this->G().v()}); } 191 | 192 | /** 193 | * @brief Get a constant copy of C (the SE3 subgroup of SE23 composed by the rotational component (R) and the 194 | * second isometry (p)) 195 | * 196 | * @return SE3 group element 197 | */ 198 | [[nodiscard]] const SE3Type C() const { return SE3Type(this->G().q(), {this->G().p()}); } 199 | }; 200 | 201 | /** 202 | * @brief The Gal3 Tangent group. This derived class represents the core components of the symmetry group 203 | * for equivariant IMU preintegration [preprint: https://arxiv.org/abs/2411.05548] 204 | * 205 | * @tparam FPType. Floating point type (float, double, long double) 206 | * 207 | */ 208 | template 209 | class Gal3TG : public Tangent> 210 | { 211 | public: 212 | using BaseType = Tangent>; 213 | using SE3Type = SEn3; //!< The underlying SE3 type 214 | 215 | /** 216 | * @brief Default constructor 217 | */ 218 | Gal3TG() : BaseType() {} 219 | 220 | /** 221 | * @brief Construct a SEn3TG object from a given group object and vector 222 | * 223 | * @param G Lie group element 224 | * @param g Lie algebra vector 225 | */ 226 | Gal3TG(const typename BaseType::Group& G, const typename BaseType::Group::VectorType& g) : BaseType(G, g) {} 227 | 228 | /** 229 | * @brief Get a constant copy of B (the SE3 subgroup of SE23 composed by the rotational component (R) and the first 230 | * isometry (v)) 231 | * 232 | * @return SE3 group element 233 | */ 234 | [[nodiscard]] const SE3Type B() const { return SE3Type(this->G().q(), {this->G().v()}); } 235 | 236 | /** 237 | * @brief Get a constant copy of C (the SE3 subgroup of SE23 composed by the rotational component (R) and the 238 | * second isometry (p)) 239 | * 240 | * @return SE3 group element 241 | */ 242 | [[nodiscard]] const SE3Type C() const { return SE3Type(this->G().q(), {this->G().p()}); } 243 | }; 244 | 245 | using SE23TGd = SEn3TG; //!< The SE23 tangent group with double precision floating point 246 | using SE23TGf = SEn3TG; //!< The SE23 tangent group with single precision floating point 247 | using Gal3TGd = Gal3TG; //!< The Gal3 tangent group with double precision floating point 248 | using Gal3TGf = Gal3TG; //!< The Gal3 tangent group with single precision floating point 249 | 250 | } // namespace group 251 | 252 | #endif // TG_HPP -------------------------------------------------------------------------------- /include/utils/tools.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // 4 | // All rights reserved. 5 | // 6 | // This software is licensed under the terms of the BSD-2-Clause-License with 7 | // no commercial use allowed, the full terms of which are made available 8 | // in the LICENSE file. No license in patents is granted. 9 | // 10 | // You can contact the authors at 11 | 12 | #ifndef TOOLS_HPP 13 | #define TOOLS_HPP 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace utils 25 | { 26 | /** 27 | * @brief Generate random numbers 28 | * 29 | * @tparam Numeric type of random number generated 30 | * @tparam Generator random number generator 31 | * @param from Lower bound 32 | * @param to Upper bound 33 | * @return Numeric 34 | * 35 | * @note Modified from: 36 | * https://stackoverflow.com/questions/2704521/generate-random-double-numbers-in-c 37 | */ 38 | template 39 | static Numeric random(Numeric from, Numeric to) 40 | { 41 | thread_local static Generator gen(std::random_device{}()); 42 | using dist_type = typename std::conditional::value, std::uniform_int_distribution, 43 | std::uniform_real_distribution>::type; 44 | thread_local static dist_type dist; 45 | return dist(gen, typename dist_type::param_type{from, to}); 46 | } 47 | } // namespace utils 48 | 49 | #endif // TOOLS_HPP 50 | -------------------------------------------------------------------------------- /resources/lie-plusplus-logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aau-cns/Lie-plusplus/bea48ea17b6de1397224cfc4d196a8e49eb9250c/resources/lie-plusplus-logo.png -------------------------------------------------------------------------------- /tests/test_common.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Pieter van Goor. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // System Theory and Robotics Lab, Australian Centre for Robotic 4 | // Vision, Australian national University, Australia. 5 | // 6 | // All rights reserved. 7 | // 8 | // This software is licensed under the terms of the BSD-2-Clause-License with 9 | // no commercial use allowed, the full terms of which are made available 10 | // in the LICENSE file. No license in patents is granted. 11 | // 12 | // You can contact the authors at , 13 | // . 14 | 15 | #ifndef TEST_COMMON_HPP 16 | #define TEST_COMMON_HPP 17 | 18 | #include 19 | #include 20 | 21 | namespace test 22 | { 23 | using fp = double; 24 | 25 | constexpr fp EPS = 1e-4; 26 | constexpr int N_TESTS = 1000; 27 | 28 | template 29 | void MatrixEquality(const Eigen::MatrixBase& A, const Eigen::MatrixBase& B, fp tol = EPS) 30 | { 31 | EXPECT_TRUE(A.isApprox(B, tol) || (A - B).norm() < tol); 32 | } 33 | 34 | template 35 | void QuaternionEquality(const Eigen::Quaternion& a, const Eigen::Quaternion& b, fp tol = EPS) 36 | { 37 | EXPECT_TRUE(a.coeffs().isApprox(b.coeffs(), tol) || a.coeffs().isApprox(-b.coeffs(), tol)); 38 | } 39 | 40 | void ScalarEquality(const fp& a, const fp& b, fp tol = EPS) { EXPECT_TRUE(std::norm(a - b) < tol); } 41 | 42 | } // namespace test 43 | 44 | #endif // TEST_COMMON_HPP -------------------------------------------------------------------------------- /tests/test_groups.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Pieter van Goor. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // System Theory and Robotics Lab, Australian Centre for Robotic 4 | // Vision, Australian national University, Australia. 5 | // 6 | // All rights reserved. 7 | // 8 | // This software is licensed under the terms of the BSD-2-Clause-License with 9 | // no commercial use allowed, the full terms of which are made available 10 | // in the LICENSE file. No license in patents is granted. 11 | // 12 | // You can contact the authors at , 13 | // . 14 | 15 | #ifndef TEST_GROUPS_HPP 16 | #define TEST_GROUPS_HPP 17 | 18 | #include 19 | 20 | #include "groups/In.hpp" 21 | #include "groups/SDB.hpp" 22 | #include "groups/SOT3.hpp" 23 | #include "groups/TG.hpp" 24 | #include "groups/Gal3.hpp" 25 | 26 | #include "utils/tools.hpp" 27 | 28 | namespace test 29 | { 30 | using namespace group; 31 | 32 | typedef testing::Types SO3Groups; 33 | typedef testing::Types SOT3Groups; 34 | typedef testing::Types SE3Groups; 35 | typedef testing::Types SEn3Groups; 36 | typedef testing::Types SDBGroups; 37 | typedef testing::Types SE23TGGroups; 38 | typedef testing::Types Gal3TGGroups; 39 | typedef testing::Types INGroups; 40 | typedef testing::Types Gal3Groups; 41 | 42 | /** 43 | * @brief Inhomogeneous Galileian group specific tests 44 | */ 45 | template 46 | class Gal3GroupsTest : public testing::Test 47 | { 48 | }; 49 | TYPED_TEST_SUITE(Gal3GroupsTest, Gal3Groups); 50 | 51 | TYPED_TEST(Gal3GroupsTest, Gal3Constructors) 52 | { 53 | for (int i = 0; i < N_TESTS; ++i) 54 | { 55 | { 56 | auto X = TypeParam(); 57 | MatrixEquality(X.asMatrix(), TypeParam::MatrixType::Identity()); 58 | MatrixEquality(X.R(), TypeParam::SO3Type::MatrixType::Identity()); 59 | QuaternionEquality(X.q(), TypeParam::SO3Type::QuaternionType::Identity()); 60 | for (const auto& it : X.t()) 61 | { 62 | MatrixEquality(it, TypeParam::SO3Type::VectorType::Zero()); 63 | } 64 | MatrixEquality(X.v(), TypeParam::SO3Type::VectorType::Zero()); 65 | MatrixEquality(X.p(), TypeParam::SO3Type::VectorType::Zero()); 66 | ScalarEquality(X.s(), 0.0); 67 | } 68 | auto q = TypeParam::SO3Type::QuaternionType::UnitRandom(); 69 | typename TypeParam::IsometriesType t; 70 | typename TypeParam::Scalar s = utils::random(0.0, 1.0); 71 | typename TypeParam::MatrixType M = TypeParam::MatrixType::Identity(); 72 | M.template block<3, 3>(0, 0) = q.toRotationMatrix(); 73 | t[0] = TypeParam::SO3Type::VectorType::Random(); 74 | M.template block<3, 1>(0, 3) = t[0]; 75 | t[1] = TypeParam::SO3Type::VectorType::Random(); 76 | M.template block<3, 1>(0, 4) = t[1]; 77 | M(3, 4) = s; 78 | { 79 | auto X = TypeParam(q, t, s); 80 | MatrixEquality(X.asMatrix(), M); 81 | MatrixEquality(X.R(), q.toRotationMatrix()); 82 | QuaternionEquality(X.q(), q); 83 | MatrixEquality(X.t()[0], t[0]); 84 | MatrixEquality(X.t()[1], t[1]); 85 | MatrixEquality(X.v(), t[0]); 86 | MatrixEquality(X.p(), t[1]); 87 | ScalarEquality(X.s(), s); 88 | } 89 | { 90 | auto X = TypeParam(q.toRotationMatrix(), t, s); 91 | MatrixEquality(X.asMatrix(), M); 92 | MatrixEquality(X.R(), q.toRotationMatrix()); 93 | QuaternionEquality(X.q(), q); 94 | MatrixEquality(X.t()[0], t[0]); 95 | MatrixEquality(X.t()[1], t[1]); 96 | MatrixEquality(X.v(), t[0]); 97 | MatrixEquality(X.p(), t[1]); 98 | ScalarEquality(X.s(), s); 99 | } 100 | { 101 | auto X = TypeParam(typename TypeParam::SO3Type(q), t, s); 102 | MatrixEquality(X.asMatrix(), M); 103 | MatrixEquality(X.R(), q.toRotationMatrix()); 104 | QuaternionEquality(X.q(), q); 105 | MatrixEquality(X.t()[0], t[0]); 106 | MatrixEquality(X.t()[1], t[1]); 107 | MatrixEquality(X.v(), t[0]); 108 | MatrixEquality(X.p(), t[1]); 109 | ScalarEquality(X.s(), s); 110 | } 111 | { 112 | auto X = TypeParam(M); 113 | MatrixEquality(X.asMatrix(), M); 114 | MatrixEquality(X.R(), q.toRotationMatrix()); 115 | QuaternionEquality(X.q(), q); 116 | MatrixEquality(X.t()[0], t[0]); 117 | MatrixEquality(X.t()[1], t[1]); 118 | MatrixEquality(X.v(), t[0]); 119 | MatrixEquality(X.p(), t[1]); 120 | ScalarEquality(X.s(), s); 121 | auto Y = X; 122 | MatrixEquality(X.asMatrix(), Y.asMatrix()); 123 | MatrixEquality(X.R(), Y.R()); 124 | QuaternionEquality(X.q(), Y.q()); 125 | MatrixEquality(X.t()[0], Y.t()[0]); 126 | MatrixEquality(X.t()[1], Y.t()[1]); 127 | MatrixEquality(X.v(), Y.v()); 128 | MatrixEquality(X.p(), Y.p()); 129 | ScalarEquality(X.s(), Y.s()); 130 | } 131 | } 132 | } 133 | 134 | TYPED_TEST(Gal3GroupsTest, Gal3Setters) 135 | { 136 | for (int i = 0; i < N_TESTS; ++i) 137 | { 138 | auto X = TypeParam(); 139 | auto q = TypeParam::SO3Type::QuaternionType::UnitRandom(); 140 | typename TypeParam::IsometriesType t; 141 | typename TypeParam::Scalar s = utils::random(0.0, 1.0); 142 | typename TypeParam::MatrixType M = TypeParam::MatrixType::Identity(); 143 | M.template block<3, 3>(0, 0) = q.toRotationMatrix(); 144 | t[0] = TypeParam::SO3Type::VectorType::Random(); 145 | M.template block<3, 1>(0, 3) = t[0]; 146 | t[1] = TypeParam::SO3Type::VectorType::Random(); 147 | M.template block<3, 1>(0, 4) = t[1]; 148 | M(3, 4) = s; 149 | X.fromMatrix(M); 150 | MatrixEquality(X.asMatrix(), M); 151 | MatrixEquality(X.R(), q.toRotationMatrix()); 152 | QuaternionEquality(X.q(), q); 153 | MatrixEquality(X.t()[0], t[0]); 154 | MatrixEquality(X.t()[1], t[1]); 155 | MatrixEquality(X.v(), t[0]); 156 | MatrixEquality(X.p(), t[1]); 157 | ScalarEquality(X.s(), s); 158 | } 159 | } 160 | 161 | TYPED_TEST(Gal3GroupsTest, TestExpLog) 162 | { 163 | for (int i = 0; i < N_TESTS; ++i) 164 | { 165 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 166 | typename TypeParam::VectorType y = TypeParam::log(TypeParam::exp(x)); 167 | MatrixEquality(x, y); 168 | 169 | x = 1e-12 * TypeParam::VectorType::Random(); 170 | y = TypeParam::log(TypeParam::exp(x)); 171 | MatrixEquality(x, y); 172 | 173 | x = TypeParam::VectorType::Random(); 174 | y = TypeParam::log(TypeParam::exp(x)); 175 | MatrixEquality(x, y); 176 | } 177 | } 178 | 179 | TYPED_TEST(Gal3GroupsTest, TestAdjoint) 180 | { 181 | for (int i = 0; i < N_TESTS; ++i) 182 | { 183 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 184 | auto AdEx = TypeParam::exp(x).Adjoint(); 185 | auto adx = TypeParam::adjoint(x); 186 | MatrixEquality(AdEx, adx.exp()); 187 | 188 | x = 1e-12 * TypeParam::VectorType::Random(); 189 | AdEx = TypeParam::exp(x).Adjoint(); 190 | adx = TypeParam::adjoint(x); 191 | MatrixEquality(AdEx, adx.exp()); 192 | 193 | x = TypeParam::VectorType::Random(); 194 | AdEx = TypeParam::exp(x).Adjoint(); 195 | adx = TypeParam::adjoint(x); 196 | MatrixEquality(AdEx, adx.exp()); 197 | } 198 | } 199 | 200 | TYPED_TEST(Gal3GroupsTest, TestLeftJacobian) 201 | { 202 | for (int i = 0; i < N_TESTS; ++i) 203 | { 204 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 205 | auto AdEx = TypeParam::exp(x).Adjoint(); 206 | auto adx = TypeParam::adjoint(x); 207 | auto Jlx = TypeParam::leftJacobian(x); 208 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 209 | 210 | x = 1e-12 * TypeParam::VectorType::Random(); 211 | AdEx = TypeParam::exp(x).Adjoint(); 212 | adx = TypeParam::adjoint(x); 213 | Jlx = TypeParam::leftJacobian(x); 214 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 215 | 216 | x = TypeParam::VectorType::Random(); 217 | AdEx = TypeParam::exp(x).Adjoint(); 218 | adx = TypeParam::adjoint(x); 219 | Jlx = TypeParam::leftJacobian(x); 220 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 221 | 222 | x = 1e-12 * TypeParam::VectorType::Random(); 223 | Jlx = TypeParam::leftJacobian(x); 224 | auto invJlx = TypeParam::invLeftJacobian(x); 225 | MatrixEquality(Jlx * invJlx, TypeParam::TMatrixType::Identity()); 226 | MatrixEquality(invJlx * Jlx, TypeParam::TMatrixType::Identity()); 227 | 228 | x = TypeParam::VectorType::Random(); 229 | Jlx = TypeParam::leftJacobian(x); 230 | invJlx = TypeParam::invLeftJacobian(x); 231 | MatrixEquality(Jlx * invJlx, TypeParam::TMatrixType::Identity()); 232 | MatrixEquality(invJlx * Jlx, TypeParam::TMatrixType::Identity()); 233 | } 234 | } 235 | 236 | TYPED_TEST(Gal3GroupsTest, TestRightJacobian) 237 | { 238 | for (int i = 0; i < N_TESTS; ++i) 239 | { 240 | typename TypeParam::VectorType x = 1e-12 * TypeParam::VectorType::Random(); 241 | auto Jrx = TypeParam::rightJacobian(x); 242 | auto invJrx = TypeParam::invRightJacobian(x); 243 | MatrixEquality(Jrx * invJrx, TypeParam::TMatrixType::Identity()); 244 | MatrixEquality(invJrx * Jrx, TypeParam::TMatrixType::Identity()); 245 | 246 | x = TypeParam::VectorType::Random(); 247 | Jrx = TypeParam::rightJacobian(x); 248 | invJrx = TypeParam::invRightJacobian(x); 249 | MatrixEquality(Jrx * invJrx, TypeParam::TMatrixType::Identity()); 250 | MatrixEquality(invJrx * Jrx, TypeParam::TMatrixType::Identity()); 251 | } 252 | } 253 | 254 | TYPED_TEST(Gal3GroupsTest, TestAssociativity) 255 | { 256 | for (int i = 0; i < N_TESTS; ++i) 257 | { 258 | auto X1 = TypeParam::exp(TypeParam::VectorType::Random()); 259 | auto X2 = TypeParam::exp(TypeParam::VectorType::Random()); 260 | auto X3 = TypeParam::exp(TypeParam::VectorType::Random()); 261 | 262 | auto Z1 = (X1 * X2) * X3; 263 | auto Z2 = X1 * (X2 * X3); 264 | 265 | MatrixEquality(Z1.asMatrix(), Z2.asMatrix()); 266 | } 267 | } 268 | 269 | TYPED_TEST(Gal3GroupsTest, TestIdentity) 270 | { 271 | for (int i = 0; i < N_TESTS; ++i) 272 | { 273 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 274 | auto I = TypeParam(); 275 | typename TypeParam::MatrixType Imat = TypeParam::MatrixType::Identity(); 276 | 277 | MatrixEquality(I.asMatrix(), Imat); 278 | 279 | auto X1 = X * I; 280 | auto X2 = I * X; 281 | 282 | MatrixEquality(X.asMatrix(), X1.asMatrix()); 283 | MatrixEquality(X.asMatrix(), X2.asMatrix()); 284 | } 285 | } 286 | 287 | TYPED_TEST(Gal3GroupsTest, TestInverse) 288 | { 289 | for (int i = 0; i < N_TESTS; ++i) 290 | { 291 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 292 | auto X_inv = X.inv(); 293 | auto I = TypeParam(); 294 | 295 | auto I1 = X * X_inv; 296 | auto I2 = X_inv * X; 297 | 298 | MatrixEquality(I.asMatrix(), I1.asMatrix()); 299 | MatrixEquality(I.asMatrix(), I2.asMatrix()); 300 | } 301 | } 302 | 303 | TYPED_TEST(Gal3GroupsTest, TestGroupProduct) 304 | { 305 | for (int i = 0; i < N_TESTS; ++i) 306 | { 307 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 308 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 309 | 310 | auto Z = X * Y; 311 | 312 | MatrixEquality(Z.asMatrix(), X.asMatrix() * Y.asMatrix()); 313 | } 314 | { 315 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 316 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 317 | 318 | auto Z = X * Y; 319 | auto W = Y * X; 320 | 321 | auto X1 = X; 322 | auto X2 = X; 323 | 324 | X1.multiplyRight(Y); 325 | 326 | MatrixEquality(Z.asMatrix(), X1.asMatrix()); 327 | 328 | X2.multiplyLeft(Y); 329 | 330 | MatrixEquality(W.asMatrix(), X2.asMatrix()); 331 | } 332 | { 333 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 334 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()).asMatrix(); 335 | 336 | auto Z = X * Y; 337 | 338 | MatrixEquality(Z, X.asMatrix() * Y); 339 | } 340 | { 341 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 342 | auto Y = TypeParam::wedge(TypeParam::VectorType::Random()); 343 | 344 | auto Z = X * Y; 345 | 346 | MatrixEquality(Z, X.asMatrix() * Y); 347 | } 348 | } 349 | 350 | /** 351 | * @brief SE23 Tangent group specific tests 352 | */ 353 | template 354 | class SE23TGGroupsTest : public testing::Test 355 | { 356 | }; 357 | TYPED_TEST_SUITE(SE23TGGroupsTest, SE23TGGroups); 358 | 359 | TYPED_TEST(SE23TGGroupsTest, SE23TGGroupsConstructors) 360 | { 361 | for (int i = 0; i < N_TESTS; ++i) 362 | { 363 | { 364 | auto X = TypeParam(); 365 | MatrixEquality(X.G().asMatrix(), TypeParam::Group::MatrixType::Identity()); 366 | MatrixEquality(X.g(), TypeParam::Group::VectorType::Zero()); 367 | } 368 | auto q = TypeParam::Group::SO3Type::QuaternionType::UnitRandom(); 369 | typename TypeParam::Group::IsometriesType t = {TypeParam::Group::SO3Type::VectorType::Random(), 370 | TypeParam::Group::SO3Type::VectorType::Random()}; 371 | typename TypeParam::Group::VectorType g = TypeParam::Group::VectorType::Random(); 372 | typename TypeParam::Group G = typename TypeParam::Group(q, t); 373 | { 374 | auto X = TypeParam(G, g); 375 | MatrixEquality(X.G().asMatrix(), G.asMatrix()); 376 | MatrixEquality(X.g(), g); 377 | auto Y = X; 378 | MatrixEquality(X.G().asMatrix(), Y.G().asMatrix()); 379 | MatrixEquality(X.g(), Y.g()); 380 | } 381 | } 382 | } 383 | 384 | TYPED_TEST(SE23TGGroupsTest, TestExpLog) 385 | { 386 | for (int i = 0; i < N_TESTS; ++i) 387 | { 388 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 389 | typename TypeParam::VectorType y = TypeParam::log(TypeParam::exp(x)); 390 | MatrixEquality(x, y); 391 | 392 | x = 1e-12 * TypeParam::VectorType::Random(); 393 | auto X = TypeParam::exp(x); 394 | y = TypeParam::log(X); 395 | MatrixEquality(x, y); 396 | 397 | x = TypeParam::VectorType::Random(); 398 | y = TypeParam::log(TypeParam::exp(x)); 399 | MatrixEquality(x, y); 400 | 401 | X = TypeParam::exp(x); 402 | 403 | Eigen::Matrix W = Eigen::Matrix::Zero(); 404 | W.block(0, 0, 9, 9) = SEn3::adjoint(x.segment(0, 9)); 405 | W.block(0, 9, 9, 1) = x.segment(9, 9); 406 | 407 | auto E = W.exp(); 408 | 409 | MatrixEquality(X.G().Adjoint(), E.block(0, 0, 9, 9)); 410 | MatrixEquality(X.g(), E.block(0, 9, 9, 1)); 411 | } 412 | } 413 | 414 | TYPED_TEST(SE23TGGroupsTest, TestAssociativity) 415 | { 416 | for (int i = 0; i < N_TESTS; ++i) 417 | { 418 | auto X1 = TypeParam::exp(TypeParam::VectorType::Random()); 419 | auto X2 = TypeParam::exp(TypeParam::VectorType::Random()); 420 | auto X3 = TypeParam::exp(TypeParam::VectorType::Random()); 421 | 422 | auto Z1 = (X1 * X2) * X3; 423 | auto Z2 = X1 * (X2 * X3); 424 | 425 | MatrixEquality(Z1.G().asMatrix(), Z2.G().asMatrix()); 426 | MatrixEquality(Z1.g(), Z2.g()); 427 | } 428 | } 429 | 430 | TYPED_TEST(SE23TGGroupsTest, TestIdentity) 431 | { 432 | for (int i = 0; i < N_TESTS; ++i) 433 | { 434 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 435 | auto I = TypeParam(); 436 | typename TypeParam::Group::MatrixType Imat = TypeParam::Group::MatrixType::Identity(); 437 | 438 | MatrixEquality(I.G().asMatrix(), Imat); 439 | MatrixEquality(I.g(), TypeParam::Group::VectorType::Zero()); 440 | 441 | auto X1 = X * I; 442 | auto X2 = I * X; 443 | 444 | MatrixEquality(X.G().asMatrix(), X1.G().asMatrix()); 445 | MatrixEquality(X.g(), X1.g()); 446 | MatrixEquality(X.G().asMatrix(), X2.G().asMatrix()); 447 | MatrixEquality(X.g(), X2.g()); 448 | } 449 | } 450 | 451 | TYPED_TEST(SE23TGGroupsTest, TestInverse) 452 | { 453 | for (int i = 0; i < N_TESTS; ++i) 454 | { 455 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 456 | auto X_inv = X.inv(); 457 | auto I = TypeParam(); 458 | 459 | auto I1 = X * X_inv; 460 | auto I2 = X_inv * X; 461 | 462 | MatrixEquality(I.G().asMatrix(), I1.G().asMatrix()); 463 | MatrixEquality(I.g(), I1.g()); 464 | MatrixEquality(I.G().asMatrix(), I2.G().asMatrix()); 465 | MatrixEquality(I.g(), I2.g()); 466 | } 467 | } 468 | 469 | TYPED_TEST(SE23TGGroupsTest, TestGroupProduct) 470 | { 471 | for (int i = 0; i < N_TESTS; ++i) 472 | { 473 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 474 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 475 | 476 | auto Z = X * Y; 477 | 478 | MatrixEquality(Z.G().asMatrix(), X.G().asMatrix() * Y.G().asMatrix()); 479 | MatrixEquality(Z.g(), X.g() + X.G().Adjoint() * Y.g()); 480 | } 481 | { 482 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 483 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 484 | 485 | auto Z = X * Y; 486 | auto W = Y * X; 487 | 488 | auto X1 = X; 489 | auto X2 = X; 490 | 491 | X1.multiplyRight(Y); 492 | 493 | MatrixEquality(Z.G().asMatrix(), X1.G().asMatrix()); 494 | MatrixEquality(Z.g(), X1.g()); 495 | 496 | X2.multiplyLeft(Y); 497 | 498 | MatrixEquality(W.G().asMatrix(), X2.G().asMatrix()); 499 | MatrixEquality(W.g(), X2.g()); 500 | } 501 | } 502 | 503 | /** 504 | * @brief Gal3 Tangent group specific tests 505 | */ 506 | template 507 | class Gal3TGGroupsTest : public testing::Test 508 | { 509 | }; 510 | TYPED_TEST_SUITE(Gal3TGGroupsTest, Gal3TGGroups); 511 | 512 | TYPED_TEST(Gal3TGGroupsTest, Gal3TGGroupsConstructors) 513 | { 514 | for (int i = 0; i < N_TESTS; ++i) 515 | { 516 | { 517 | auto X = TypeParam(); 518 | MatrixEquality(X.G().asMatrix(), TypeParam::Group::MatrixType::Identity()); 519 | MatrixEquality(X.g(), TypeParam::Group::VectorType::Zero()); 520 | } 521 | auto q = TypeParam::Group::SO3Type::QuaternionType::UnitRandom(); 522 | typename TypeParam::Group::IsometriesType t = {TypeParam::Group::SO3Type::VectorType::Random(), 523 | TypeParam::Group::SO3Type::VectorType::Random()}; 524 | typename TypeParam::Scalar s = utils::random(0.0, 1.0); 525 | typename TypeParam::Group::VectorType g = TypeParam::Group::VectorType::Random(); 526 | typename TypeParam::Group G = typename TypeParam::Group(q, t, s); 527 | { 528 | auto X = TypeParam(G, g); 529 | MatrixEquality(X.G().asMatrix(), G.asMatrix()); 530 | MatrixEquality(X.g(), g); 531 | auto Y = X; 532 | MatrixEquality(X.G().asMatrix(), Y.G().asMatrix()); 533 | MatrixEquality(X.g(), Y.g()); 534 | } 535 | } 536 | } 537 | 538 | TYPED_TEST(Gal3TGGroupsTest, TestExpLog) 539 | { 540 | for (int i = 0; i < N_TESTS; ++i) 541 | { 542 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 543 | typename TypeParam::VectorType y = TypeParam::log(TypeParam::exp(x)); 544 | MatrixEquality(x, y); 545 | 546 | x = 1e-12 * TypeParam::VectorType::Random(); 547 | auto X = TypeParam::exp(x); 548 | y = TypeParam::log(X); 549 | MatrixEquality(x, y); 550 | 551 | x = TypeParam::VectorType::Random(); 552 | y = TypeParam::log(TypeParam::exp(x)); 553 | MatrixEquality(x, y); 554 | 555 | X = TypeParam::exp(x); 556 | 557 | Eigen::Matrix W = Eigen::Matrix::Zero(); 558 | W.block(0, 0, 10, 10) = Gal3::adjoint(x.segment(0, 10)); 559 | W.block(0, 10, 10, 1) = x.segment(10, 10); 560 | 561 | auto E = W.exp(); 562 | 563 | MatrixEquality(X.G().Adjoint(), E.block(0, 0, 10, 10)); 564 | MatrixEquality(X.g(), E.block(0, 10, 10, 1)); 565 | } 566 | } 567 | 568 | TYPED_TEST(Gal3TGGroupsTest, TestAssociativity) 569 | { 570 | for (int i = 0; i < N_TESTS; ++i) 571 | { 572 | auto X1 = TypeParam::exp(TypeParam::VectorType::Random()); 573 | auto X2 = TypeParam::exp(TypeParam::VectorType::Random()); 574 | auto X3 = TypeParam::exp(TypeParam::VectorType::Random()); 575 | 576 | auto Z1 = (X1 * X2) * X3; 577 | auto Z2 = X1 * (X2 * X3); 578 | 579 | MatrixEquality(Z1.G().asMatrix(), Z2.G().asMatrix()); 580 | MatrixEquality(Z1.g(), Z2.g()); 581 | } 582 | } 583 | 584 | TYPED_TEST(Gal3TGGroupsTest, TestIdentity) 585 | { 586 | for (int i = 0; i < N_TESTS; ++i) 587 | { 588 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 589 | auto I = TypeParam(); 590 | typename TypeParam::Group::MatrixType Imat = TypeParam::Group::MatrixType::Identity(); 591 | 592 | MatrixEquality(I.G().asMatrix(), Imat); 593 | MatrixEquality(I.g(), TypeParam::Group::VectorType::Zero()); 594 | 595 | auto X1 = X * I; 596 | auto X2 = I * X; 597 | 598 | MatrixEquality(X.G().asMatrix(), X1.G().asMatrix()); 599 | MatrixEquality(X.g(), X1.g()); 600 | MatrixEquality(X.G().asMatrix(), X2.G().asMatrix()); 601 | MatrixEquality(X.g(), X2.g()); 602 | } 603 | } 604 | 605 | TYPED_TEST(Gal3TGGroupsTest, TestInverse) 606 | { 607 | for (int i = 0; i < N_TESTS; ++i) 608 | { 609 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 610 | auto X_inv = X.inv(); 611 | auto I = TypeParam(); 612 | 613 | auto I1 = X * X_inv; 614 | auto I2 = X_inv * X; 615 | 616 | MatrixEquality(I.G().asMatrix(), I1.G().asMatrix()); 617 | MatrixEquality(I.g(), I1.g()); 618 | MatrixEquality(I.G().asMatrix(), I2.G().asMatrix()); 619 | MatrixEquality(I.g(), I2.g()); 620 | } 621 | } 622 | 623 | TYPED_TEST(Gal3TGGroupsTest, TestGroupProduct) 624 | { 625 | for (int i = 0; i < N_TESTS; ++i) 626 | { 627 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 628 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 629 | 630 | auto Z = X * Y; 631 | 632 | MatrixEquality(Z.G().asMatrix(), X.G().asMatrix() * Y.G().asMatrix()); 633 | MatrixEquality(Z.g(), X.g() + X.G().Adjoint() * Y.g()); 634 | } 635 | { 636 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 637 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 638 | 639 | auto Z = X * Y; 640 | auto W = Y * X; 641 | 642 | auto X1 = X; 643 | auto X2 = X; 644 | 645 | X1.multiplyRight(Y); 646 | 647 | MatrixEquality(Z.G().asMatrix(), X1.G().asMatrix()); 648 | MatrixEquality(Z.g(), X1.g()); 649 | 650 | X2.multiplyLeft(Y); 651 | 652 | MatrixEquality(W.G().asMatrix(), X2.G().asMatrix()); 653 | MatrixEquality(W.g(), X2.g()); 654 | } 655 | } 656 | 657 | /** 658 | * @brief Semi Direct Bias group specific tests 659 | */ 660 | template 661 | class SDBGroupsTest : public testing::Test 662 | { 663 | }; 664 | TYPED_TEST_SUITE(SDBGroupsTest, SDBGroups); 665 | 666 | TYPED_TEST(SDBGroupsTest, SDBGroupsConstructors) 667 | { 668 | for (int i = 0; i < N_TESTS; ++i) 669 | { 670 | { 671 | auto X = TypeParam(); 672 | MatrixEquality(X.D().asMatrix(), TypeParam::SE23Type::MatrixType::Identity()); 673 | MatrixEquality(X.delta(), TypeParam::Vector6Type::Zero()); 674 | } 675 | auto q = TypeParam::SE23Type::SO3Type::QuaternionType::UnitRandom(); 676 | typename TypeParam::SE23Type::IsometriesType t = {TypeParam::SE23Type::SO3Type::VectorType::Random(), 677 | TypeParam::SE23Type::SO3Type::VectorType::Random()}; 678 | typename TypeParam::Vector6Type delta = TypeParam::Vector6Type::Random(); 679 | { 680 | auto X = TypeParam(typename TypeParam::SE23Type(q, t), delta); 681 | MatrixEquality(X.D().asMatrix(), typename TypeParam::SE23Type(q, t).asMatrix()); 682 | MatrixEquality(X.delta(), delta); 683 | auto Y = X; 684 | MatrixEquality(X.D().asMatrix(), Y.D().asMatrix()); 685 | MatrixEquality(X.delta(), Y.delta()); 686 | } 687 | } 688 | } 689 | 690 | TYPED_TEST(SDBGroupsTest, TestExpLog) 691 | { 692 | for (int i = 0; i < N_TESTS; ++i) 693 | { 694 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 695 | typename TypeParam::VectorType y = TypeParam::log(TypeParam::exp(x)); 696 | MatrixEquality(x, y); 697 | 698 | x = 1e-12 * TypeParam::VectorType::Random(); 699 | auto X = TypeParam::exp(x); 700 | y = TypeParam::log(X); 701 | MatrixEquality(x, y); 702 | 703 | x = TypeParam::VectorType::Random(); 704 | y = TypeParam::log(TypeParam::exp(x)); 705 | MatrixEquality(x, y); 706 | } 707 | } 708 | 709 | TYPED_TEST(SDBGroupsTest, TestAssociativity) 710 | { 711 | for (int i = 0; i < N_TESTS; ++i) 712 | { 713 | auto X1 = TypeParam::exp(TypeParam::VectorType::Random()); 714 | auto X2 = TypeParam::exp(TypeParam::VectorType::Random()); 715 | auto X3 = TypeParam::exp(TypeParam::VectorType::Random()); 716 | 717 | auto Z1 = (X1 * X2) * X3; 718 | auto Z2 = X1 * (X2 * X3); 719 | 720 | MatrixEquality(Z1.D().asMatrix(), Z2.D().asMatrix()); 721 | MatrixEquality(Z1.delta(), Z2.delta()); 722 | } 723 | } 724 | 725 | TYPED_TEST(SDBGroupsTest, TestIdentity) 726 | { 727 | for (int i = 0; i < N_TESTS; ++i) 728 | { 729 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 730 | auto I = TypeParam(); 731 | typename TypeParam::SE23Type::MatrixType Imat = TypeParam::SE23Type::MatrixType::Identity(); 732 | 733 | MatrixEquality(I.D().asMatrix(), Imat); 734 | MatrixEquality(I.delta(), TypeParam::Vector6Type::Zero()); 735 | 736 | auto X1 = X * I; 737 | auto X2 = I * X; 738 | 739 | MatrixEquality(X.D().asMatrix(), X1.D().asMatrix()); 740 | MatrixEquality(X.delta(), X1.delta()); 741 | MatrixEquality(X.D().asMatrix(), X2.D().asMatrix()); 742 | MatrixEquality(X.delta(), X2.delta()); 743 | } 744 | } 745 | 746 | TYPED_TEST(SDBGroupsTest, TestInverse) 747 | { 748 | for (int i = 0; i < N_TESTS; ++i) 749 | { 750 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 751 | auto X_inv = X.inv(); 752 | auto I = TypeParam(); 753 | 754 | auto I1 = X * X_inv; 755 | auto I2 = X_inv * X; 756 | 757 | MatrixEquality(I.D().asMatrix(), I1.D().asMatrix()); 758 | MatrixEquality(I.delta(), I1.delta()); 759 | MatrixEquality(I.D().asMatrix(), I2.D().asMatrix()); 760 | MatrixEquality(I.delta(), I2.delta()); 761 | } 762 | } 763 | 764 | TYPED_TEST(SDBGroupsTest, TestGroupProduct) 765 | { 766 | for (int i = 0; i < N_TESTS; ++i) 767 | { 768 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 769 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 770 | 771 | auto Z = X * Y; 772 | 773 | MatrixEquality(Z.D().asMatrix(), X.D().asMatrix() * Y.D().asMatrix()); 774 | MatrixEquality(Z.delta(), X.delta() + X.B().Adjoint() * Y.delta()); 775 | } 776 | { 777 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 778 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 779 | 780 | auto Z = X * Y; 781 | auto W = Y * X; 782 | 783 | auto X1 = X; 784 | auto X2 = X; 785 | 786 | X1.multiplyRight(Y); 787 | 788 | MatrixEquality(Z.D().asMatrix(), X1.D().asMatrix()); 789 | MatrixEquality(Z.delta(), X1.delta()); 790 | 791 | X2.multiplyLeft(Y); 792 | 793 | MatrixEquality(W.D().asMatrix(), X2.D().asMatrix()); 794 | MatrixEquality(W.delta(), X2.delta()); 795 | } 796 | } 797 | 798 | /** 799 | * @brief Intrinsic group specific tests 800 | */ 801 | template 802 | class InGroupsTest : public testing::Test 803 | { 804 | }; 805 | TYPED_TEST_SUITE(InGroupsTest, INGroups); 806 | 807 | TYPED_TEST(InGroupsTest, InGroupsConstructors) 808 | { 809 | for (int i = 0; i < N_TESTS; ++i) 810 | { 811 | { 812 | auto X = TypeParam(); 813 | MatrixEquality(X.K(), TypeParam::MatrixType::Identity()); 814 | } 815 | fp fx = utils::random(400, 800); 816 | fp fy = utils::random(400, 800); 817 | fp cx = utils::random(200, 400); 818 | fp cy = utils::random(200, 400); 819 | typename TypeParam::MatrixType L; 820 | L << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; 821 | { 822 | auto X = TypeParam(L); 823 | MatrixEquality(X.K(), L); 824 | } 825 | { 826 | typename TypeParam::VectorType l; 827 | l << fx, fy, cx, cy; 828 | auto X = TypeParam(l); 829 | MatrixEquality(X.K(), L); 830 | } 831 | { 832 | auto X = TypeParam(fx, fy, cx, cy); 833 | MatrixEquality(X.K(), L); 834 | auto Y = X; 835 | MatrixEquality(X.K(), Y.K()); 836 | } 837 | } 838 | } 839 | 840 | TYPED_TEST(InGroupsTest, InAction) 841 | { 842 | for (int i = 0; i < N_TESTS; ++i) 843 | { 844 | fp fx = utils::random(400, 800); 845 | fp fy = utils::random(400, 800); 846 | fp cx = utils::random(200, 400); 847 | fp cy = utils::random(200, 400); 848 | typename TypeParam::Vector3Type x = TypeParam::Vector3Type::Random(); 849 | auto X = TypeParam(fx, fy, cx, cy); 850 | MatrixEquality(X * x, X.K() * x); 851 | } 852 | } 853 | 854 | TYPED_TEST(InGroupsTest, TestGroupProduct) 855 | { 856 | for (int i = 0; i < N_TESTS; ++i) 857 | { 858 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 859 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 860 | 861 | auto Z = X * Y; 862 | 863 | MatrixEquality(Z.asMatrix(), X.asMatrix() * Y.asMatrix()); 864 | } 865 | { 866 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 867 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 868 | 869 | auto Z = X * Y; 870 | auto W = Y * X; 871 | 872 | auto X1 = X; 873 | auto X2 = X; 874 | 875 | X1.multiplyRight(Y); 876 | 877 | MatrixEquality(Z.asMatrix(), X1.asMatrix()); 878 | 879 | X2.multiplyLeft(Y); 880 | 881 | MatrixEquality(W.asMatrix(), X2.asMatrix()); 882 | } 883 | { 884 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 885 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()).asMatrix(); 886 | 887 | auto Z = X * Y; 888 | 889 | MatrixEquality(Z, X.asMatrix() * Y); 890 | } 891 | { 892 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 893 | auto Y = TypeParam::wedge(TypeParam::VectorType::Random()); 894 | 895 | auto Z = X * Y; 896 | 897 | MatrixEquality(Z, X.asMatrix() * Y); 898 | } 899 | } 900 | 901 | /** 902 | * @brief SO3 specific tests 903 | */ 904 | template 905 | class SO3GroupsTest : public testing::Test 906 | { 907 | }; 908 | TYPED_TEST_SUITE(SO3GroupsTest, SO3Groups); 909 | 910 | TYPED_TEST(SO3GroupsTest, SO3Constructors) 911 | { 912 | for (int i = 0; i < N_TESTS; ++i) 913 | { 914 | { 915 | auto X = TypeParam(); 916 | MatrixEquality(X.R(), TypeParam::MatrixType::Identity()); 917 | QuaternionEquality(X.q(), TypeParam::QuaternionType::Identity()); 918 | } 919 | typename TypeParam::QuaternionType q = TypeParam::QuaternionType::UnitRandom(); 920 | { 921 | auto X = TypeParam(q); 922 | MatrixEquality(X.R(), q.toRotationMatrix()); 923 | QuaternionEquality(X.q(), q); 924 | } 925 | { 926 | auto X = TypeParam(q.toRotationMatrix()); 927 | MatrixEquality(X.R(), q.toRotationMatrix()); 928 | QuaternionEquality(X.q(), q); 929 | auto Y = X; 930 | MatrixEquality(X.R(), Y.R()); 931 | QuaternionEquality(X.q(), Y.q()); 932 | } 933 | { 934 | typename TypeParam::VectorType u = TypeParam::VectorType::Random(); 935 | 936 | typename TypeParam::VectorType axis = u.cross(TypeParam::VectorType::Random()); 937 | axis /= axis.norm(); 938 | 939 | fp ang = utils::random(0.0, M_PI); 940 | 941 | q = TypeParam::exp(ang * axis).q(); 942 | 943 | typename TypeParam::VectorType v = q.toRotationMatrix() * u; 944 | auto X = TypeParam(u, v); 945 | 946 | MatrixEquality(X * u, v); 947 | } 948 | } 949 | } 950 | 951 | TYPED_TEST(SO3GroupsTest, SO3Setters) 952 | { 953 | for (int i = 0; i < N_TESTS; ++i) 954 | { 955 | auto X = TypeParam(); 956 | auto q = TypeParam::QuaternionType::UnitRandom(); 957 | X.fromq(q); 958 | MatrixEquality(X.R(), q.toRotationMatrix()); 959 | QuaternionEquality(X.q(), q); 960 | q = TypeParam::QuaternionType::UnitRandom(); 961 | X.fromR(q.toRotationMatrix()); 962 | MatrixEquality(X.R(), q.toRotationMatrix()); 963 | QuaternionEquality(X.q(), q); 964 | } 965 | } 966 | 967 | TYPED_TEST(SO3GroupsTest, SO3Action) 968 | { 969 | for (int i = 0; i < N_TESTS; ++i) 970 | { 971 | auto q = TypeParam::QuaternionType::UnitRandom(); 972 | auto X = TypeParam(q); 973 | typename TypeParam::VectorType x = TypeParam::VectorType::Random(); 974 | MatrixEquality(X * x, q.toRotationMatrix() * x); 975 | } 976 | } 977 | 978 | TYPED_TEST(SO3GroupsTest, TestGroupProduct) 979 | { 980 | for (int i = 0; i < N_TESTS; ++i) 981 | { 982 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 983 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 984 | 985 | auto Z = X * Y; 986 | 987 | MatrixEquality(Z.asMatrix(), X.asMatrix() * Y.asMatrix()); 988 | } 989 | { 990 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 991 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 992 | 993 | auto Z = X * Y; 994 | auto W = Y * X; 995 | 996 | auto X1 = X; 997 | auto X2 = X; 998 | 999 | X1.multiplyRight(Y); 1000 | 1001 | MatrixEquality(Z.asMatrix(), X1.asMatrix()); 1002 | 1003 | X2.multiplyLeft(Y); 1004 | 1005 | MatrixEquality(W.asMatrix(), X2.asMatrix()); 1006 | } 1007 | { 1008 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1009 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()).asMatrix(); 1010 | 1011 | auto Z = X * Y; 1012 | 1013 | MatrixEquality(Z, X.asMatrix() * Y); 1014 | } 1015 | { 1016 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1017 | auto Y = TypeParam::wedge(TypeParam::VectorType::Random()); 1018 | 1019 | auto Z = X * Y; 1020 | 1021 | MatrixEquality(Z, X.asMatrix() * Y); 1022 | } 1023 | } 1024 | 1025 | TYPED_TEST(SO3GroupsTest, TestLeftJacobian) 1026 | { 1027 | for (int i = 0; i < N_TESTS; ++i) 1028 | { 1029 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 1030 | auto AdEx = TypeParam::exp(x).Adjoint(); 1031 | auto adx = TypeParam::adjoint(x); 1032 | auto Jlx = TypeParam::leftJacobian(x); 1033 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 1034 | 1035 | x = 1e-12 * TypeParam::VectorType::Random(); 1036 | AdEx = TypeParam::exp(x).Adjoint(); 1037 | adx = TypeParam::adjoint(x); 1038 | Jlx = TypeParam::leftJacobian(x); 1039 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 1040 | 1041 | x = TypeParam::VectorType::Random(); 1042 | AdEx = TypeParam::exp(x).Adjoint(); 1043 | adx = TypeParam::adjoint(x); 1044 | Jlx = TypeParam::leftJacobian(x); 1045 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 1046 | 1047 | x = 1e-12 * TypeParam::VectorType::Random(); 1048 | Jlx = TypeParam::leftJacobian(x); 1049 | auto invJlx = TypeParam::invLeftJacobian(x); 1050 | MatrixEquality(Jlx * invJlx, TypeParam::TMatrixType::Identity()); 1051 | MatrixEquality(invJlx * Jlx, TypeParam::TMatrixType::Identity()); 1052 | 1053 | x = TypeParam::VectorType::Random(); 1054 | Jlx = TypeParam::leftJacobian(x); 1055 | invJlx = TypeParam::invLeftJacobian(x); 1056 | MatrixEquality(Jlx * invJlx, TypeParam::TMatrixType::Identity()); 1057 | MatrixEquality(invJlx * Jlx, TypeParam::TMatrixType::Identity()); 1058 | } 1059 | } 1060 | 1061 | TYPED_TEST(SO3GroupsTest, TestRightJacobian) 1062 | { 1063 | for (int i = 0; i < N_TESTS; ++i) 1064 | { 1065 | typename TypeParam::VectorType x = 1e-12 * TypeParam::VectorType::Random(); 1066 | auto Jrx = TypeParam::rightJacobian(x); 1067 | auto invJrx = TypeParam::invRightJacobian(x); 1068 | MatrixEquality(Jrx * invJrx, TypeParam::TMatrixType::Identity()); 1069 | MatrixEquality(invJrx * Jrx, TypeParam::TMatrixType::Identity()); 1070 | 1071 | x = TypeParam::VectorType::Random(); 1072 | Jrx = TypeParam::rightJacobian(x); 1073 | invJrx = TypeParam::invRightJacobian(x); 1074 | MatrixEquality(Jrx * invJrx, TypeParam::TMatrixType::Identity()); 1075 | MatrixEquality(invJrx * Jrx, TypeParam::TMatrixType::Identity()); 1076 | } 1077 | } 1078 | 1079 | /** 1080 | * @brief SOT3 specific tests 1081 | */ 1082 | template 1083 | class SOT3GroupsTest : public testing::Test 1084 | { 1085 | }; 1086 | TYPED_TEST_SUITE(SOT3GroupsTest, SOT3Groups); 1087 | 1088 | TYPED_TEST(SOT3GroupsTest, SOT3Constructors) 1089 | { 1090 | for (int i = 0; i < N_TESTS; ++i) 1091 | { 1092 | { 1093 | auto X = TypeParam(); 1094 | MatrixEquality(X.Q(), TypeParam::MatrixType::Identity()); 1095 | MatrixEquality(X.R(), TypeParam::SO3Type::MatrixType::Identity()); 1096 | QuaternionEquality(X.q(), TypeParam::SO3Type::QuaternionType::Identity()); 1097 | ScalarEquality(X.s(), 1.0); 1098 | } 1099 | typename TypeParam::SO3Type::QuaternionType q = TypeParam::SO3Type::QuaternionType::UnitRandom(); 1100 | fp s = utils::random(0.1, 10); 1101 | typename TypeParam::MatrixType Q = TypeParam::MatrixType::Identity(); 1102 | Q.template block<3, 3>(0, 0) = q.toRotationMatrix(); 1103 | Q(3, 3) = s; 1104 | { 1105 | auto X = TypeParam(q, s); 1106 | MatrixEquality(X.Q(), Q); 1107 | MatrixEquality(X.R(), q.toRotationMatrix()); 1108 | QuaternionEquality(X.q(), q); 1109 | ScalarEquality(X.s(), s); 1110 | } 1111 | { 1112 | auto X = TypeParam(q.toRotationMatrix(), s); 1113 | MatrixEquality(X.Q(), Q); 1114 | MatrixEquality(X.R(), q.toRotationMatrix()); 1115 | QuaternionEquality(X.q(), q); 1116 | ScalarEquality(X.s(), s); 1117 | } 1118 | { 1119 | auto X = TypeParam(typename TypeParam::SO3Type(q), s); 1120 | MatrixEquality(X.Q(), Q); 1121 | MatrixEquality(X.R(), q.toRotationMatrix()); 1122 | QuaternionEquality(X.q(), q); 1123 | ScalarEquality(X.s(), s); 1124 | } 1125 | { 1126 | auto X = TypeParam(Q); 1127 | MatrixEquality(X.Q(), Q); 1128 | MatrixEquality(X.R(), q.toRotationMatrix()); 1129 | QuaternionEquality(X.q(), q); 1130 | ScalarEquality(X.s(), s); 1131 | auto Y = X; 1132 | MatrixEquality(X.Q(), Y.Q()); 1133 | MatrixEquality(X.R(), Y.R()); 1134 | QuaternionEquality(X.q(), Y.q()); 1135 | ScalarEquality(X.s(), X.s()); 1136 | } 1137 | { 1138 | typename TypeParam::SO3Type::VectorType u = TypeParam::SO3Type::VectorType::Random(); 1139 | 1140 | typename TypeParam::SO3Type::VectorType axis = u.cross(TypeParam::SO3Type::VectorType::Random()); 1141 | axis /= axis.norm(); 1142 | 1143 | fp ang = utils::random(0.0, M_PI); 1144 | 1145 | q = TypeParam::SO3Type::exp(ang * axis).q(); 1146 | Q.template block<3, 3>(0, 0) = q.toRotationMatrix(); 1147 | 1148 | typename TypeParam::SO3Type::VectorType v = s * (q.toRotationMatrix() * u); 1149 | auto X = TypeParam(u, v); 1150 | 1151 | MatrixEquality(X.s() * X.R() * u, v); 1152 | } 1153 | } 1154 | } 1155 | 1156 | TYPED_TEST(SOT3GroupsTest, SOT3Setters) 1157 | { 1158 | for (int i = 0; i < N_TESTS; ++i) 1159 | { 1160 | auto X = TypeParam(); 1161 | auto q = TypeParam::SO3Type::QuaternionType::UnitRandom(); 1162 | fp s = utils::random(0.1, 10); 1163 | typename TypeParam::MatrixType Q = TypeParam::MatrixType::Identity(); 1164 | Q.template block<3, 3>(0, 0) = q.toRotationMatrix(); 1165 | Q(3, 3) = s; 1166 | X.fromQ(Q); 1167 | MatrixEquality(X.Q(), Q); 1168 | MatrixEquality(X.R(), q.toRotationMatrix()); 1169 | QuaternionEquality(X.q(), q); 1170 | ScalarEquality(X.s(), s); 1171 | } 1172 | } 1173 | 1174 | TYPED_TEST(SOT3GroupsTest, SOT3Action) 1175 | { 1176 | for (int i = 0; i < N_TESTS; ++i) 1177 | { 1178 | auto q = TypeParam::SO3Type::QuaternionType::UnitRandom(); 1179 | fp s = utils::random(0.1, 10); 1180 | auto X = TypeParam(q, s); 1181 | typename TypeParam::SO3Type::VectorType x = TypeParam::SO3Type::VectorType::Random(); 1182 | MatrixEquality(X * x, s * q.toRotationMatrix() * x); 1183 | } 1184 | } 1185 | 1186 | TYPED_TEST(SOT3GroupsTest, TestGroupProduct) 1187 | { 1188 | for (int i = 0; i < N_TESTS; ++i) 1189 | { 1190 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1191 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 1192 | 1193 | auto Z = X * Y; 1194 | 1195 | MatrixEquality(Z.asMatrix(), X.asMatrix() * Y.asMatrix()); 1196 | } 1197 | { 1198 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1199 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 1200 | 1201 | auto Z = X * Y; 1202 | auto W = Y * X; 1203 | 1204 | auto X1 = X; 1205 | auto X2 = X; 1206 | 1207 | X1.multiplyRight(Y); 1208 | 1209 | MatrixEquality(Z.asMatrix(), X1.asMatrix()); 1210 | 1211 | X2.multiplyLeft(Y); 1212 | 1213 | MatrixEquality(W.asMatrix(), X2.asMatrix()); 1214 | } 1215 | { 1216 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1217 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()).asMatrix(); 1218 | 1219 | auto Z = X * Y; 1220 | 1221 | MatrixEquality(Z, X.asMatrix() * Y); 1222 | } 1223 | { 1224 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1225 | auto Y = TypeParam::wedge(TypeParam::VectorType::Random()); 1226 | 1227 | auto Z = X * Y; 1228 | 1229 | MatrixEquality(Z, X.asMatrix() * Y); 1230 | } 1231 | } 1232 | 1233 | TYPED_TEST(SOT3GroupsTest, TestLeftJacobian) 1234 | { 1235 | for (int i = 0; i < N_TESTS; ++i) 1236 | { 1237 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 1238 | auto AdEx = TypeParam::exp(x).Adjoint(); 1239 | auto adx = TypeParam::adjoint(x); 1240 | auto Jlx = TypeParam::leftJacobian(x); 1241 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 1242 | 1243 | x = 1e-12 * TypeParam::VectorType::Random(); 1244 | AdEx = TypeParam::exp(x).Adjoint(); 1245 | adx = TypeParam::adjoint(x); 1246 | Jlx = TypeParam::leftJacobian(x); 1247 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 1248 | 1249 | x = TypeParam::VectorType::Random(); 1250 | AdEx = TypeParam::exp(x).Adjoint(); 1251 | adx = TypeParam::adjoint(x); 1252 | Jlx = TypeParam::leftJacobian(x); 1253 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 1254 | 1255 | x = 1e-12 * TypeParam::VectorType::Random(); 1256 | Jlx = TypeParam::leftJacobian(x); 1257 | auto invJlx = TypeParam::invLeftJacobian(x); 1258 | MatrixEquality(Jlx * invJlx, TypeParam::TMatrixType::Identity()); 1259 | MatrixEquality(invJlx * Jlx, TypeParam::TMatrixType::Identity()); 1260 | 1261 | x = TypeParam::VectorType::Random(); 1262 | Jlx = TypeParam::leftJacobian(x); 1263 | invJlx = TypeParam::invLeftJacobian(x); 1264 | MatrixEquality(Jlx * invJlx, TypeParam::TMatrixType::Identity()); 1265 | MatrixEquality(invJlx * Jlx, TypeParam::TMatrixType::Identity()); 1266 | } 1267 | } 1268 | 1269 | TYPED_TEST(SOT3GroupsTest, TestRightJacobian) 1270 | { 1271 | for (int i = 0; i < N_TESTS; ++i) 1272 | { 1273 | typename TypeParam::VectorType x = 1e-12 * TypeParam::VectorType::Random(); 1274 | auto Jrx = TypeParam::rightJacobian(x); 1275 | auto invJrx = TypeParam::invRightJacobian(x); 1276 | MatrixEquality(Jrx * invJrx, TypeParam::TMatrixType::Identity()); 1277 | MatrixEquality(invJrx * Jrx, TypeParam::TMatrixType::Identity()); 1278 | 1279 | x = TypeParam::VectorType::Random(); 1280 | Jrx = TypeParam::rightJacobian(x); 1281 | invJrx = TypeParam::invRightJacobian(x); 1282 | MatrixEquality(Jrx * invJrx, TypeParam::TMatrixType::Identity()); 1283 | MatrixEquality(invJrx * Jrx, TypeParam::TMatrixType::Identity()); 1284 | } 1285 | } 1286 | 1287 | /** 1288 | * @brief SEn3 specific tests 1289 | */ 1290 | template 1291 | class SEn3GroupsTest : public testing::Test 1292 | { 1293 | }; 1294 | TYPED_TEST_SUITE(SEn3GroupsTest, SEn3Groups); 1295 | 1296 | TYPED_TEST(SEn3GroupsTest, SEn3Constructors) 1297 | { 1298 | for (int i = 0; i < N_TESTS; ++i) 1299 | { 1300 | int n = TypeParam().t().size(); 1301 | { 1302 | auto X = TypeParam(); 1303 | MatrixEquality(X.T(), TypeParam::MatrixType::Identity()); 1304 | MatrixEquality(X.R(), TypeParam::SO3Type::MatrixType::Identity()); 1305 | QuaternionEquality(X.q(), TypeParam::SO3Type::QuaternionType::Identity()); 1306 | for (const auto& it : X.t()) 1307 | { 1308 | MatrixEquality(it, TypeParam::SO3Type::VectorType::Zero()); 1309 | } 1310 | } 1311 | auto q = TypeParam::SO3Type::QuaternionType::UnitRandom(); 1312 | typename TypeParam::IsometriesType t; 1313 | typename TypeParam::MatrixType T = TypeParam::MatrixType::Identity(); 1314 | T.template block<3, 3>(0, 0) = q.toRotationMatrix(); 1315 | for (int i = 0; i < n; ++i) 1316 | { 1317 | t[i] = TypeParam::SO3Type::VectorType::Random(); 1318 | T.template block<3, 1>(0, 3 + i) = t[i]; 1319 | } 1320 | { 1321 | auto X = TypeParam(q, t); 1322 | MatrixEquality(X.T(), T); 1323 | MatrixEquality(X.R(), q.toRotationMatrix()); 1324 | QuaternionEquality(X.q(), q); 1325 | for (int i = 0; i < n; ++i) 1326 | { 1327 | MatrixEquality(X.t()[i], t[i]); 1328 | } 1329 | } 1330 | { 1331 | auto X = TypeParam(q.toRotationMatrix(), t); 1332 | MatrixEquality(X.T(), T); 1333 | MatrixEquality(X.R(), q.toRotationMatrix()); 1334 | QuaternionEquality(X.q(), q); 1335 | for (int i = 0; i < n; ++i) 1336 | { 1337 | MatrixEquality(X.t()[i], t[i]); 1338 | } 1339 | } 1340 | { 1341 | auto X = TypeParam(typename TypeParam::SO3Type(q), t); 1342 | MatrixEquality(X.T(), T); 1343 | MatrixEquality(X.R(), q.toRotationMatrix()); 1344 | QuaternionEquality(X.q(), q); 1345 | for (int i = 0; i < n; ++i) 1346 | { 1347 | MatrixEquality(X.t()[i], t[i]); 1348 | } 1349 | } 1350 | { 1351 | auto X = TypeParam(T); 1352 | MatrixEquality(X.T(), T); 1353 | MatrixEquality(X.R(), q.toRotationMatrix()); 1354 | QuaternionEquality(X.q(), q); 1355 | for (int i = 0; i < n; ++i) 1356 | { 1357 | MatrixEquality(X.t()[i], t[i]); 1358 | } 1359 | auto Y = X; 1360 | MatrixEquality(X.T(), Y.T()); 1361 | MatrixEquality(X.R(), Y.R()); 1362 | QuaternionEquality(X.q(), Y.q()); 1363 | for (int i = 0; i < n; ++i) 1364 | { 1365 | MatrixEquality(X.t()[i], Y.t()[i]); 1366 | } 1367 | } 1368 | } 1369 | } 1370 | 1371 | TYPED_TEST(SEn3GroupsTest, SEn3Setters) 1372 | { 1373 | for (int i = 0; i < N_TESTS; ++i) 1374 | { 1375 | int n = TypeParam().t().size(); 1376 | auto X = TypeParam(); 1377 | auto q = TypeParam::SO3Type::QuaternionType::UnitRandom(); 1378 | typename TypeParam::IsometriesType t; 1379 | typename TypeParam::MatrixType T = TypeParam::MatrixType::Identity(); 1380 | T.template block<3, 3>(0, 0) = q.toRotationMatrix(); 1381 | for (int i = 0; i < n; ++i) 1382 | { 1383 | t[i] = TypeParam::SO3Type::VectorType::Random(); 1384 | T.template block<3, 1>(0, 3 + i) = t[i]; 1385 | } 1386 | X.fromT(T); 1387 | MatrixEquality(X.T(), T); 1388 | MatrixEquality(X.R(), q.toRotationMatrix()); 1389 | QuaternionEquality(X.q(), q); 1390 | for (int i = 0; i < n; ++i) 1391 | { 1392 | MatrixEquality(X.t()[i], t[i]); 1393 | } 1394 | } 1395 | } 1396 | 1397 | TYPED_TEST(SEn3GroupsTest, TestGroupProduct) 1398 | { 1399 | for (int i = 0; i < N_TESTS; ++i) 1400 | { 1401 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1402 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 1403 | 1404 | auto Z = X * Y; 1405 | 1406 | MatrixEquality(Z.asMatrix(), X.asMatrix() * Y.asMatrix()); 1407 | } 1408 | { 1409 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1410 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()); 1411 | 1412 | auto Z = X * Y; 1413 | auto W = Y * X; 1414 | 1415 | auto X1 = X; 1416 | auto X2 = X; 1417 | 1418 | X1.multiplyRight(Y); 1419 | 1420 | MatrixEquality(Z.asMatrix(), X1.asMatrix()); 1421 | 1422 | X2.multiplyLeft(Y); 1423 | 1424 | MatrixEquality(W.asMatrix(), X2.asMatrix()); 1425 | } 1426 | { 1427 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1428 | auto Y = TypeParam::exp(TypeParam::VectorType::Random()).asMatrix(); 1429 | 1430 | auto Z = X * Y; 1431 | 1432 | MatrixEquality(Z, X.asMatrix() * Y); 1433 | } 1434 | { 1435 | auto X = TypeParam::exp(TypeParam::VectorType::Random()); 1436 | auto Y = TypeParam::wedge(TypeParam::VectorType::Random()); 1437 | 1438 | auto Z = X * Y; 1439 | 1440 | MatrixEquality(Z, X.asMatrix() * Y); 1441 | } 1442 | } 1443 | 1444 | TYPED_TEST(SEn3GroupsTest, TestExpLog) 1445 | { 1446 | for (int i = 0; i < N_TESTS; ++i) 1447 | { 1448 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 1449 | typename TypeParam::VectorType y = TypeParam::log(TypeParam::exp(x)); 1450 | MatrixEquality(x, y); 1451 | 1452 | x = 1e-12 * TypeParam::VectorType::Random(); 1453 | y = TypeParam::log(TypeParam::exp(x)); 1454 | MatrixEquality(x, y); 1455 | 1456 | x = TypeParam::VectorType::Random(); 1457 | y = TypeParam::log(TypeParam::exp(x)); 1458 | MatrixEquality(x, y); 1459 | } 1460 | } 1461 | 1462 | TYPED_TEST(SEn3GroupsTest, TestAdjoint) 1463 | { 1464 | for (int i = 0; i < N_TESTS; ++i) 1465 | { 1466 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 1467 | auto AdEx = TypeParam::exp(x).Adjoint(); 1468 | auto adx = TypeParam::adjoint(x); 1469 | MatrixEquality(AdEx, adx.exp()); 1470 | 1471 | x = 1e-12 * TypeParam::VectorType::Random(); 1472 | AdEx = TypeParam::exp(x).Adjoint(); 1473 | adx = TypeParam::adjoint(x); 1474 | MatrixEquality(AdEx, adx.exp()); 1475 | 1476 | x = TypeParam::VectorType::Random(); 1477 | AdEx = TypeParam::exp(x).Adjoint(); 1478 | adx = TypeParam::adjoint(x); 1479 | MatrixEquality(AdEx, adx.exp()); 1480 | } 1481 | } 1482 | 1483 | TYPED_TEST(SEn3GroupsTest, TestLeftJacobian) 1484 | { 1485 | for (int i = 0; i < N_TESTS; ++i) 1486 | { 1487 | typename TypeParam::VectorType x = TypeParam::VectorType::Zero(); 1488 | auto AdEx = TypeParam::exp(x).Adjoint(); 1489 | auto adx = TypeParam::adjoint(x); 1490 | auto Jlx = TypeParam::leftJacobian(x); 1491 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 1492 | 1493 | x = 1e-12 * TypeParam::VectorType::Random(); 1494 | AdEx = TypeParam::exp(x).Adjoint(); 1495 | adx = TypeParam::adjoint(x); 1496 | Jlx = TypeParam::leftJacobian(x); 1497 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 1498 | 1499 | x = TypeParam::VectorType::Random(); 1500 | AdEx = TypeParam::exp(x).Adjoint(); 1501 | adx = TypeParam::adjoint(x); 1502 | Jlx = TypeParam::leftJacobian(x); 1503 | MatrixEquality(AdEx, TypeParam::TMatrixType::Identity() + adx * Jlx); 1504 | 1505 | x = 1e-12 * TypeParam::VectorType::Random(); 1506 | Jlx = TypeParam::leftJacobian(x); 1507 | auto invJlx = TypeParam::invLeftJacobian(x); 1508 | MatrixEquality(Jlx * invJlx, TypeParam::TMatrixType::Identity()); 1509 | MatrixEquality(invJlx * Jlx, TypeParam::TMatrixType::Identity()); 1510 | 1511 | x = TypeParam::VectorType::Random(); 1512 | Jlx = TypeParam::leftJacobian(x); 1513 | invJlx = TypeParam::invLeftJacobian(x); 1514 | MatrixEquality(Jlx * invJlx, TypeParam::TMatrixType::Identity()); 1515 | MatrixEquality(invJlx * Jlx, TypeParam::TMatrixType::Identity()); 1516 | } 1517 | } 1518 | 1519 | TYPED_TEST(SEn3GroupsTest, TestRightJacobian) 1520 | { 1521 | for (int i = 0; i < N_TESTS; ++i) 1522 | { 1523 | typename TypeParam::VectorType x = 1e-12 * TypeParam::VectorType::Random(); 1524 | auto Jrx = TypeParam::rightJacobian(x); 1525 | auto invJrx = TypeParam::invRightJacobian(x); 1526 | MatrixEquality(Jrx * invJrx, TypeParam::TMatrixType::Identity()); 1527 | MatrixEquality(invJrx * Jrx, TypeParam::TMatrixType::Identity()); 1528 | 1529 | x = TypeParam::VectorType::Random(); 1530 | Jrx = TypeParam::rightJacobian(x); 1531 | invJrx = TypeParam::invRightJacobian(x); 1532 | MatrixEquality(Jrx * invJrx, TypeParam::TMatrixType::Identity()); 1533 | MatrixEquality(invJrx * Jrx, TypeParam::TMatrixType::Identity()); 1534 | } 1535 | } 1536 | 1537 | /** 1538 | * @brief SE3 specific tests 1539 | */ 1540 | template 1541 | class SE3ActionTest : public testing::Test 1542 | { 1543 | }; 1544 | TYPED_TEST_SUITE(SE3ActionTest, SE3Groups); 1545 | 1546 | TYPED_TEST(SE3ActionTest, SE3Action) 1547 | { 1548 | for (int i = 0; i < N_TESTS; ++i) 1549 | { 1550 | auto q = TypeParam::SO3Type::QuaternionType::UnitRandom(); 1551 | typename TypeParam::SO3Type::VectorType t = TypeParam::SO3Type::VectorType::Random(); 1552 | auto X = TypeParam(q, {t}); 1553 | typename TypeParam::SO3Type::VectorType x = TypeParam::SO3Type::VectorType::Random(); 1554 | MatrixEquality(X * x, q.toRotationMatrix() * x + t); 1555 | } 1556 | } 1557 | 1558 | } // namespace test 1559 | 1560 | #endif // TEST_GROUPS_HPP -------------------------------------------------------------------------------- /tests/tests.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2023 Alessandro Fornasier, Pieter van Goor. 2 | // Control of Networked Systems, University of Klagenfurt, Austria. 3 | // System Theory and Robotics Lab, Australian Centre for Robotic 4 | // Vision, Australian national University, Australia. 5 | // 6 | // All rights reserved. 7 | // 8 | // This software is licensed under the terms of the BSD-2-Clause-License with 9 | // no commercial use allowed, the full terms of which are made available 10 | // in the LICENSE file. No license in patents is granted. 11 | // 12 | // You can contact the authors at , 13 | // . 14 | 15 | #include 16 | #include 17 | 18 | #include "test_common.hpp" 19 | #include "test_groups.hpp" 20 | 21 | int main(int argc, char **argv) 22 | { 23 | srand(static_cast(time(0))); 24 | testing::InitGoogleTest(&argc, argv); 25 | return RUN_ALL_TESTS(); 26 | } 27 | --------------------------------------------------------------------------------