├── CMakeLists.txt ├── LICENSE ├── LICENSE.txt ├── README.md ├── SophusConfig.cmake.in ├── appveyor.yml ├── cmake_modules └── FindEigen3.cmake ├── data └── cauchy │ ├── init.txt │ ├── loop_point.txt │ ├── odometry.txt │ └── semantic_p_matrix.txt ├── py ├── cpp_gencode │ ├── Se2_Dx_exp_x.cpp │ ├── Se2_Dx_this_mul_exp_x_at_0.cpp │ ├── Se3_Dx_exp_x.cpp │ ├── Se3_Dx_this_mul_exp_x_at_0.cpp │ ├── So2_Dx_exp_x.cpp │ ├── So2_Dx_this_mul_exp_x_at_0.cpp │ ├── So3_Dx_exp_x.cpp │ └── So3_Dx_this_mul_exp_x_at_0.cpp ├── run_tests.sh └── sophus │ ├── __init__.py │ ├── complex.py │ ├── cse_codegen.py │ ├── dual_quaternion.py │ ├── matrix.py │ ├── quaternion.py │ ├── se2.py │ ├── se3.py │ ├── so2.py │ ├── so3.py │ └── so3_codegen.py ├── run_format.sh ├── scripts ├── install_linux_deps.sh ├── install_osx_deps.sh ├── run_cpp_tests.sh ├── run_robust_pcl_reconstruction_example_cauchy_no_sem.sh └── run_robust_pcl_reconstruction_example_cauchy_two_EM.sh ├── sophus ├── average.hpp ├── common.hpp ├── example_ensure_handler.cpp ├── geometry.hpp ├── interpolate.hpp ├── interpolate_details.hpp ├── num_diff.hpp ├── rotation_matrix.hpp ├── rxso2.hpp ├── rxso3.hpp ├── se2.hpp ├── se3.hpp ├── sim2.hpp ├── sim3.hpp ├── sim_details.hpp ├── so2.hpp ├── so3.hpp ├── test_macros.hpp ├── types.hpp └── velocities.hpp └── test ├── CMakeLists.txt ├── ceres ├── CMakeLists.txt ├── cauchy_em_two_EM.cpp ├── local_parameterization_se3.hpp └── test_ceres_se3.cpp └── core ├── CMakeLists.txt ├── test_common.cpp ├── test_geometry.cpp ├── test_rxso2.cpp ├── test_rxso3.cpp ├── test_se2.cpp ├── test_se3.cpp ├── test_sim2.cpp ├── test_sim3.cpp ├── test_so2.cpp ├── test_so3.cpp ├── test_velocities.cpp └── tests.hpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | project(Sophus VERSION 1.0.0) 3 | 4 | include(CMakePackageConfigHelpers) 5 | 6 | # Release by default 7 | # Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug" 8 | if(NOT CMAKE_BUILD_TYPE) 9 | set(CMAKE_BUILD_TYPE Release) 10 | endif() 11 | 12 | set(CMAKE_CXX_STANDARD 11) 13 | 14 | # Set compiler specific settings (FixMe: Should not cmake do this for us automatically?) 15 | IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 16 | SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") 17 | SET(CMAKE_CXX_FLAGS_RELEASE "-O3") 18 | #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -Wno-deprecated-register -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics") 19 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wno-deprecated-register -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics") 20 | ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") 21 | SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") 22 | SET(CMAKE_CXX_FLAGS_RELEASE "-O3") 23 | #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++11 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0") 24 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -std=c++11 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0") 25 | SET(CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_DEBUG} --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") 26 | SET(CMAKE_EXE_LINKER_FLAGS_COVERAGE "${CMAKE_EXE_LINKER_FLAGS_DEBUG} --coverage") 27 | SET(CMAKE_SHARED_LINKER_FLAGS_COVERAGE "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} --coverage") 28 | ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES "^MSVC$") 29 | ADD_DEFINITIONS("-D _USE_MATH_DEFINES /bigobj /wd4305 /wd4244 /MP") 30 | ENDIF() 31 | 32 | # Add local path for finding packages, set the local version first 33 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules") 34 | 35 | # Find Eigen 3 (dependency) 36 | find_package(Eigen3 3.3.0 REQUIRED) 37 | 38 | # Define interface library target 39 | add_library(sophus INTERFACE) 40 | 41 | set(SOPHUS_HEADER_FILES 42 | sophus/average.hpp 43 | sophus/common.hpp 44 | sophus/geometry.hpp 45 | sophus/interpolate.hpp 46 | sophus/interpolate_details.hpp 47 | sophus/num_diff.hpp 48 | sophus/rotation_matrix.hpp 49 | sophus/rxso2.hpp 50 | sophus/rxso3.hpp 51 | sophus/se2.hpp 52 | sophus/se3.hpp 53 | sophus/sim2.hpp 54 | sophus/sim3.hpp 55 | sophus/sim_details.hpp 56 | sophus/so2.hpp 57 | sophus/so3.hpp 58 | sophus/types.hpp 59 | sophus/velocities.hpp 60 | ) 61 | 62 | set(SOPHUS_OTHER_FILES 63 | sophus/test_macros.hpp 64 | sophus/example_ensure_handler.cpp 65 | ) 66 | 67 | if(MSVC) 68 | # Define common math constants if we compile with MSVC 69 | target_compile_definitions (sophus INTERFACE _USE_MATH_DEFINES) 70 | endif (MSVC) 71 | 72 | # Add Eigen interface dependency, depending on available cmake info 73 | if(TARGET Eigen3::Eigen) 74 | target_link_libraries(sophus INTERFACE Eigen3::Eigen) 75 | set(Eigen3_DEPENDENCY "find_dependency (Eigen3 ${Eigen3_VERSION})") 76 | else(TARGET Eigen3::Eigen) 77 | target_include_directories (sophus SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR}) 78 | endif(TARGET Eigen3::Eigen) 79 | 80 | # Associate target with include directory 81 | target_include_directories(sophus INTERFACE 82 | "$" 83 | "$" 84 | ) 85 | 86 | # Declare all used C++11 features 87 | target_compile_features (sophus INTERFACE 88 | cxx_auto_type 89 | cxx_decltype 90 | cxx_nullptr 91 | cxx_right_angle_brackets 92 | cxx_variadic_macros 93 | cxx_variadic_templates 94 | ) 95 | 96 | # Add sources as custom target so that they are shown in IDE's 97 | add_custom_target(other SOURCES ${SOPHUS_OTHER_FILES}) 98 | 99 | # Create 'test' make target using ctest 100 | option(BUILD_TESTS "Build tests." ON) 101 | if(BUILD_TESTS) 102 | enable_testing() 103 | add_subdirectory(test) 104 | endif() 105 | 106 | # Export package for use from the build tree 107 | include(GNUInstallDirs) 108 | set(SOPHUS_CMAKE_EXPORT_DIR ${CMAKE_INSTALL_DATADIR}/sophus/cmake) 109 | 110 | set_target_properties(sophus PROPERTIES EXPORT_NAME Sophus) 111 | 112 | install(TARGETS sophus EXPORT SophusTargets) 113 | install(EXPORT SophusTargets 114 | NAMESPACE Sophus:: 115 | DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR} 116 | ) 117 | 118 | export(TARGETS sophus NAMESPACE Sophus:: FILE SophusTargets.cmake) 119 | export(PACKAGE Sophus) 120 | 121 | configure_package_config_file( 122 | SophusConfig.cmake.in 123 | ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake 124 | INSTALL_DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR} 125 | NO_CHECK_REQUIRED_COMPONENTS_MACRO 126 | ) 127 | 128 | # Remove architecture dependence. Sophus is a header-only library. 129 | set(TEMP_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P}) 130 | unset(CMAKE_SIZEOF_VOID_P) 131 | 132 | # Write version to file 133 | write_basic_package_version_file ( 134 | SophusConfigVersion.cmake 135 | VERSION ${PROJECT_VERSION} 136 | COMPATIBILITY SameMajorVersion 137 | ) 138 | 139 | # Recover architecture dependence 140 | set(CMAKE_SIZEOF_VOID_P ${TEMP_SIZEOF_VOID_P}) 141 | 142 | # Install cmake targets 143 | install( 144 | FILES ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake 145 | ${CMAKE_CURRENT_BINARY_DIR}/SophusConfigVersion.cmake 146 | DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR} 147 | ) 148 | 149 | # Install header files 150 | install( 151 | FILES ${SOPHUS_HEADER_FILES} 152 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sophus 153 | ) 154 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Tang Yujie 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright 2011-2017 Hauke Strasdat 2 | 2012-2017 Steven Lovegrove 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to 6 | deal in the Software without restriction, including without limitation the 7 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 8 | sell copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 20 | IN THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # HORCL : Robust Multi-robot Collaborative Localization in High Outlier ratio Scenes 2 | 3 | ## Overview 4 | This is a c++ implementation based on [strasdat/Sophus](https://github.com/strasdat/Sophus). The solvers' cpp files are in the **test/ceres/** directory. 5 | 6 | ## Dependency 7 | 8 | - Eigen 3.3.4 9 | - Google ceres 2.0.0 10 | 11 | ## Quickstart 12 | Assume the current directory is the root of this repository. 13 | 14 | > Compile 15 | ```sh 16 | $ chmod +x ./scripts/run_cpp_tests.sh 17 | $ ./scripts/run_cpp_tests.sh 18 | ``` 19 | 20 | > Run 21 | ```sh 22 | $ chmod +x ./scripts/run_robust_pcl_reconstruction_example_cauchy_two_EM.sh 23 | $ ./scripts/run_robust_pcl_reconstruction_example_cauchy_two_EM.sh 24 | ``` 25 | 26 | ## Common problems 27 | 1. **ccache** may not be installed by default. Simply install it. 28 | ```sh 29 | $ sudo apt-get install ccache 30 | ``` 31 | 32 | ## Acknowledge 33 | Thanks for the open source code: [RobustPCLReconstruction](https://github.com/ziquan111/RobustPCLReconstruction)! 34 | -------------------------------------------------------------------------------- /SophusConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include (CMakeFindDependencyMacro) 4 | 5 | @Eigen3_DEPENDENCY@ 6 | 7 | include ("${CMAKE_CURRENT_LIST_DIR}/SophusTargets.cmake") 8 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | branches: 2 | only: 3 | - master 4 | 5 | os: Visual Studio 2015 6 | 7 | clone_folder: c:\projects\sophus 8 | 9 | platform: x64 10 | configuration: Release 11 | 12 | build: 13 | project: c:\projects\sophus\build\Sophus.sln 14 | 15 | install: 16 | - ps: wget http://bitbucket.org/eigen/eigen/get/3.3.4.zip -outfile eigen3.zip 17 | - cmd: 7z x eigen3.zip -o"C:\projects" -y > nul 18 | 19 | before_build: 20 | - cd c:\projects\sophus 21 | - mkdir build 22 | - cd build 23 | - cmake -G "Visual Studio 14 2015 Win64" -D EIGEN3_INCLUDE_DIR=C:\projects\eigen-eigen-5a0156e40feb .. 24 | 25 | after_build: 26 | - ctest 27 | -------------------------------------------------------------------------------- /cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | /usr/local/include 67 | /opt/local/include 68 | ${CMAKE_INSTALL_PREFIX}/include 69 | ${KDE4_INCLUDE_DIR} 70 | PATH_SUFFIXES eigen3 eigen 71 | ) 72 | 73 | if(EIGEN3_INCLUDE_DIR) 74 | _eigen3_check_version() 75 | endif(EIGEN3_INCLUDE_DIR) 76 | 77 | include(FindPackageHandleStandardArgs) 78 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 79 | 80 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 81 | 82 | endif(EIGEN3_INCLUDE_DIR) 83 | -------------------------------------------------------------------------------- /data/cauchy/semantic_p_matrix.txt: -------------------------------------------------------------------------------- 1 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 | 0 0.596766 0.000811944 0 0 0.000206782 0.000205451 0 0.000214326 0 0.000370331 0.000252137 0.000160571 0.000639846 0.000154752 0 7.91536e-06 0 0.00313944 0.000793448 9.67399e-05 0 0 0 3 | 0 0.000811944 0.707594 0 0.0054915 0.00134996 5.46729e-05 0 0.125265 0 0.0372298 0.0402488 0.0182369 0.00912361 0.0146149 0 0.00131623 0.00376953 6.22747e-05 0.00272967 0 0.00154344 3.45481e-05 0 4 | 0 0 0 0.0084172 0 0 0 0 0.00201671 0 2.10135e-05 0.00119413 0 0.0195709 0 0 0 0 0 0 0 0 0 0 5 | 0 0 0.0054915 0 0.172725 0 0 0 0.00452267 0 0.00249176 0.00303479 0.00304562 0 0.00114891 0 0 0.0312533 0.00641479 0 0 0 0 0 6 | 0 0.000206782 0.00134996 0 0 0.839355 0 0 0.0129706 0 0.00210465 0.00443643 0 0 0 0 0 0 0 0 1.0409e-05 0 0 0 7 | 0 0.000205451 5.46729e-05 0 0 0 0.875635 0 0.00975156 0 0.0038116 0.000301357 0.000989952 0 0.00352743 0 0 3.50955e-05 0 0 3.44725e-05 0 0 0 8 | 0 0 0 0 0 0 0 0.0466057 0.00226086 0 4.64586e-06 0.00104202 0 0.0394276 0 0 0 0 0 0.00676504 0 0 0 0 9 | 0 0.000214326 0.125265 0.00201671 0.00452267 0.0129706 0.00975156 0.00226086 0.551125 0 0.067983 0.010288 0.00969141 0.00155636 0.00599544 1.65091e-06 0.0387979 0.00271438 1.7133e-06 0.0123215 2.58445e-06 0.00187629 0.00459031 0 10 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 11 | 0 0.000370331 0.0372298 2.10135e-05 0.00249176 0.00210465 0.0038116 4.64586e-06 0.067983 0 0.0992394 0.0520457 0.0576837 0.0104112 0.0255629 5.91297e-05 0.00496628 0.00213443 0 0.00971682 0 0.000288527 0.00173757 0 12 | 0 0.000252137 0.0402488 0.00119413 0.00303479 0.00443643 0.000301357 0.00104202 0.010288 0 0.0520457 0.935711 0.0227485 0.00169912 0.0382098 0.0006108 0.00356928 0.00237217 0.000376453 0.0120357 0 0.00015887 0.0006261 0 13 | 0 0.000160571 0.0182369 0 0.00304562 0 0.000989952 0 0.00969141 0 0.0576837 0.0227485 0.454613 0.00398248 0.0927762 0.00209906 0.00192879 0.0117588 0.000339355 0.0106075 0 0.000235944 0.00042422 0 14 | 0 0.000639846 0.00912361 0.0195709 0 0 0 0.0394276 0.00155636 0 0.0104112 0.00169912 0.00398248 0.657731 0.00740351 0 1.98772e-05 0.00708941 0 0.0146218 0 0 0 0 15 | 0 0.000154752 0.0146149 0 0.00114891 0 0.00352743 0 0.00599544 0 0.0255629 0.0382098 0.0927762 0.00740351 0.898706 0.00243062 0.108804 0.00342263 9.52547e-05 0.00513489 0 0 0.000601255 0 16 | 0 0 0 0 0 0 0 0 1.65091e-06 0 5.91297e-05 0.0006108 0.00209906 0 0.00243062 0.0982397 0.0164644 0 0 0 0 0 0 0 17 | 0 7.91536e-06 0.00131623 0 0 0 0 0 0.0387979 0 0.00496628 0.00356928 0.00192879 1.98772e-05 0.108804 0.0164644 0.540981 0.0163865 0.000607836 0.00132301 0 0.000245273 0 0 18 | 0 0 0.00376953 0 0.0312533 0 3.50955e-05 0 0.00271438 0 0.00213443 0.00237217 0.0117588 0.00708941 0.00342263 0 0.0163865 0.204566 0.0107655 3.54189e-06 0 0 0 0 19 | 0 0.00313944 6.22747e-05 0 0.00641479 0 0 0 1.7133e-06 0 0 0.000376453 0.000339355 0 9.52547e-05 0 0.000607836 0.0107655 0.346406 0 0 0 0 0 20 | 0 0.000793448 0.00272967 0 0 0 0 0.00676504 0.0123215 0 0.00971682 0.0120357 0.0106075 0.0146218 0.00513489 0 0.00132301 3.54189e-06 0 0.339052 0 0 0.00944571 0 21 | 0 9.67399e-05 0 0 0 1.0409e-05 3.44725e-05 0 2.58445e-06 0 0 0 0 0 0 0 0 0 0 0 0.125 0 0 0 22 | 0 0 0.00154344 0 0 0 0 0 0.00187629 0 0.000288527 0.00015887 0.000235944 0 0 0 0.000245273 0 0 0 0 0.17149 0 0 23 | 0 0 3.45481e-05 0 0 0 0 0 0.00459031 0 0.00173757 0.0006261 0.00042422 0 0.000601255 0 0 0 0 0.00944571 0 0 0.159191 0 24 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 25 | -------------------------------------------------------------------------------- /py/cpp_gencode/Se2_Dx_exp_x.cpp: -------------------------------------------------------------------------------- 1 | Scalar const c0 = sin(theta); 2 | Scalar const c1 = cos(theta); 3 | Scalar const c2 = 1.0/theta; 4 | Scalar const c3 = c0*c2; 5 | Scalar const c4 = -c1 + 1; 6 | Scalar const c5 = c2*c4; 7 | Scalar const c6 = c1*c2; 8 | Scalar const c7 = pow(theta, -2); 9 | Scalar const c8 = c0*c7; 10 | Scalar const c9 = c4*c7; 11 | result[0] = 0; 12 | result[1] = 0; 13 | result[2] = -c0; 14 | result[3] = 0; 15 | result[4] = 0; 16 | result[5] = c1; 17 | result[6] = c3; 18 | result[7] = -c5; 19 | result[8] = -c3*upsilon[1] + c6*upsilon[0] - c8*upsilon[0] + c9*upsilon[1]; 20 | result[9] = c5; 21 | result[10] = c3; 22 | result[11] = c3*upsilon[0] + c6*upsilon[1] - c8*upsilon[1] - c9*upsilon[0]; 23 | -------------------------------------------------------------------------------- /py/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp: -------------------------------------------------------------------------------- 1 | Scalar const c0 = -c[1]; 2 | result[0] = 0; 3 | result[1] = 0; 4 | result[2] = c0; 5 | result[3] = 0; 6 | result[4] = 0; 7 | result[5] = c[0]; 8 | result[6] = c[0]; 9 | result[7] = c0; 10 | result[8] = 0; 11 | result[9] = c[1]; 12 | result[10] = c[0]; 13 | result[11] = 0; 14 | -------------------------------------------------------------------------------- /py/cpp_gencode/Se3_Dx_exp_x.cpp: -------------------------------------------------------------------------------- 1 | Scalar const c0 = pow(omega[0], 2); 2 | Scalar const c1 = pow(omega[1], 2); 3 | Scalar const c2 = pow(omega[2], 2); 4 | Scalar const c3 = c0 + c1 + c2; 5 | Scalar const c4 = sqrt(c3); 6 | Scalar const c5 = 1.0/c4; 7 | Scalar const c6 = 0.5*c4; 8 | Scalar const c7 = sin(c6); 9 | Scalar const c8 = c5*c7; 10 | Scalar const c9 = pow(c3, -3.0L/2.0L); 11 | Scalar const c10 = c7*c9; 12 | Scalar const c11 = 1.0/c3; 13 | Scalar const c12 = cos(c6); 14 | Scalar const c13 = 0.5*c11*c12; 15 | Scalar const c14 = c7*c9*omega[0]; 16 | Scalar const c15 = 0.5*c11*c12*omega[0]; 17 | Scalar const c16 = -c14*omega[1] + c15*omega[1]; 18 | Scalar const c17 = -c14*omega[2] + c15*omega[2]; 19 | Scalar const c18 = omega[1]*omega[2]; 20 | Scalar const c19 = -c10*c18 + c13*c18; 21 | Scalar const c20 = c5*omega[0]; 22 | Scalar const c21 = 0.5*c7; 23 | Scalar const c22 = c5*omega[1]; 24 | Scalar const c23 = c5*omega[2]; 25 | Scalar const c24 = -c1; 26 | Scalar const c25 = -c2; 27 | Scalar const c26 = c24 + c25; 28 | Scalar const c27 = sin(c4); 29 | Scalar const c28 = -c27 + c4; 30 | Scalar const c29 = c28*c9; 31 | Scalar const c30 = cos(c4); 32 | Scalar const c31 = -c30 + 1; 33 | Scalar const c32 = c11*c31; 34 | Scalar const c33 = c32*omega[2]; 35 | Scalar const c34 = c29*omega[0]; 36 | Scalar const c35 = c34*omega[1]; 37 | Scalar const c36 = c32*omega[1]; 38 | Scalar const c37 = c34*omega[2]; 39 | Scalar const c38 = pow(c3, -5.0L/2.0L); 40 | Scalar const c39 = 3*c28*c38*omega[0]; 41 | Scalar const c40 = c26*c9; 42 | Scalar const c41 = -c20*c30 + c20; 43 | Scalar const c42 = c27*c9*omega[0]; 44 | Scalar const c43 = c42*omega[1]; 45 | Scalar const c44 = pow(c3, -2); 46 | Scalar const c45 = 2*c31*c44*omega[0]; 47 | Scalar const c46 = c45*omega[1]; 48 | Scalar const c47 = c29*omega[2]; 49 | Scalar const c48 = c43 - c46 + c47; 50 | Scalar const c49 = 3*c0*c28*c38; 51 | Scalar const c50 = c9*omega[0]*omega[2]; 52 | Scalar const c51 = c41*c50 - c49*omega[2]; 53 | Scalar const c52 = c9*omega[0]*omega[1]; 54 | Scalar const c53 = c41*c52 - c49*omega[1]; 55 | Scalar const c54 = c42*omega[2]; 56 | Scalar const c55 = c45*omega[2]; 57 | Scalar const c56 = c29*omega[1]; 58 | Scalar const c57 = -c54 + c55 + c56; 59 | Scalar const c58 = -2*c56; 60 | Scalar const c59 = 3*c28*c38*omega[1]; 61 | Scalar const c60 = -c22*c30 + c22; 62 | Scalar const c61 = -c18*c39; 63 | Scalar const c62 = c32 + c61; 64 | Scalar const c63 = c27*c9; 65 | Scalar const c64 = c1*c63; 66 | Scalar const c65 = 2*c31*c44; 67 | Scalar const c66 = c1*c65; 68 | Scalar const c67 = c50*c60; 69 | Scalar const c68 = -c1*c39 + c52*c60; 70 | Scalar const c69 = c18*c63; 71 | Scalar const c70 = c18*c65; 72 | Scalar const c71 = c34 - c69 + c70; 73 | Scalar const c72 = -2*c47; 74 | Scalar const c73 = 3*c28*c38*omega[2]; 75 | Scalar const c74 = -c23*c30 + c23; 76 | Scalar const c75 = -c32 + c61; 77 | Scalar const c76 = c2*c63; 78 | Scalar const c77 = c2*c65; 79 | Scalar const c78 = c52*c74; 80 | Scalar const c79 = c34 + c69 - c70; 81 | Scalar const c80 = -c2*c39 + c50*c74; 82 | Scalar const c81 = -c0; 83 | Scalar const c82 = c25 + c81; 84 | Scalar const c83 = c32*omega[0]; 85 | Scalar const c84 = c18*c29; 86 | Scalar const c85 = -2*c34; 87 | Scalar const c86 = c82*c9; 88 | Scalar const c87 = c0*c63; 89 | Scalar const c88 = c0*c65; 90 | Scalar const c89 = c9*omega[1]*omega[2]; 91 | Scalar const c90 = c41*c89; 92 | Scalar const c91 = c54 - c55 + c56; 93 | Scalar const c92 = -c1*c73 + c60*c89; 94 | Scalar const c93 = -c43 + c46 + c47; 95 | Scalar const c94 = -c2*c59 + c74*c89; 96 | Scalar const c95 = c24 + c81; 97 | Scalar const c96 = c9*c95; 98 | result[0] = 0; 99 | result[1] = 0; 100 | result[2] = 0; 101 | result[3] = -c0*c10 + c0*c13 + c8; 102 | result[4] = c16; 103 | result[5] = c17; 104 | result[6] = 0; 105 | result[7] = 0; 106 | result[8] = 0; 107 | result[9] = c16; 108 | result[10] = -c1*c10 + c1*c13 + c8; 109 | result[11] = c19; 110 | result[12] = 0; 111 | result[13] = 0; 112 | result[14] = 0; 113 | result[15] = c17; 114 | result[16] = c19; 115 | result[17] = -c10*c2 + c13*c2 + c8; 116 | result[18] = 0; 117 | result[19] = 0; 118 | result[20] = 0; 119 | result[21] = -c20*c21; 120 | result[22] = -c21*c22; 121 | result[23] = -c21*c23; 122 | result[24] = c26*c29 + 1; 123 | result[25] = -c33 + c35; 124 | result[26] = c36 + c37; 125 | result[27] = upsilon[0]*(-c26*c39 + c40*c41) + upsilon[1]*(c53 + c57) + upsilon[2]*(c48 + c51); 126 | result[28] = upsilon[0]*(-c26*c59 + c40*c60 + c58) + upsilon[1]*(c68 + c71) + upsilon[2]*(c62 + c64 - c66 + c67); 127 | result[29] = upsilon[0]*(-c26*c73 + c40*c74 + c72) + upsilon[1]*(c75 - c76 + c77 + c78) + upsilon[2]*(c79 + c80); 128 | result[30] = c33 + c35; 129 | result[31] = c29*c82 + 1; 130 | result[32] = -c83 + c84; 131 | result[33] = upsilon[0]*(c53 + c91) + upsilon[1]*(-c39*c82 + c41*c86 + c85) + upsilon[2]*(c75 - c87 + c88 + c90); 132 | result[34] = upsilon[0]*(c68 + c79) + upsilon[1]*(-c59*c82 + c60*c86) + upsilon[2]*(c92 + c93); 133 | result[35] = upsilon[0]*(c62 + c76 - c77 + c78) + upsilon[1]*(c72 - c73*c82 + c74*c86) + upsilon[2]*(c57 + c94); 134 | result[36] = -c36 + c37; 135 | result[37] = c83 + c84; 136 | result[38] = c29*c95 + 1; 137 | result[39] = upsilon[0]*(c51 + c93) + upsilon[1]*(c62 + c87 - c88 + c90) + upsilon[2]*(-c39*c95 + c41*c96 + c85); 138 | result[40] = upsilon[0]*(-c64 + c66 + c67 + c75) + upsilon[1]*(c48 + c92) + upsilon[2]*(c58 - c59*c95 + c60*c96); 139 | result[41] = upsilon[0]*(c71 + c80) + upsilon[1]*(c91 + c94) + upsilon[2]*(-c73*c95 + c74*c96); 140 | -------------------------------------------------------------------------------- /py/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp: -------------------------------------------------------------------------------- 1 | Scalar const c0 = 0.5*q.w(); 2 | Scalar const c1 = 0.5*q.z(); 3 | Scalar const c2 = -c1; 4 | Scalar const c3 = 0.5*q.y(); 5 | Scalar const c4 = 0.5*q.x(); 6 | Scalar const c5 = -c4; 7 | Scalar const c6 = -c3; 8 | Scalar const c7 = pow(q.w(), 2); 9 | Scalar const c8 = pow(q.x(), 2); 10 | Scalar const c9 = pow(q.y(), 2); 11 | Scalar const c10 = -c9; 12 | Scalar const c11 = pow(q.z(), 2); 13 | Scalar const c12 = -c11; 14 | Scalar const c13 = 2*q.w(); 15 | Scalar const c14 = c13*q.z(); 16 | Scalar const c15 = 2*q.x(); 17 | Scalar const c16 = c15*q.y(); 18 | Scalar const c17 = c13*q.y(); 19 | Scalar const c18 = c15*q.z(); 20 | Scalar const c19 = c7 - c8; 21 | Scalar const c20 = c13*q.x(); 22 | Scalar const c21 = 2*q.y()*q.z(); 23 | result[0] = 0; 24 | result[1] = 0; 25 | result[2] = 0; 26 | result[3] = c0; 27 | result[4] = c2; 28 | result[5] = c3; 29 | result[6] = 0; 30 | result[7] = 0; 31 | result[8] = 0; 32 | result[9] = c1; 33 | result[10] = c0; 34 | result[11] = c5; 35 | result[12] = 0; 36 | result[13] = 0; 37 | result[14] = 0; 38 | result[15] = c6; 39 | result[16] = c4; 40 | result[17] = c0; 41 | result[18] = 0; 42 | result[19] = 0; 43 | result[20] = 0; 44 | result[21] = c5; 45 | result[22] = c6; 46 | result[23] = c2; 47 | result[24] = c10 + c12 + c7 + c8; 48 | result[25] = -c14 + c16; 49 | result[26] = c17 + c18; 50 | result[27] = 0; 51 | result[28] = 0; 52 | result[29] = 0; 53 | result[30] = c14 + c16; 54 | result[31] = c12 + c19 + c9; 55 | result[32] = -c20 + c21; 56 | result[33] = 0; 57 | result[34] = 0; 58 | result[35] = 0; 59 | result[36] = -c17 + c18; 60 | result[37] = c20 + c21; 61 | result[38] = c10 + c11 + c19; 62 | result[39] = 0; 63 | result[40] = 0; 64 | result[41] = 0; 65 | -------------------------------------------------------------------------------- /py/cpp_gencode/So2_Dx_exp_x.cpp: -------------------------------------------------------------------------------- 1 | result[0] = -sin(theta); 2 | result[1] = cos(theta); 3 | -------------------------------------------------------------------------------- /py/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp: -------------------------------------------------------------------------------- 1 | result[0] = -c[1]; 2 | result[1] = c[0]; 3 | -------------------------------------------------------------------------------- /py/cpp_gencode/So3_Dx_exp_x.cpp: -------------------------------------------------------------------------------- 1 | Scalar const c0 = pow(omega[0], 2); 2 | Scalar const c1 = pow(omega[1], 2); 3 | Scalar const c2 = pow(omega[2], 2); 4 | Scalar const c3 = c0 + c1 + c2; 5 | Scalar const c4 = sqrt(c3); 6 | Scalar const c5 = 1.0/c4; 7 | Scalar const c6 = 0.5*c4; 8 | Scalar const c7 = sin(c6); 9 | Scalar const c8 = c5*c7; 10 | Scalar const c9 = pow(c3, -3.0L/2.0L); 11 | Scalar const c10 = c7*c9; 12 | Scalar const c11 = 1.0/c3; 13 | Scalar const c12 = cos(c6); 14 | Scalar const c13 = 0.5*c11*c12; 15 | Scalar const c14 = c7*c9*omega[0]; 16 | Scalar const c15 = 0.5*c11*c12*omega[0]; 17 | Scalar const c16 = -c14*omega[1] + c15*omega[1]; 18 | Scalar const c17 = -c14*omega[2] + c15*omega[2]; 19 | Scalar const c18 = omega[1]*omega[2]; 20 | Scalar const c19 = -c10*c18 + c13*c18; 21 | Scalar const c20 = 0.5*c5*c7; 22 | result[0] = -c0*c10 + c0*c13 + c8; 23 | result[1] = c16; 24 | result[2] = c17; 25 | result[3] = c16; 26 | result[4] = -c1*c10 + c1*c13 + c8; 27 | result[5] = c19; 28 | result[6] = c17; 29 | result[7] = c19; 30 | result[8] = -c10*c2 + c13*c2 + c8; 31 | result[9] = -c20*omega[0]; 32 | result[10] = -c20*omega[1]; 33 | result[11] = -c20*omega[2]; 34 | -------------------------------------------------------------------------------- /py/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp: -------------------------------------------------------------------------------- 1 | Scalar const c0 = 0.5*q.w(); 2 | Scalar const c1 = 0.5*q.z(); 3 | Scalar const c2 = -c1; 4 | Scalar const c3 = 0.5*q.y(); 5 | Scalar const c4 = 0.5*q.x(); 6 | Scalar const c5 = -c4; 7 | Scalar const c6 = -c3; 8 | result[0] = c0; 9 | result[1] = c2; 10 | result[2] = c3; 11 | result[3] = c1; 12 | result[4] = c0; 13 | result[5] = c5; 14 | result[6] = c6; 15 | result[7] = c4; 16 | result[8] = c0; 17 | result[9] = c5; 18 | result[10] = c6; 19 | result[11] = c2; 20 | -------------------------------------------------------------------------------- /py/run_tests.sh: -------------------------------------------------------------------------------- 1 | #bin/bash 2 | 3 | python3 -m sophus.complex 4 | python3 -m sophus.quaternion 5 | python3 -m sophus.dual_quaternion 6 | python3 -m sophus.so2 7 | python3 -m sophus.se2 8 | python3 -m sophus.so3 9 | python3 -m sophus.se3 -------------------------------------------------------------------------------- /py/sophus/__init__.py: -------------------------------------------------------------------------------- 1 | from sophus.cse_codegen import cse_codegen 2 | from sophus.matrix import dot, squared_norm, Vector2, Vector3, Vector6, ZeroVector2, ZeroVector3, ZeroVector6, proj, unproj 3 | from sophus.complex import Complex 4 | from sophus.quaternion import Quaternion 5 | from sophus.so2 import So2 6 | from sophus.so3 import So3 7 | -------------------------------------------------------------------------------- /py/sophus/complex.py: -------------------------------------------------------------------------------- 1 | import sophus 2 | import sympy 3 | import sys 4 | import unittest 5 | 6 | 7 | class Complex: 8 | """ Complex class """ 9 | 10 | def __init__(self, real, imag): 11 | self.real = real 12 | self.imag = imag 13 | 14 | def __mul__(self, right): 15 | """ complex multiplication """ 16 | return Complex(self.real * right.real - self.imag * right.imag, 17 | self.imag * right.real + self.real * right.imag) 18 | 19 | def __add__(self, right): 20 | return Complex(elf.real + right.real, self.imag + right.imag) 21 | 22 | def __neg__(self): 23 | return Complex(-self.real, -self.image) 24 | 25 | def __truediv__(self, scalar): 26 | """ scalar division """ 27 | return Complex(self.real / scalar, self.imag / scalar) 28 | 29 | def __repr__(self): 30 | return "( " + repr(self.real) + " + " + repr(self.imag) + "i )" 31 | 32 | def __getitem__(self, key): 33 | """ We use the following convention [real, imag] """ 34 | if key == 0: 35 | return self.real 36 | else: 37 | return self.imag 38 | 39 | def squared_norm(self): 40 | """ squared norm when considering the complex number as tuple """ 41 | return self.real**2 + self.imag**2 42 | 43 | def conj(self): 44 | """ complex conjugate """ 45 | return Complex(self.real, -self.imag) 46 | 47 | def inv(self): 48 | """ complex inverse """ 49 | return self.conj() / self.squared_norm() 50 | 51 | @staticmethod 52 | def identity(): 53 | return Complex(1, 0) 54 | 55 | @staticmethod 56 | def zero(): 57 | return Complex(0, 0) 58 | 59 | def __eq__(self, other): 60 | if isinstance(self, other.__class__): 61 | return self.real == other.real and self.imag == other.imag 62 | return False 63 | 64 | def subs(self, x, y): 65 | return Complex(self.real.subs(x, y), self.imag.subs(x, y)) 66 | 67 | def simplify(self): 68 | return Complex(sympy.simplify(self.real), 69 | sympy.simplify(self.imag)) 70 | 71 | @staticmethod 72 | def Da_a_mul_b(a, b): 73 | """ derivatice of complex muliplication wrt left multiplier a """ 74 | return sympy.Matrix([[b.real, -b.imag], 75 | [b.imag, b.real]]) 76 | 77 | @staticmethod 78 | def Db_a_mul_b(a, b): 79 | """ derivatice of complex muliplication wrt right multiplicand b """ 80 | return sympy.Matrix([[a.real, -a.imag], 81 | [a.imag, a.real]]) 82 | 83 | 84 | class TestComplex(unittest.TestCase): 85 | def setUp(self): 86 | x, y = sympy.symbols('x y', real=True) 87 | u, v = sympy.symbols('u v', real=True) 88 | self.a = Complex(x, y) 89 | self.b = Complex(u, v) 90 | 91 | def test_muliplications(self): 92 | product = self.a * self.a.inv() 93 | self.assertEqual(product.simplify(), 94 | Complex.identity()) 95 | product = self.a.inv() * self.a 96 | self.assertEqual(product.simplify(), 97 | Complex.identity()) 98 | 99 | def test_derivatives(self): 100 | d = sympy.Matrix(2, 2, lambda r, c: sympy.diff( 101 | (self.a * self.b)[r], self.a[c])) 102 | self.assertEqual(d, 103 | Complex.Da_a_mul_b(self.a, self.b)) 104 | d = sympy.Matrix(2, 2, lambda r, c: sympy.diff( 105 | (self.a * self.b)[r], self.b[c])) 106 | self.assertEqual(d, 107 | Complex.Db_a_mul_b(self.a, self.b)) 108 | 109 | 110 | if __name__ == '__main__': 111 | unittest.main() 112 | print('hello') 113 | -------------------------------------------------------------------------------- /py/sophus/cse_codegen.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import io 3 | 4 | 5 | def cse_codegen(symbols): 6 | cse_results = sympy.cse(symbols, sympy.numbered_symbols("c")) 7 | output = io.StringIO() 8 | for helper in cse_results[0]: 9 | output.write("Scalar const ") 10 | output.write(sympy.printing.ccode(helper[1], helper[0])) 11 | output.write("\n") 12 | assert len(cse_results[1]) == 1 13 | 14 | output.write(sympy.printing.ccode(cse_results[1][0], "result")) 15 | output.write("\n") 16 | output.seek(0) 17 | return output 18 | -------------------------------------------------------------------------------- /py/sophus/dual_quaternion.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import sys 3 | import unittest 4 | import sophus 5 | 6 | 7 | class DualQuaternion: 8 | """ Dual quaternion class """ 9 | 10 | def __init__(self, real_q, inf_q): 11 | """ Dual quaternion consists of a real quaternion, and an infinitesimal 12 | quaternion """ 13 | self.real_q = real_q 14 | self.inf_q = inf_q 15 | 16 | def __mul__(self, right): 17 | """ dual quaternion multiplication """ 18 | return DualQuaternion(self.real_q * right.real_q, 19 | self.real_q * right.inf_q + 20 | self.inf_q * right.real_q) 21 | 22 | def __truediv__(self, scalar): 23 | """ scalar division """ 24 | return DualQuaternion(self.real_q / scalar, self.inf_q / scalar) 25 | 26 | def __repr__(self): 27 | return "( " + repr(self.real_q) + \ 28 | " + " + repr(self.inf_q) + ")" 29 | 30 | def __getitem__(self, key): 31 | assert (key >= 0 and key < 8) 32 | if key < 4: 33 | return self.real_q[i] 34 | else: 35 | return self.inf_q[i - 4] 36 | 37 | def squared_norm(self): 38 | """ squared norm when considering the dual quaternion as 8-tuple """ 39 | return self.real_q.squared_norm() + self.inf_q.squared_norm() 40 | 41 | def conj(self): 42 | """ dual quaternion conjugate """ 43 | return DualQuaternion(self.real_q.conj(), self.inf_q.conj()) 44 | 45 | def inv(self): 46 | """ dual quaternion inverse """ 47 | return DualQuaternion(self.real_q.inv(), 48 | -self.real_q.inv() * self.inf_q * 49 | self.real_q.inv()) 50 | 51 | def simplify(self): 52 | return DualQuaternion(self.real_q.simplify(), 53 | self.inf_q.simplify()) 54 | 55 | @staticmethod 56 | def identity(): 57 | return DualQuaternion(sophus.Quaternion.identity(), 58 | sophus.Quaternion.zero()) 59 | 60 | def __eq__(self, other): 61 | if isinstance(self, other.__class__): 62 | return self.real_q == other.real_q and self.inf_q == other.inf_q 63 | return False 64 | 65 | 66 | class TestDualQuaternion(unittest.TestCase): 67 | def setUp(self): 68 | w, s0, s1, s2 = sympy.symbols('w s0 s1 s2', real=True) 69 | x, t0, t1, t2 = sympy.symbols('x t0 t1 t2', real=True) 70 | y, u0, u1, u2 = sympy.symbols('y u0 u1 u2', real=True) 71 | z, v0, v1, v2 = sympy.symbols('z v0 v1 v2', real=True) 72 | 73 | s = sophus.Vector3(s0, s1, s2) 74 | t = sophus.Vector3(t0, t1, t2) 75 | u = sophus.Vector3(u0, u1, u2) 76 | v = sophus.Vector3(v0, v1, v2) 77 | self.a = DualQuaternion(sophus.Quaternion(w, s), 78 | sophus.Quaternion(x, t)) 79 | self.b = DualQuaternion(sophus.Quaternion(y, u), 80 | sophus.Quaternion(z, v)) 81 | 82 | def test_muliplications(self): 83 | product = self.a * self.a.inv() 84 | self.assertEqual(product.simplify(), 85 | DualQuaternion.identity()) 86 | product = self.a.inv() * self.a 87 | self.assertEqual(product.simplify(), 88 | DualQuaternion.identity()) 89 | 90 | 91 | if __name__ == '__main__': 92 | unittest.main() 93 | -------------------------------------------------------------------------------- /py/sophus/matrix.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import sys 3 | 4 | assert sys.version_info >= (3, 5) 5 | 6 | 7 | def dot(left, right): 8 | assert(isinstance(left, sympy.Matrix)) 9 | assert(isinstance(right, sympy.Matrix)) 10 | 11 | sum = 0 12 | for c in range(0, left.cols): 13 | for r in range(0, left.rows): 14 | sum += left[r, c] * right[r, c] 15 | return sum 16 | 17 | 18 | def squared_norm(m): 19 | assert(isinstance(m, sympy.Matrix)) 20 | return dot(m, m) 21 | 22 | 23 | def Vector2(x, y): 24 | return sympy.Matrix([x, y]) 25 | 26 | 27 | def ZeroVector2(): 28 | return Vector2(0, 0) 29 | 30 | 31 | def Vector3(x, y, z): 32 | return sympy.Matrix([x, y, z]) 33 | 34 | 35 | def ZeroVector3(): 36 | return Vector3(0, 0, 0) 37 | 38 | 39 | def Vector6(a, b, c, d, e, f): 40 | return sympy.Matrix([a, b, c, d, e, f]) 41 | 42 | 43 | def ZeroVector6(): 44 | return Vector6(0, 0, 0, 0, 0, 0) 45 | 46 | 47 | def proj(v): 48 | m, n = v.shape 49 | assert m > 1 50 | assert n == 1 51 | l = [v[i] / v[m - 1] for i in range(0, m - 1)] 52 | r = sympy.Matrix(m - 1, 1, l) 53 | return r 54 | 55 | 56 | def unproj(v): 57 | m, n = v.shape 58 | assert m >= 1 59 | assert n == 1 60 | return v.col_join(sympy.Matrix.ones(1, 1)) 61 | -------------------------------------------------------------------------------- /py/sophus/quaternion.py: -------------------------------------------------------------------------------- 1 | """ run with: python3 -m sophus.quaternion """ 2 | 3 | import sophus 4 | import sympy 5 | import sys 6 | import unittest 7 | 8 | 9 | class Quaternion: 10 | """ Quaternion class """ 11 | 12 | def __init__(self, real, vec): 13 | """ Quaternion consists of a real scalar, and an imaginary 3-vector """ 14 | assert isinstance(vec, sympy.Matrix) 15 | assert vec.shape == (3, 1), vec.shape 16 | self.real = real 17 | self.vec = vec 18 | 19 | def __mul__(self, right): 20 | """ quaternion multiplication """ 21 | return Quaternion(self[3] * right[3] - self.vec.dot(right.vec), 22 | self[3] * right.vec + right[3] * self.vec + 23 | self.vec.cross(right.vec)) 24 | 25 | def __add__(self, right): 26 | """ quaternion multiplication """ 27 | return Quaternion(self[3] + right[3], self.vec + right.vec) 28 | 29 | def __neg__(self): 30 | return Quaternion(-self[3], -self.vec) 31 | 32 | def __truediv__(self, scalar): 33 | """ scalar division """ 34 | return Quaternion(self.real / scalar, self.vec / scalar) 35 | 36 | def __repr__(self): 37 | return "( " + repr(self[3]) + " + " + repr(self.vec) + "i )" 38 | 39 | def __getitem__(self, key): 40 | """ We use the following convention [vec0, vec1, vec2, real] """ 41 | assert (key >= 0 and key < 4) 42 | if key == 3: 43 | return self.real 44 | else: 45 | return self.vec[key] 46 | 47 | def squared_norm(self): 48 | """ squared norm when considering the quaternion as 4-tuple """ 49 | return sophus.squared_norm(self.vec) + self.real**2 50 | 51 | def conj(self): 52 | """ quaternion conjugate """ 53 | return Quaternion(self.real, -self.vec) 54 | 55 | def inv(self): 56 | """ quaternion inverse """ 57 | return self.conj() / self.squared_norm() 58 | 59 | @staticmethod 60 | def identity(): 61 | return Quaternion(1, sophus.Vector3(0, 0, 0)) 62 | 63 | @staticmethod 64 | def zero(): 65 | return Quaternion(0, sophus.Vector3(0, 0, 0)) 66 | 67 | def subs(self, x, y): 68 | return Quaternion(self.real.subs(x, y), self.vec.subs(x, y)) 69 | 70 | def simplify(self): 71 | v = sympy.simplify(self.vec) 72 | return Quaternion(sympy.simplify(self.real), 73 | sophus.Vector3(v[0], v[1], v[2])) 74 | 75 | def __eq__(self, other): 76 | if isinstance(self, other.__class__): 77 | return self.real == other.real and self.vec == other.vec 78 | return False 79 | 80 | @staticmethod 81 | def Da_a_mul_b(a, b): 82 | """ derivatice of quaternion muliplication wrt left multiplier a """ 83 | v0 = b.vec[0] 84 | v1 = b.vec[1] 85 | v2 = b.vec[2] 86 | y = b.real 87 | return sympy.Matrix([[y, v2, -v1, v0], 88 | [-v2, y, v0, v1], 89 | [v1, -v0, y, v2], 90 | [-v0, -v1, -v2, y]]) 91 | 92 | @staticmethod 93 | def Db_a_mul_b(a, b): 94 | """ derivatice of quaternion muliplication wrt right multiplicand b """ 95 | u0 = a.vec[0] 96 | u1 = a.vec[1] 97 | u2 = a.vec[2] 98 | x = a.real 99 | return sympy.Matrix([[x, -u2, u1, u0], 100 | [u2, x, -u0, u1], 101 | [-u1, u0, x, u2], 102 | [-u0, -u1, -u2, x]]) 103 | 104 | 105 | class TestQuaternion(unittest.TestCase): 106 | def setUp(self): 107 | x, u0, u1, u2 = sympy.symbols('x u0 u1 u2', real=True) 108 | y, v0, v1, v2 = sympy.symbols('y v0 v1 v2', real=True) 109 | u = sophus.Vector3(u0, u1, u2) 110 | v = sophus.Vector3(v0, v1, v2) 111 | self.a = Quaternion(x, u) 112 | self.b = Quaternion(y, v) 113 | 114 | def test_muliplications(self): 115 | product = self.a * self.a.inv() 116 | self.assertEqual(product.simplify(), 117 | Quaternion.identity()) 118 | product = self.a.inv() * self.a 119 | self.assertEqual(product.simplify(), 120 | Quaternion.identity()) 121 | 122 | def test_derivatives(self): 123 | d = sympy.Matrix(4, 4, lambda r, c: sympy.diff( 124 | (self.a * self.b)[r], self.a[c])) 125 | self.assertEqual(d, 126 | Quaternion.Da_a_mul_b(self.a, self.b)) 127 | d = sympy.Matrix(4, 4, lambda r, c: sympy.diff( 128 | (self.a * self.b)[r], self.b[c])) 129 | self.assertEqual(d, 130 | Quaternion.Db_a_mul_b(self.a, self.b)) 131 | 132 | 133 | if __name__ == '__main__': 134 | unittest.main() 135 | print('hello') 136 | -------------------------------------------------------------------------------- /py/sophus/se2.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import sys 3 | import unittest 4 | import sophus 5 | import functools 6 | 7 | 8 | class Se2: 9 | """ 2 dimensional group of rigid body transformations """ 10 | 11 | def __init__(self, so2, t): 12 | """ internally represented by a unit complex number z and a translation 13 | 2-vector """ 14 | self.so2 = so2 15 | self.t = t 16 | 17 | @staticmethod 18 | def exp(v): 19 | """ exponential map """ 20 | theta = v[2] 21 | so2 = sophus.So2.exp(theta) 22 | 23 | a = so2.z.imag / theta 24 | b = (1 - so2.z.real) / theta 25 | 26 | t = sophus.Vector2(a * v[0] - b * v[1], 27 | b * v[0] + a * v[1]) 28 | return Se2(so2, t) 29 | 30 | def log(self): 31 | theta = self.so2.log() 32 | halftheta = 0.5 * theta 33 | a = -(halftheta * self.so2.z.imag) / (self.so2.z.real - 1) 34 | 35 | V_inv = sympy.Matrix([[a, halftheta], 36 | [-halftheta, a]]) 37 | upsilon = V_inv * self.t 38 | return sophus.Vector3(upsilon[0], upsilon[1], theta) 39 | 40 | def __repr__(self): 41 | return "Se2: [" + repr(self.so2) + " " + repr(self.t) 42 | 43 | @staticmethod 44 | def hat(v): 45 | upsilon = sophus.Vector2(v[0], v[1]) 46 | theta = v[2] 47 | return sophus.So2.hat(theta).\ 48 | row_join(upsilon).\ 49 | col_join(sympy.Matrix.zeros(1, 3)) 50 | 51 | def matrix(self): 52 | """ returns matrix representation """ 53 | R = self.so2.matrix() 54 | return (R.row_join(self.t)).col_join(sympy.Matrix(1, 3, [0, 0, 1])) 55 | 56 | def __mul__(self, right): 57 | """ left-multiplication 58 | either rotation concatenation or point-transform """ 59 | if isinstance(right, sympy.Matrix): 60 | assert right.shape == (2, 1), right.shape 61 | return self.so2 * right + self.t 62 | elif isinstance(right, Se2): 63 | return Se2(self.so2 * right.so2, 64 | self.t + self.so2 * right.t) 65 | assert False, "unsupported type: {0}".format(type(right)) 66 | 67 | def __getitem__(self, key): 68 | """ We use the following convention [q0, q1, q2, q3, t0, t1, t2] """ 69 | assert (key >= 0 and key < 4) 70 | if key < 2: 71 | return self.so2[key] 72 | else: 73 | return self.t[key - 2] 74 | 75 | @staticmethod 76 | def calc_Dx_exp_x(x): 77 | return sympy.Matrix(4, 3, lambda r, c: 78 | sympy.diff(Se2.exp(x)[r], x[c])) 79 | 80 | @staticmethod 81 | def Dx_exp_x_at_0(): 82 | return sympy.Matrix([[0, 0, 0], 83 | [0, 0, 1], 84 | [1, 0, 0], 85 | [0, 1, 0]]) 86 | 87 | def calc_Dx_this_mul_exp_x_at_0(self, x): 88 | v = Se2.exp(x) 89 | return sympy.Matrix(4, 3, lambda r, c: 90 | sympy.diff((self * Se2.exp(x))[r], x[c])). \ 91 | subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) 92 | 93 | @staticmethod 94 | def calc_Dx_exp_x_at_0(x): 95 | return Se2.calc_Dx_exp_x(x).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) 96 | 97 | @staticmethod 98 | def Dxi_x_matrix(x, i): 99 | if i < 2: 100 | return sophus.So2.Dxi_x_matrix(x, i).\ 101 | row_join(sympy.Matrix.zeros(2, 1)).\ 102 | col_join(sympy.Matrix.zeros(1, 3)) 103 | M = sympy.Matrix.zeros(3, 3) 104 | M[i - 2, 2] = 1 105 | return M 106 | 107 | @staticmethod 108 | def calc_Dxi_x_matrix(x, i): 109 | return sympy.Matrix(3, 3, lambda r, c: 110 | sympy.diff(x.matrix()[r, c], x[i])) 111 | 112 | @staticmethod 113 | def Dxi_exp_x_matrix(x, i): 114 | T = Se2.exp(x) 115 | Dx_exp_x = Se2.calc_Dx_exp_x(x) 116 | l = [Dx_exp_x[j, i] * Se2.Dxi_x_matrix(T, j) for j in range(0, 4)] 117 | return functools.reduce((lambda a, b: a + b), l) 118 | 119 | @staticmethod 120 | def calc_Dxi_exp_x_matrix(x, i): 121 | return sympy.Matrix(3, 3, lambda r, c: 122 | sympy.diff(Se2.exp(x).matrix()[r, c], x[i])) 123 | 124 | @staticmethod 125 | def Dxi_exp_x_matrix_at_0(i): 126 | v = sophus.ZeroVector3() 127 | v[i] = 1 128 | return Se2.hat(v) 129 | 130 | @staticmethod 131 | def calc_Dxi_exp_x_matrix_at_0(x, i): 132 | return sympy.Matrix(3, 3, lambda r, c: 133 | sympy.diff(Se2.exp(x).matrix()[r, c], x[i]) 134 | ).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) 135 | 136 | 137 | class TestSe2(unittest.TestCase): 138 | def setUp(self): 139 | upsilon0, upsilon1, theta = sympy.symbols( 140 | 'upsilon[0], upsilon[1], theta', 141 | real=True) 142 | x, y = sympy.symbols('c[0] c[1]', real=True) 143 | p0, p1 = sympy.symbols('p0 p1', real=True) 144 | t0, t1 = sympy.symbols('t[0] t[1]', real=True) 145 | self.upsilon_theta = sophus.Vector3( 146 | upsilon0, upsilon1, theta) 147 | self.t = sophus.Vector2(t0, t1) 148 | self.a = Se2(sophus.So2(sophus.Complex(x, y)), self.t) 149 | self.p = sophus.Vector2(p0, p1) 150 | 151 | def test_exp_log(self): 152 | for v in [sophus.Vector3(0., 1, 0.5), 153 | sophus.Vector3(0.1, 0.1, 0.1), 154 | sophus.Vector3(0.01, 0.2, 0.03)]: 155 | w = Se2.exp(v).log() 156 | for i in range(0, 3): 157 | self.assertAlmostEqual(v[i], w[i]) 158 | 159 | def test_matrix(self): 160 | T_foo_bar = Se2.exp(self.upsilon_theta) 161 | Tmat_foo_bar = T_foo_bar.matrix() 162 | point_bar = self.p 163 | p1_foo = T_foo_bar * point_bar 164 | p2_foo = sophus.proj(Tmat_foo_bar * sophus.unproj(point_bar)) 165 | self.assertEqual(sympy.simplify(p1_foo - p2_foo), 166 | sophus.ZeroVector2()) 167 | 168 | def test_derivatives(self): 169 | self.assertEqual(sympy.simplify( 170 | Se2.calc_Dx_exp_x_at_0(self.upsilon_theta) - 171 | Se2.Dx_exp_x_at_0()), 172 | sympy.Matrix.zeros(4, 3)) 173 | for i in range(0, 4): 174 | self.assertEqual(sympy.simplify(Se2.calc_Dxi_x_matrix(self.a, i) - 175 | Se2.Dxi_x_matrix(self.a, i)), 176 | sympy.Matrix.zeros(3, 3)) 177 | for i in range(0, 3): 178 | self.assertEqual(sympy.simplify( 179 | Se2.Dxi_exp_x_matrix(self.upsilon_theta, i) - 180 | Se2.calc_Dxi_exp_x_matrix(self.upsilon_theta, i)), 181 | sympy.Matrix.zeros(3, 3)) 182 | self.assertEqual(sympy.simplify( 183 | Se2.Dxi_exp_x_matrix_at_0(i) - 184 | Se2.calc_Dxi_exp_x_matrix_at_0(self.upsilon_theta, i)), 185 | sympy.Matrix.zeros(3, 3)) 186 | 187 | def test_codegen(self): 188 | stream = sophus.cse_codegen(Se2.calc_Dx_exp_x(self.upsilon_theta)) 189 | filename = "cpp_gencode/Se2_Dx_exp_x.cpp" 190 | 191 | # set to true to generate codegen files 192 | if False: 193 | file = open(filename, "w") 194 | for line in stream: 195 | file.write(line) 196 | file.close() 197 | else: 198 | file = open(filename, "r") 199 | file_lines = file.readlines() 200 | for i, line in enumerate(stream): 201 | self.assertEqual(line, file_lines[i]) 202 | file.close() 203 | stream.close 204 | 205 | stream = sophus.cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0( 206 | self.upsilon_theta)) 207 | filename = "cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp" 208 | # set to true to generate codegen files 209 | if False: 210 | file = open(filename, "w") 211 | for line in stream: 212 | file.write(line) 213 | file.close() 214 | else: 215 | file = open(filename, "r") 216 | file_lines = file.readlines() 217 | for i, line in enumerate(stream): 218 | self.assertEqual(line, file_lines[i]) 219 | file.close() 220 | stream.close 221 | 222 | 223 | if __name__ == '__main__': 224 | unittest.main() 225 | -------------------------------------------------------------------------------- /py/sophus/se3.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import sys 3 | import unittest 4 | import sophus 5 | import functools 6 | 7 | 8 | class Se3: 9 | """ 3 dimensional group of rigid body transformations """ 10 | 11 | def __init__(self, so3, t): 12 | """ internally represented by a unit quaternion q and a translation 13 | 3-vector """ 14 | assert isinstance(so3, sophus.So3) 15 | assert isinstance(t, sympy.Matrix) 16 | assert t.shape == (3, 1), t.shape 17 | 18 | self.so3 = so3 19 | self.t = t 20 | 21 | @staticmethod 22 | def exp(v): 23 | """ exponential map """ 24 | upsilon = v[0:3, :] 25 | omega = sophus.Vector3(v[3], v[4], v[5]) 26 | so3 = sophus.So3.exp(omega) 27 | Omega = sophus.So3.hat(omega) 28 | Omega_sq = Omega * Omega 29 | theta = sympy.sqrt(sophus.squared_norm(omega)) 30 | V = (sympy.Matrix.eye(3) + 31 | (1 - sympy.cos(theta)) / (theta**2) * Omega + 32 | (theta - sympy.sin(theta)) / (theta**3) * Omega_sq) 33 | return Se3(so3, V * upsilon) 34 | 35 | def log(self): 36 | 37 | omega = self.so3.log() 38 | theta = sympy.sqrt(sophus.squared_norm(omega)) 39 | Omega = sophus.So3.hat(omega) 40 | 41 | half_theta = 0.5 * theta 42 | 43 | V_inv = sympy.Matrix.eye(3) - 0.5 * Omega + (1 - theta * sympy.cos( 44 | half_theta) / (2 * sympy.sin(half_theta))) / (theta * theta) *\ 45 | (Omega * Omega) 46 | upsilon = V_inv * self.t 47 | return upsilon.col_join(omega) 48 | 49 | def __repr__(self): 50 | return "Se3: [" + repr(self.so3) + " " + repr(self.t) 51 | 52 | @staticmethod 53 | def hat(v): 54 | upsilon = sophus.Vector3(v[0], v[1], v[2]) 55 | omega = sophus.Vector3(v[3], v[4], v[5]) 56 | return sophus.So3.hat(omega).\ 57 | row_join(upsilon).\ 58 | col_join(sympy.Matrix.zeros(1, 4)) 59 | 60 | def matrix(self): 61 | """ returns matrix representation """ 62 | R = self.so3.matrix() 63 | return (R.row_join(self.t)).col_join(sympy.Matrix(1, 4, [0, 0, 0, 1])) 64 | 65 | def __mul__(self, right): 66 | """ left-multiplication 67 | either rotation concatenation or point-transform """ 68 | if isinstance(right, sympy.Matrix): 69 | assert right.shape == (3, 1), right.shape 70 | return self.so3 * right + self.t 71 | elif isinstance(right, Se3): 72 | r = self.so3 * right.so3 73 | t = self.t + self.so3 * right.t 74 | return Se3(r, t) 75 | assert False, "unsupported type: {0}".format(type(right)) 76 | 77 | def __getitem__(self, key): 78 | """ We use the following convention [q0, q1, q2, q3, t0, t1, t2] """ 79 | assert (key >= 0 and key < 7) 80 | if key < 4: 81 | return self.so3[key] 82 | else: 83 | return self.t[key - 4] 84 | 85 | @staticmethod 86 | def calc_Dx_exp_x(x): 87 | return sympy.Matrix(7, 6, lambda r, c: 88 | sympy.diff(Se3.exp(x)[r], x[c])) 89 | 90 | @staticmethod 91 | def Dx_exp_x_at_0(): 92 | return sympy.Matrix([[0.0, 0.0, 0.0, 0.5, 0.0, 0.0], 93 | [0.0, 0.0, 0.0, 0.0, 0.5, 0.0], 94 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.5], 95 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 96 | [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], 97 | [0.0, 1.0, 0.0, 0.0, 0.0, 0.0], 98 | [0.0, 0.0, 1.0, 0.0, 0.0, 0.0]]) 99 | 100 | def calc_Dx_this_mul_exp_x_at_0(self, x): 101 | v = Se3.exp(x) 102 | return sympy.Matrix(7, 6, lambda r, c: 103 | sympy.diff((self * Se3.exp(x))[r], x[c])). \ 104 | subs(x[0], 0).subs(x[1], 0).subs(x[2], 0).\ 105 | subs(x[3], 0).subs(x[4], 0).limit(x[5], 0) 106 | 107 | @staticmethod 108 | def calc_Dx_exp_x_at_0(x): 109 | return Se3.calc_Dx_exp_x(x).subs(x[0], 0).subs(x[1], 0).subs(x[2], 0).\ 110 | subs(x[3], 0).subs(x[4], 0).limit(x[5], 0) 111 | 112 | @staticmethod 113 | def Dxi_x_matrix(x, i): 114 | if i < 4: 115 | return sophus.So3.Dxi_x_matrix(x, i).\ 116 | row_join(sympy.Matrix.zeros(3, 1)).\ 117 | col_join(sympy.Matrix.zeros(1, 4)) 118 | M = sympy.Matrix.zeros(4, 4) 119 | M[i - 4, 3] = 1 120 | return M 121 | 122 | @staticmethod 123 | def calc_Dxi_x_matrix(x, i): 124 | return sympy.Matrix(4, 4, lambda r, c: 125 | sympy.diff(x.matrix()[r, c], x[i])) 126 | 127 | @staticmethod 128 | def Dxi_exp_x_matrix(x, i): 129 | T = Se3.exp(x) 130 | Dx_exp_x = Se3.calc_Dx_exp_x(x) 131 | l = [Dx_exp_x[j, i] * Se3.Dxi_x_matrix(T, j) for j in range(0, 7)] 132 | return functools.reduce((lambda a, b: a + b), l) 133 | 134 | @staticmethod 135 | def calc_Dxi_exp_x_matrix(x, i): 136 | return sympy.Matrix(4, 4, lambda r, c: 137 | sympy.diff(Se3.exp(x).matrix()[r, c], x[i])) 138 | 139 | @staticmethod 140 | def Dxi_exp_x_matrix_at_0(i): 141 | v = sophus.ZeroVector6() 142 | v[i] = 1 143 | return Se3.hat(v) 144 | 145 | @staticmethod 146 | def calc_Dxi_exp_x_matrix_at_0(x, i): 147 | return sympy.Matrix(4, 4, lambda r, c: 148 | sympy.diff(Se3.exp(x).matrix()[r, c], x[i]) 149 | ).subs(x[0], 0).subs(x[1], 0).subs(x[2], 0).\ 150 | subs(x[3], 0).subs(x[4], 0).limit(x[5], 0) 151 | 152 | 153 | class TestSe3(unittest.TestCase): 154 | def setUp(self): 155 | upsilon0, upsilon1, upsilon2, omega0, omega1, omega2 = sympy.symbols( 156 | 'upsilon[0], upsilon[1], upsilon[2], omega[0], omega[1], omega[2]', 157 | real=True) 158 | x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True) 159 | p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True) 160 | t0, t1, t2 = sympy.symbols('t[0] t[1] t[2]', real=True) 161 | v = sophus.Vector3(v0, v1, v2) 162 | self.upsilon_omega = sophus.Vector6( 163 | upsilon0, upsilon1, upsilon2, omega0, omega1, omega2) 164 | self.t = sophus.Vector3(t0, t1, t2) 165 | self.a = Se3(sophus.So3(sophus.Quaternion(x, v)), self.t) 166 | self.p = sophus.Vector3(p0, p1, p2) 167 | 168 | def test_exp_log(self): 169 | for v in [sophus.Vector6(0., 1, 0.5, 2., 1, 0.5), 170 | sophus.Vector6(0.1, 0.1, 0.1, 0., 1, 0.5), 171 | sophus.Vector6(0.01, 0.2, 0.03, 0.01, 0.2, 0.03)]: 172 | w = Se3.exp(v).log() 173 | for i in range(0, 3): 174 | self.assertAlmostEqual(v[i], w[i]) 175 | 176 | def test_matrix(self): 177 | T_foo_bar = Se3.exp(self.upsilon_omega) 178 | Tmat_foo_bar = T_foo_bar.matrix() 179 | point_bar = self.p 180 | p1_foo = T_foo_bar * point_bar 181 | p2_foo = sophus.proj(Tmat_foo_bar * sophus.unproj(point_bar)) 182 | self.assertEqual(sympy.simplify(p1_foo - p2_foo), 183 | sophus.ZeroVector3()) 184 | 185 | def test_derivatives(self): 186 | self.assertEqual(sympy.simplify( 187 | Se3.calc_Dx_exp_x_at_0(self.upsilon_omega) - 188 | Se3.Dx_exp_x_at_0()), 189 | sympy.Matrix.zeros(7, 6)) 190 | 191 | for i in range(0, 7): 192 | self.assertEqual(sympy.simplify(Se3.calc_Dxi_x_matrix(self.a, i) - 193 | Se3.Dxi_x_matrix(self.a, i)), 194 | sympy.Matrix.zeros(4, 4)) 195 | for i in range(0, 6): 196 | self.assertEqual(sympy.simplify( 197 | Se3.Dxi_exp_x_matrix(self.upsilon_omega, i) - 198 | Se3.calc_Dxi_exp_x_matrix(self.upsilon_omega, i)), 199 | sympy.Matrix.zeros(4, 4)) 200 | self.assertEqual(sympy.simplify( 201 | Se3.Dxi_exp_x_matrix_at_0(i) - 202 | Se3.calc_Dxi_exp_x_matrix_at_0(self.upsilon_omega, i)), 203 | sympy.Matrix.zeros(4, 4)) 204 | 205 | def test_codegen(self): 206 | stream = sophus.cse_codegen(self.a.calc_Dx_exp_x(self.upsilon_omega)) 207 | filename = "cpp_gencode/Se3_Dx_exp_x.cpp" 208 | # set to true to generate codegen files 209 | if True: 210 | file = open(filename, "w") 211 | for line in stream: 212 | file.write(line) 213 | file.close() 214 | else: 215 | file = open(filename, "r") 216 | file_lines = file.readlines() 217 | for i, line in enumerate(stream): 218 | self.assertEqual(line, file_lines[i]) 219 | file.close() 220 | stream.close 221 | 222 | stream = sophus.cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0( 223 | self.upsilon_omega)) 224 | filename = "cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp" 225 | # set to true to generate codegen files 226 | if True: 227 | file = open(filename, "w") 228 | for line in stream: 229 | file.write(line) 230 | file.close() 231 | else: 232 | file = open(filename, "r") 233 | file_lines = file.readlines() 234 | for i, line in enumerate(stream): 235 | self.assertEqual(line, file_lines[i]) 236 | file.close() 237 | stream.close 238 | 239 | 240 | if __name__ == '__main__': 241 | unittest.main() 242 | -------------------------------------------------------------------------------- /py/sophus/so2.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import sys 3 | import unittest 4 | import sophus 5 | import functools 6 | 7 | 8 | class So2: 9 | """ 2 dimensional group of orthogonal matrices with determinant 1 """ 10 | 11 | def __init__(self, z): 12 | """ internally represented by a unit complex number z """ 13 | self.z = z 14 | 15 | @staticmethod 16 | def exp(theta): 17 | """ exponential map """ 18 | return So2( 19 | sophus.Complex( 20 | sympy.cos(theta), 21 | sympy.sin(theta))) 22 | 23 | def log(self): 24 | """ logarithmic map""" 25 | return sympy.atan2(self.z.imag, self.z.real) 26 | 27 | def __repr__(self): 28 | return "So2:" + repr(self.z) 29 | 30 | @staticmethod 31 | def hat(theta): 32 | return sympy.Matrix([[0, -theta], 33 | [theta, 0]]) 34 | 35 | def matrix(self): 36 | """ returns matrix representation """ 37 | return sympy.Matrix([ 38 | [self.z.real, -self.z.imag], 39 | [self.z.imag, self.z.real]]) 40 | 41 | def __mul__(self, right): 42 | """ left-multiplication 43 | either rotation concatenation or point-transform """ 44 | if isinstance(right, sympy.Matrix): 45 | assert right.shape == (2, 1), right.shape 46 | return self.matrix() * right 47 | elif isinstance(right, So2): 48 | return So2(self.z * right.z) 49 | assert False, "unsupported type: {0}".format(type(right)) 50 | 51 | def __getitem__(self, key): 52 | return self.z[key] 53 | 54 | @staticmethod 55 | def calc_Dx_exp_x(x): 56 | return sympy.Matrix(2, 1, lambda r, c: 57 | sympy.diff(So2.exp(x)[r], x)) 58 | 59 | @staticmethod 60 | def Dx_exp_x_at_0(): 61 | return sympy.Matrix([0, 1]) 62 | 63 | @staticmethod 64 | def calc_Dx_exp_x_at_0(x): 65 | return So2.calc_Dx_exp_x(x).limit(x, 0) 66 | 67 | def calc_Dx_this_mul_exp_x_at_0(self, x): 68 | return sympy.Matrix(2, 1, lambda r, c: 69 | sympy.diff((self * So2.exp(x))[r], x))\ 70 | .limit(x, 0) 71 | 72 | @staticmethod 73 | def Dxi_x_matrix(x, i): 74 | if i == 0: 75 | return sympy.Matrix([[1, 0], 76 | [0, 1]]) 77 | if i == 1: 78 | return sympy.Matrix([[0, -1], 79 | [1, 0]]) 80 | 81 | @staticmethod 82 | def calc_Dxi_x_matrix(x, i): 83 | return sympy.Matrix(2, 2, lambda r, c: 84 | sympy.diff(x.matrix()[r, c], x[i])) 85 | 86 | @staticmethod 87 | def Dx_exp_x_matrix(x): 88 | R = So2.exp(x) 89 | Dx_exp_x = So2.calc_Dx_exp_x(x) 90 | l = [Dx_exp_x[j] * So2.Dxi_x_matrix(R, j) for j in [0, 1]] 91 | return functools.reduce((lambda a, b: a + b), l) 92 | 93 | @staticmethod 94 | def calc_Dx_exp_x_matrix(x): 95 | return sympy.Matrix(2, 2, lambda r, c: 96 | sympy.diff(So2.exp(x).matrix()[r, c], x)) 97 | 98 | @staticmethod 99 | def Dx_exp_x_matrix_at_0(): 100 | return So2.hat(1) 101 | 102 | @staticmethod 103 | def calc_Dx_exp_x_matrix_at_0(x): 104 | return sympy.Matrix(2, 2, lambda r, c: 105 | sympy.diff(So2.exp(x).matrix()[r, c], x) 106 | ).limit(x, 0) 107 | 108 | 109 | class TestSo2(unittest.TestCase): 110 | def setUp(self): 111 | self.theta = sympy.symbols( 112 | 'theta', real=True) 113 | x, y = sympy.symbols('c[0] c[1]', real=True) 114 | p0, p1 = sympy.symbols('p0 p1', real=True) 115 | self.a = So2(sophus.Complex(x, y)) 116 | self.p = sophus.Vector2(p0, p1) 117 | 118 | def test_exp_log(self): 119 | for theta in [0., 0.5, 0.1]: 120 | w = So2.exp(theta).log() 121 | self.assertAlmostEqual(theta, w) 122 | 123 | def test_matrix(self): 124 | R_foo_bar = So2.exp(self.theta) 125 | Rmat_foo_bar = R_foo_bar.matrix() 126 | point_bar = self.p 127 | p1_foo = R_foo_bar * point_bar 128 | p2_foo = Rmat_foo_bar * point_bar 129 | self.assertEqual(sympy.simplify(p1_foo - p2_foo), 130 | sophus.ZeroVector2()) 131 | 132 | def test_derivatives(self): 133 | self.assertEqual(sympy.simplify(So2.calc_Dx_exp_x_at_0(self.theta) - 134 | So2.Dx_exp_x_at_0()), 135 | sympy.Matrix.zeros(2, 1)) 136 | for i in [0, 1]: 137 | self.assertEqual(sympy.simplify(So2.calc_Dxi_x_matrix(self.a, i) - 138 | So2.Dxi_x_matrix(self.a, i)), 139 | sympy.Matrix.zeros(2, 2)) 140 | 141 | self.assertEqual(sympy.simplify( 142 | So2.Dx_exp_x_matrix(self.theta) - 143 | So2.calc_Dx_exp_x_matrix(self.theta)), 144 | sympy.Matrix.zeros(2, 2)) 145 | self.assertEqual(sympy.simplify( 146 | So2.Dx_exp_x_matrix_at_0() - 147 | So2.calc_Dx_exp_x_matrix_at_0(self.theta)), 148 | sympy.Matrix.zeros(2, 2)) 149 | 150 | def test_codegen(self): 151 | stream = sophus.cse_codegen(So2.calc_Dx_exp_x(self.theta)) 152 | filename = "cpp_gencode/So2_Dx_exp_x.cpp" 153 | # set to true to generate codegen files 154 | if False: 155 | file = open(filename, "w") 156 | for line in stream: 157 | file.write(line) 158 | file.close() 159 | else: 160 | file = open(filename, "r") 161 | file_lines = file.readlines() 162 | for i, line in enumerate(stream): 163 | self.assertEqual(line, file_lines[i]) 164 | file.close() 165 | stream.close 166 | 167 | stream = sophus.cse_codegen( 168 | self.a.calc_Dx_this_mul_exp_x_at_0(self.theta)) 169 | filename = "cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp" 170 | # set to true to generate codegen files 171 | if False: 172 | file = open(filename, "w") 173 | for line in stream: 174 | file.write(line) 175 | file.close() 176 | else: 177 | file = open(filename, "r") 178 | file_lines = file.readlines() 179 | for i, line in enumerate(stream): 180 | self.assertEqual(line, file_lines[i]) 181 | file.close() 182 | stream.close 183 | 184 | 185 | if __name__ == '__main__': 186 | unittest.main() 187 | -------------------------------------------------------------------------------- /py/sophus/so3.py: -------------------------------------------------------------------------------- 1 | import sympy 2 | import sys 3 | import unittest 4 | import sophus 5 | import functools 6 | 7 | 8 | class So3: 9 | """ 3 dimensional group of orthogonal matrices with determinant 1 """ 10 | 11 | def __init__(self, q): 12 | """ internally represented by a unit quaternion q """ 13 | self.q = q 14 | 15 | @staticmethod 16 | def exp(v): 17 | """ exponential map """ 18 | theta_sq = sophus.squared_norm(v) 19 | theta = sympy.sqrt(theta_sq) 20 | return So3( 21 | sophus.Quaternion( 22 | sympy.cos(0.5 * theta), 23 | sympy.sin(0.5 * theta) / theta * v)) 24 | 25 | def log(self): 26 | """ logarithmic map""" 27 | n = sympy.sqrt(sophus.squared_norm(self.q.vec)) 28 | return 2 * sympy.atan(n / self.q.real) / n * self.q.vec 29 | 30 | def __repr__(self): 31 | return "So3:" + repr(self.q) 32 | 33 | @staticmethod 34 | def hat(o): 35 | return sympy.Matrix([[0, -o[2], o[1]], 36 | [o[2], 0, -o[0]], 37 | [-o[1], o[0], 0]]) 38 | 39 | def matrix(self): 40 | """ returns matrix representation """ 41 | return sympy.Matrix([[ 42 | 1 - 2 * self.q.vec[1]**2 - 2 * self.q.vec[2]**2, 43 | 2 * self.q.vec[0] * self.q.vec[1] - 44 | 2 * self.q.vec[2] * self.q[3], 45 | 2 * self.q.vec[0] * self.q.vec[2] + 46 | 2 * self.q.vec[1] * self.q[3] 47 | ], [ 48 | 2 * self.q.vec[0] * self.q.vec[1] + 49 | 2 * self.q.vec[2] * self.q[3], 50 | 1 - 2 * self.q.vec[0]**2 - 2 * self.q.vec[2]**2, 51 | 2 * self.q.vec[1] * self.q.vec[2] - 52 | 2 * self.q.vec[0] * self.q[3] 53 | ], [ 54 | 2 * self.q.vec[0] * self.q.vec[2] - 55 | 2 * self.q.vec[1] * self.q[3], 56 | 2 * self.q.vec[1] * self.q.vec[2] + 57 | 2 * self.q.vec[0] * self.q[3], 58 | 1 - 2 * self.q.vec[0]**2 - 2 * self.q.vec[1]**2 59 | ]]) 60 | 61 | def __mul__(self, right): 62 | """ left-multiplication 63 | either rotation concatenation or point-transform """ 64 | if isinstance(right, sympy.Matrix): 65 | assert right.shape == (3, 1), right.shape 66 | return (self.q * sophus.Quaternion(0, right) * self.q.conj()).vec 67 | elif isinstance(right, So3): 68 | return So3(self.q * right.q) 69 | assert False, "unsupported type: {0}".format(type(right)) 70 | 71 | def __getitem__(self, key): 72 | return self.q[key] 73 | 74 | @staticmethod 75 | def calc_Dx_exp_x(x): 76 | return sympy.Matrix(4, 3, lambda r, c: 77 | sympy.diff(So3.exp(x)[r], x[c])) 78 | 79 | @staticmethod 80 | def Dx_exp_x_at_0(): 81 | return sympy.Matrix([[0.5, 0.0, 0.0], 82 | [0.0, 0.5, 0.0], 83 | [0.0, 0.0, 0.5], 84 | [0.0, 0.0, 0.0]]) 85 | 86 | @staticmethod 87 | def calc_Dx_exp_x_at_0(x): 88 | return So3.calc_Dx_exp_x(x).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) 89 | 90 | def calc_Dx_this_mul_exp_x_at_0(self, x): 91 | return sympy.Matrix(4, 3, lambda r, c: 92 | sympy.diff((self * So3.exp(x))[r], x[c]))\ 93 | .subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) 94 | 95 | def calc_Dx_exp_x_mul_this_at_0(self, x): 96 | return sympy.Matrix(3, 4, lambda r, c: 97 | sympy.diff((self * So3.exp(x))[c], x[r, 0]))\ 98 | .subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) 99 | 100 | @staticmethod 101 | def Dxi_x_matrix(x, i): 102 | if i == 0: 103 | return sympy.Matrix([[0, 2 * x[1], 2 * x[2]], 104 | [2 * x[1], -4 * x[0], -2 * x[3]], 105 | [2 * x[2], 2 * x[3], -4 * x[0]]]) 106 | if i == 1: 107 | return sympy.Matrix([[-4 * x[1], 2 * x[0], 2 * x[3]], 108 | [2 * x[0], 0, 2 * x[2]], 109 | [-2 * x[3], 2 * x[2], -4 * x[1]]]) 110 | if i == 2: 111 | return sympy.Matrix([[-4 * x[2], -2 * x[3], 2 * x[0]], 112 | [2 * x[3], -4 * x[2], 2 * x[1]], 113 | [2 * x[0], 2 * x[1], 0]]) 114 | if i == 3: 115 | return sympy.Matrix([[0, -2 * x[2], 2 * x[1]], 116 | [2 * x[2], 0, -2 * x[0]], 117 | [-2 * x[1], 2 * x[0], 0]]) 118 | 119 | @staticmethod 120 | def calc_Dxi_x_matrix(x, i): 121 | return sympy.Matrix(3, 3, lambda r, c: 122 | sympy.diff(x.matrix()[r, c], x[i])) 123 | 124 | @staticmethod 125 | def Dxi_exp_x_matrix(x, i): 126 | R = So3.exp(x) 127 | Dx_exp_x = So3.calc_Dx_exp_x(x) 128 | l = [Dx_exp_x[j, i] * So3.Dxi_x_matrix(R, j) for j in [0, 1, 2, 3]] 129 | return functools.reduce((lambda a, b: a + b), l) 130 | 131 | @staticmethod 132 | def calc_Dxi_exp_x_matrix(x, i): 133 | return sympy.Matrix(3, 3, lambda r, c: 134 | sympy.diff(So3.exp(x).matrix()[r, c], x[i])) 135 | 136 | @staticmethod 137 | def Dxi_exp_x_matrix_at_0(i): 138 | v = sophus.ZeroVector3() 139 | v[i] = 1 140 | return So3.hat(v) 141 | 142 | @staticmethod 143 | def calc_Dxi_exp_x_matrix_at_0(x, i): 144 | return sympy.Matrix(3, 3, lambda r, c: 145 | sympy.diff(So3.exp(x).matrix()[r, c], x[i]) 146 | ).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) 147 | 148 | 149 | class TestSo3(unittest.TestCase): 150 | def setUp(self): 151 | omega0, omega1, omega2 = sympy.symbols( 152 | 'omega[0], omega[1], omega[2]', real=True) 153 | x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True) 154 | p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True) 155 | v = sophus.Vector3(v0, v1, v2) 156 | self.omega = sophus.Vector3(omega0, omega1, omega2) 157 | self.a = So3(sophus.Quaternion(x, v)) 158 | self.p = sophus.Vector3(p0, p1, p2) 159 | 160 | def test_exp_log(self): 161 | for o in [sophus.Vector3(0., 1, 0.5), 162 | sophus.Vector3(0.1, 0.1, 0.1), 163 | sophus.Vector3(0.01, 0.2, 0.03)]: 164 | w = So3.exp(o).log() 165 | for i in range(0, 3): 166 | self.assertAlmostEqual(o[i], w[i]) 167 | 168 | def test_matrix(self): 169 | R_foo_bar = So3.exp(self.omega) 170 | Rmat_foo_bar = R_foo_bar.matrix() 171 | point_bar = self.p 172 | p1_foo = R_foo_bar * point_bar 173 | p2_foo = Rmat_foo_bar * point_bar 174 | self.assertEqual(sympy.simplify(p1_foo - p2_foo), 175 | sophus.ZeroVector3()) 176 | 177 | def test_derivatives(self): 178 | self.assertEqual(sympy.simplify(So3.calc_Dx_exp_x_at_0(self.omega) - 179 | So3.Dx_exp_x_at_0()), 180 | sympy.Matrix.zeros(4, 3)) 181 | 182 | for i in [0, 1, 2, 3]: 183 | self.assertEqual(sympy.simplify(So3.calc_Dxi_x_matrix(self.a, i) - 184 | So3.Dxi_x_matrix(self.a, i)), 185 | sympy.Matrix.zeros(3, 3)) 186 | for i in [0, 1, 2]: 187 | self.assertEqual(sympy.simplify( 188 | So3.Dxi_exp_x_matrix(self.omega, i) - 189 | So3.calc_Dxi_exp_x_matrix(self.omega, i)), 190 | sympy.Matrix.zeros(3, 3)) 191 | self.assertEqual(sympy.simplify( 192 | So3.Dxi_exp_x_matrix_at_0(i) - 193 | So3.calc_Dxi_exp_x_matrix_at_0(self.omega, i)), 194 | sympy.Matrix.zeros(3, 3)) 195 | 196 | def test_codegen(self): 197 | stream = sophus.cse_codegen(So3.calc_Dx_exp_x(self.omega)) 198 | filename = "cpp_gencode/So3_Dx_exp_x.cpp" 199 | # set to true to generate codegen files 200 | if False: 201 | file = open(filename, "w") 202 | for line in stream: 203 | file.write(line) 204 | file.close() 205 | else: 206 | file = open(filename, "r") 207 | file_lines = file.readlines() 208 | for i, line in enumerate(stream): 209 | self.assertEqual(line, file_lines[i]) 210 | file.close() 211 | stream.close 212 | 213 | stream = sophus.cse_codegen( 214 | self.a.calc_Dx_this_mul_exp_x_at_0(self.omega)) 215 | filename = "cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp" 216 | # set to true to generate codegen files 217 | if False: 218 | file = open(filename, "w") 219 | for line in stream: 220 | file.write(line) 221 | file.close() 222 | else: 223 | file = open(filename, "r") 224 | file_lines = file.readlines() 225 | for i, line in enumerate(stream): 226 | self.assertEqual(line, file_lines[i]) 227 | file.close() 228 | stream.close 229 | 230 | 231 | if __name__ == '__main__': 232 | unittest.main() 233 | -------------------------------------------------------------------------------- /py/sophus/so3_codegen.py: -------------------------------------------------------------------------------- 1 | import sophus 2 | from sympy.utilities.codegen import codegen 3 | import sympy 4 | -------------------------------------------------------------------------------- /run_format.sh: -------------------------------------------------------------------------------- 1 | find . -iname *.hpp -o -iname *.cpp | xargs clang-format -i 2 | -------------------------------------------------------------------------------- /scripts/install_linux_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | wget https://cmake.org/files/v3.9/cmake-3.9.0-Linux-x86_64.sh 7 | chmod +x cmake-3.9.0-Linux-x86_64.sh 8 | set +x # echo off 9 | sudo ./cmake-3.9.0-Linux-x86_64.sh --skip-license --prefix=/usr/local 10 | set -x # echo on 11 | sudo update-alternatives --install /usr/bin/cmake cmake /usr/local/bin/cmake 1 --force 12 | cmake --version 13 | 14 | sudo apt-get -qq update 15 | sudo apt-get install gfortran libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev 16 | sudo unlink /usr/bin/gcc && sudo ln -s /usr/bin/gcc-5 /usr/bin/gcc 17 | sudo unlink /usr/bin/g++ && sudo ln -s /usr/bin/g++-5 /usr/bin/g++ 18 | gcc --version 19 | g++ --version 20 | wget http://bitbucket.org/eigen/eigen/get/3.3.4.tar.bz2 21 | tar xvf 3.3.4.tar.bz2 22 | mkdir build-eigen 23 | cd build-eigen 24 | cmake ../eigen-eigen-5a0156e40feb -DEIGEN_DEFAULT_TO_ROW_MAJOR=$ROW_MAJOR_DEFAULT 25 | sudo make install 26 | git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver 27 | cd ceres-solver 28 | git reset --hard afe93546b67cee0ad205fe8044325646ed5deea9 29 | mkdir build 30 | cd build 31 | ccache -M 50G 32 | ccache -s 33 | cmake -DCXX11=On -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DOPENMP=Off .. 34 | make -j3 35 | sudo make install 36 | -------------------------------------------------------------------------------- /scripts/install_osx_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | brew update 6 | brew install eigen 7 | brew install glog 8 | brew install suite-sparse 9 | brew install ccache 10 | export PATH="/usr/local/opt/ccache/libexec:$PATH" 11 | whereis ccache 12 | git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver 13 | cd ceres-solver 14 | git reset --hard afe93546b67cee0ad205fe8044325646ed5deea9 15 | mkdir build 16 | cd build 17 | ccache -M 50G 18 | ccache -s 19 | cmake -DCXX11=On -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. 20 | make -j3 21 | make install -------------------------------------------------------------------------------- /scripts/run_cpp_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | mkdir -p build 7 | cd build 8 | pwd 9 | cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=$BUILD_TYPE .. 10 | make -j2 11 | make CTEST_OUTPUT_ON_FAILURE=1 test 12 | -------------------------------------------------------------------------------- /scripts/run_robust_pcl_reconstruction_example_cauchy_no_sem.sh: -------------------------------------------------------------------------------- 1 | DIRECTORY=`dirname $0` 2 | 3 | ROOTDIRECTORY=$DIRECTORY/.. 4 | 5 | $ROOTDIRECTORY/build/test/ceres/cauchy_em_no_sem $ROOTDIRECTORY/data/cauchy/odometry.txt $ROOTDIRECTORY/data/cauchy/loop_point.txt $ROOTDIRECTORY/data/cauchy/init.txt $ROOTDIRECTORY/data/cauchy/semantic_p_matrix.txt $ROOTDIRECTORY/data/cauchy/poses_new_no_sem.txt $ROOTDIRECTORY/data/cauchy/keep_new.txt 6 | -------------------------------------------------------------------------------- /scripts/run_robust_pcl_reconstruction_example_cauchy_two_EM.sh: -------------------------------------------------------------------------------- 1 | DIRECTORY=`dirname $0` 2 | 3 | ROOTDIRECTORY=$DIRECTORY/.. 4 | 5 | $ROOTDIRECTORY/build/test/ceres/cauchy_em_two_EM $ROOTDIRECTORY/data/cauchy/odometry.txt $ROOTDIRECTORY/data/cauchy/loop_point.txt $ROOTDIRECTORY/data/cauchy/init.txt $ROOTDIRECTORY/data/cauchy/semantic_p_matrix.txt $ROOTDIRECTORY/data/cauchy/poses_two_EM.txt $ROOTDIRECTORY/data/cauchy/keep_new.txt 6 | -------------------------------------------------------------------------------- /sophus/average.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_AVERAGE_HPP 2 | #define SOPHUS_AVERAGE_HPP 3 | 4 | #include "common.hpp" 5 | #include "rxso2.hpp" 6 | #include "rxso3.hpp" 7 | #include "se2.hpp" 8 | #include "se3.hpp" 9 | #include "sim2.hpp" 10 | #include "sim3.hpp" 11 | #include "so2.hpp" 12 | #include "so3.hpp" 13 | 14 | namespace Sophus { 15 | 16 | template 17 | optional iterativeMean( 18 | SequenceContainer const& foo_Ts_bar, int max_num_iterations) { 19 | size_t N = foo_Ts_bar.size(); 20 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 21 | 22 | using Group = typename SequenceContainer::value_type; 23 | using Scalar = typename Group::Scalar; 24 | using Tangent = typename Group::Tangent; 25 | 26 | // This implements the algorithm in the beginning of Sec. 4.2 in 27 | // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. 28 | Group foo_T_average = foo_Ts_bar.front(); 29 | Scalar w = Scalar(1. / N); 30 | for (int i = 0; i < max_num_iterations; ++i) { 31 | Tangent average; 32 | setToZero(average); 33 | for (Group const& foo_T_bar : foo_Ts_bar) { 34 | average += w * (foo_T_average.inverse() * foo_T_bar).log(); 35 | } 36 | Group foo_T_newaverage = foo_T_average * Group::exp(average); 37 | if (squaredNorm( 38 | (foo_T_newaverage.inverse() * foo_T_average).log()) < 39 | Constants::epsilon()) { 40 | return foo_T_newaverage; 41 | } 42 | 43 | foo_T_average = foo_T_newaverage; 44 | } 45 | // LCOV_EXCL_START 46 | return nullopt; 47 | // LCOV_EXCL_STOP 48 | } 49 | 50 | // Mean implementation for SO(2). 51 | template 53 | enable_if_t< 54 | std::is_same>::value, 55 | optional> 56 | average(SequenceContainer const& foo_Ts_bar) { 57 | // This implements rotational part of Proposition 12 from Sec. 6.2 of 58 | // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. 59 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 60 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 61 | SO2 foo_T_average = foo_Ts_bar.front(); 62 | Scalar w = Scalar(1. / N); 63 | 64 | Scalar average(0); 65 | for (SO2 const& foo_T_bar : foo_Ts_bar) { 66 | average += w * (foo_T_average.inverse() * foo_T_bar).log(); 67 | } 68 | return foo_T_average * SO2::exp(average); 69 | } 70 | 71 | // Mean implementation for RxSO(2). 72 | template 74 | enable_if_t< 75 | std::is_same>::value, 76 | optional> 77 | average(SequenceContainer const& foo_Ts_bar) { 78 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 79 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 80 | RxSO2 foo_T_average = foo_Ts_bar.front(); 81 | Scalar w = Scalar(1. / N); 82 | 83 | Vector2 average(Scalar(0), Scalar(0)); 84 | for (RxSO2 const& foo_T_bar : foo_Ts_bar) { 85 | average += w * (foo_T_average.inverse() * foo_T_bar).log(); 86 | } 87 | return foo_T_average * RxSO2::exp(average); 88 | } 89 | 90 | namespace details { 91 | template 92 | void getQuaternion(T const&); 93 | 94 | template 95 | Eigen::Quaternion getUnitQuaternion(SO3 const& R) { 96 | return R.unit_quaternion(); 97 | } 98 | 99 | template 100 | Eigen::Quaternion getUnitQuaternion(RxSO3 const& sR) { 101 | return sR.so3().unit_quaternion(); 102 | } 103 | 104 | template 106 | Eigen::Quaternion averageUnitQuaternion( 107 | SequenceContainer const& foo_Ts_bar) { 108 | // This: http://stackoverflow.com/a/27410865/1221742 109 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 110 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 111 | Eigen::Matrix Q(4, N); 112 | int i = 0; 113 | Scalar w = Scalar(1. / N); 114 | for (auto const& foo_T_bar : foo_Ts_bar) { 115 | Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs(); 116 | ++i; 117 | } 118 | 119 | Eigen::Matrix QQt = Q * Q.transpose(); 120 | // TODO: Figure out why we can't use SelfAdjointEigenSolver here. 121 | Eigen::EigenSolver> es(QQt); 122 | 123 | std::complex max_eigenvalue = es.eigenvalues()[0]; 124 | Eigen::Matrix, 4, 1> max_eigenvector = 125 | es.eigenvectors().col(0); 126 | 127 | for (int i = 1; i < 4; i++) { 128 | if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) { 129 | max_eigenvalue = es.eigenvalues()[i]; 130 | max_eigenvector = es.eigenvectors().col(i); 131 | } 132 | } 133 | Eigen::Quaternion quat; 134 | quat.coeffs() << // 135 | max_eigenvector[0].real(), // 136 | max_eigenvector[1].real(), // 137 | max_eigenvector[2].real(), // 138 | max_eigenvector[3].real(); 139 | return quat; 140 | } 141 | } // namespace details 142 | 143 | // Mean implementation for SO(3). 144 | // 145 | // TODO: Detect degenerated cases and return nullopt. 146 | template 148 | enable_if_t< 149 | std::is_same>::value, 150 | optional> 151 | average(SequenceContainer const& foo_Ts_bar) { 152 | return SO3(details::averageUnitQuaternion(foo_Ts_bar)); 153 | } 154 | 155 | // Mean implementation for R x SO(3). 156 | template 158 | enable_if_t< 159 | std::is_same>::value, 160 | optional> 161 | average(SequenceContainer const& foo_Ts_bar) { 162 | size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); 163 | 164 | SOPHUS_ENSURE(N >= 1, "N must be >= 1."); 165 | Scalar scale_sum = Scalar(0); 166 | using std::exp; 167 | using std::log; 168 | for (RxSO3 const& foo_T_bar : foo_Ts_bar) { 169 | scale_sum += log(foo_T_bar.scale()); 170 | } 171 | return RxSO3(exp(scale_sum / Scalar(N)), 172 | SO3(details::averageUnitQuaternion(foo_Ts_bar))); 173 | } 174 | 175 | template 177 | enable_if_t< 178 | std::is_same>::value, 179 | optional> 180 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 181 | // TODO: Implement Proposition 12 from Sec. 6.2 of 182 | // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. 183 | return iterativeMean(foo_Ts_bar, max_num_iterations); 184 | } 185 | 186 | template 188 | enable_if_t< 189 | std::is_same>::value, 190 | optional> 191 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 192 | return iterativeMean(foo_Ts_bar, max_num_iterations); 193 | } 194 | 195 | template 197 | enable_if_t< 198 | std::is_same>::value, 199 | optional> 200 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 201 | return iterativeMean(foo_Ts_bar, max_num_iterations); 202 | } 203 | 204 | template 206 | enable_if_t< 207 | std::is_same>::value, 208 | optional> 209 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { 210 | return iterativeMean(foo_Ts_bar, max_num_iterations); 211 | } 212 | 213 | } // namespace Sophus 214 | 215 | #endif // SOPHUS_AVERAGE_HPP 216 | -------------------------------------------------------------------------------- /sophus/common.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_COMMON_HPP 2 | #define SOPHUS_COMMON_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | // following boost's assert.hpp 14 | #undef SOPHUS_ENSURE 15 | 16 | // ENSURES are similar to ASSERTS, but they are always checked for (including in 17 | // release builds). At the moment there are no ASSERTS in Sophus which should 18 | // only be used for checks which are performance critical. 19 | 20 | #ifdef __GNUC__ 21 | #define SOPHUS_FUNCTION __PRETTY_FUNCTION__ 22 | #elif (_MSC_VER >= 1310) 23 | #define SOPHUS_FUNCTION __FUNCTION__ 24 | #else 25 | #define SOPHUS_FUNCTION "unknown" 26 | #endif 27 | 28 | // Make sure this compiles with older versions of Eigen which do not have 29 | // EIGEN_DEVICE_FUNC defined. 30 | #ifndef EIGEN_DEVICE_FUNC 31 | #define EIGEN_DEVICE_FUNC 32 | #endif 33 | 34 | #define SOPHUS_FUNC EIGEN_DEVICE_FUNC 35 | 36 | namespace Sophus { 37 | namespace details { 38 | 39 | // Following: http://stackoverflow.com/a/22759544 40 | template 41 | class IsStreamable { 42 | private: 43 | template 44 | static auto test(int) 45 | -> decltype(std::declval() << std::declval(), 46 | std::true_type()); 47 | 48 | template 49 | static auto test(...) -> std::false_type; 50 | 51 | public: 52 | static bool const value = decltype(test(0))::value; 53 | }; 54 | 55 | template 56 | class ArgToStream { 57 | public: 58 | static void impl(std::stringstream& stream, T&& arg) { 59 | stream << std::forward(arg); 60 | } 61 | }; 62 | 63 | inline void FormatStream(std::stringstream& stream, char const* text) { 64 | stream << text; 65 | return; 66 | } 67 | 68 | // Following: http://en.cppreference.com/w/cpp/language/parameter_pack 69 | template 70 | void FormatStream(std::stringstream& stream, char const* text, T&& arg, 71 | Args&&... args) { 72 | static_assert(IsStreamable::value, 73 | "One of the args has no ostream overload!"); 74 | for (; *text != '\0'; ++text) { 75 | if (*text == '%') { 76 | ArgToStream::impl(stream, std::forward(arg)); 77 | FormatStream(stream, text + 1, std::forward(args)...); 78 | return; 79 | } 80 | stream << *text; 81 | } 82 | stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1 83 | << " args unused."; 84 | return; 85 | } 86 | 87 | template 88 | std::string FormatString(char const* text, Args&&... args) { 89 | std::stringstream stream; 90 | FormatStream(stream, text, std::forward(args)...); 91 | return stream.str(); 92 | } 93 | 94 | inline std::string FormatString() { return std::string(); } 95 | } // namespace details 96 | } // namespace Sophus 97 | 98 | #if defined(SOPHUS_DISABLE_ENSURES) 99 | 100 | #define SOPHUS_ENSURE(expr, ...) ((void)0) 101 | 102 | #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER) 103 | 104 | namespace Sophus { 105 | void ensureFailed(char const* function, char const* file, int line, 106 | char const* description); 107 | } 108 | 109 | #define SOPHUS_ENSURE(expr, ...) \ 110 | ((expr) ? ((void)0) \ 111 | : ::Sophus::ensureFailed( \ 112 | SOPHUS_FUNCTION, __FILE__, __LINE__, \ 113 | Sophus::details::FormatString(##__VA_ARGS__).c_str())) 114 | #else 115 | // LCOV_EXCL_START 116 | 117 | namespace Sophus { 118 | template 119 | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line, 120 | char const* description, Args&&... args) { 121 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 122 | function, file, line); 123 | #ifdef __CUDACC__ 124 | std::printf("%s", description); 125 | #else 126 | std::cout << details::FormatString(description, std::forward(args)...) 127 | << std::endl; 128 | std::abort(); 129 | #endif 130 | } 131 | } // namespace Sophus 132 | 133 | // LCOV_EXCL_STOP 134 | #define SOPHUS_ENSURE(expr, ...) \ 135 | ((expr) ? ((void)0) \ 136 | : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \ 137 | ##__VA_ARGS__)) 138 | #endif 139 | 140 | namespace Sophus { 141 | 142 | template 143 | struct Constants { 144 | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); } 145 | 146 | SOPHUS_FUNC static Scalar epsilonSqrt() { 147 | using std::sqrt; 148 | return sqrt(epsilon()); 149 | } 150 | 151 | SOPHUS_FUNC static Scalar pi() { return Scalar(M_PI); } 152 | }; 153 | 154 | template <> 155 | struct Constants { 156 | SOPHUS_FUNC static float constexpr epsilon() { 157 | return static_cast(1e-5); 158 | } 159 | 160 | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); } 161 | 162 | SOPHUS_FUNC static float constexpr pi() { return static_cast(M_PI); } 163 | }; 164 | 165 | // Leightweight optional implementation which require ``T`` to have a 166 | // default constructor. 167 | // 168 | // TODO: Replace with std::optional once Sophus moves to c++17. 169 | // 170 | struct nullopt_t { 171 | explicit constexpr nullopt_t() {} 172 | }; 173 | 174 | constexpr nullopt_t nullopt{}; 175 | template 176 | 177 | class optional { 178 | public: 179 | optional() : is_valid_(false) {} 180 | 181 | optional(nullopt_t) : is_valid_(false) {} 182 | 183 | optional(T const& type) : type_(type), is_valid_(true) {} 184 | 185 | explicit operator bool() const { return is_valid_; } 186 | 187 | T const* operator->() const { 188 | SOPHUS_ENSURE(is_valid_, "must be valid"); 189 | return &type_; 190 | } 191 | 192 | T* operator->() { 193 | SOPHUS_ENSURE(is_valid_, "must be valid"); 194 | return &type_; 195 | } 196 | 197 | T const& operator*() const { 198 | SOPHUS_ENSURE(is_valid_, "must be valid"); 199 | return type_; 200 | } 201 | 202 | T& operator*() { 203 | SOPHUS_ENSURE(is_valid_, "must be valid"); 204 | return type_; 205 | } 206 | 207 | private: 208 | T type_; 209 | bool is_valid_; 210 | }; 211 | 212 | template 213 | using enable_if_t = typename std::enable_if::type; 214 | 215 | template 216 | struct IsUniformRandomBitGenerator { 217 | static const bool value = std::is_unsigned::value && 218 | std::is_unsigned::value && 219 | std::is_unsigned::value; 220 | }; 221 | } // namespace Sophus 222 | 223 | #endif // SOPHUS_COMMON_HPP 224 | -------------------------------------------------------------------------------- /sophus/example_ensure_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "common.hpp" 2 | 3 | #include 4 | #include 5 | 6 | namespace Sophus { 7 | void ensureFailed(char const* function, char const* file, int line, 8 | char const* description) { 9 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 10 | file, function, line); 11 | std::printf("Description: %s\n", description); 12 | std::abort(); 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /sophus/geometry.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_HPP 2 | #define GEOMETRY_HPP 3 | 4 | #include "se2.hpp" 5 | #include "se3.hpp" 6 | #include "so2.hpp" 7 | #include "so3.hpp" 8 | #include "types.hpp" 9 | 10 | namespace Sophus { 11 | 12 | // Takes in a rotation ``R_foo_plane`` and returns the corresponding line 13 | // normal along the y-axis (in reference frame ``foo``). 14 | // 15 | template 16 | Vector2 normalFromSO2(SO2 const& R_foo_line) { 17 | return R_foo_line.matrix().col(1); 18 | } 19 | 20 | // Takes in line normal in reference frame foo and constructs a corresponding 21 | // rotation matrix ``R_foo_line``. 22 | // 23 | // Precondition: ``normal_foo`` must not be close to zero. 24 | // 25 | template 26 | SO2 SO2FromNormal(Vector2 normal_foo) { 27 | SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants::epsilon(), "%", 28 | normal_foo.transpose()); 29 | normal_foo.normalize(); 30 | return SO2(normal_foo.y(), -normal_foo.x()); 31 | } 32 | 33 | // Takes in a rotation ``R_foo_plane`` and returns the corresponding plane 34 | // normal along the z-axis 35 | // (in reference frame ``foo``). 36 | // 37 | template 38 | Vector3 normalFromSO3(SO3 const& R_foo_plane) { 39 | return R_foo_plane.matrix().col(2); 40 | } 41 | 42 | // Takes in plane normal in reference frame foo and constructs a corresponding 43 | // rotation matrix ``R_foo_plane``. 44 | // 45 | // Note: The ``plane`` frame is defined as such that the normal points along the 46 | // positive z-axis. One can specify hints for the x-axis and y-axis of the 47 | // ``plane`` frame. 48 | // 49 | // Preconditions: 50 | // - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to 51 | // zero. 52 | // - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular. 53 | // 54 | template 55 | Matrix3 rotationFromNormal(Vector3 const& normal_foo, 56 | Vector3 xDirHint_foo = Vector3(T(1), T(0), 57 | T(0)), 58 | Vector3 yDirHint_foo = Vector3(T(0), T(1), 59 | T(0))) { 60 | SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants::epsilon(), 61 | "xDirHint (%) and yDirHint (%) must be perpendicular.", 62 | xDirHint_foo.transpose(), yDirHint_foo.transpose()); 63 | using std::abs; 64 | using std::sqrt; 65 | T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); 66 | T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); 67 | T const normal_foo_sqr_length = normal_foo.squaredNorm(); 68 | SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants::epsilon(), "%", 69 | xDirHint_foo.transpose()); 70 | SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants::epsilon(), "%", 71 | yDirHint_foo.transpose()); 72 | SOPHUS_ENSURE(normal_foo_sqr_length > Constants::epsilon(), "%", 73 | normal_foo.transpose()); 74 | 75 | Matrix3 basis_foo; 76 | basis_foo.col(2) = normal_foo; 77 | 78 | if (abs(xDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 79 | xDirHint_foo.normalize(); 80 | } 81 | if (abs(yDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 82 | yDirHint_foo.normalize(); 83 | } 84 | if (abs(normal_foo_sqr_length - T(1)) > Constants::epsilon()) { 85 | basis_foo.col(2).normalize(); 86 | } 87 | 88 | T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo)); 89 | T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo)); 90 | if (abs_x_dot_z < abs_y_dot_z) { 91 | // basis_foo.z and xDirHint are far from parallel. 92 | basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized(); 93 | basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2)); 94 | } else { 95 | // basis_foo.z and yDirHint are far from parallel. 96 | basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized(); 97 | basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0)); 98 | } 99 | T det = basis_foo.determinant(); 100 | // sanity check 101 | SOPHUS_ENSURE(abs(det - T(1)) < Constants::epsilon(), 102 | "Determinant of basis is not 1, but %. Basis is \n%\n", det, 103 | basis_foo); 104 | return basis_foo; 105 | } 106 | 107 | // Takes in plane normal in reference frame foo and constructs a corresponding 108 | // rotation matrix ``R_foo_plane``. 109 | // 110 | // See ``rotationFromNormal`` for details. 111 | // 112 | template 113 | SO3 SO3FromNormal(Vector3 const& normal_foo) { 114 | return SO3(rotationFromNormal(normal_foo)); 115 | } 116 | 117 | // Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in 118 | // reference frame ``foo``. 119 | // 120 | // Note: The plane is defined by X-axis of the ``line`` frame. 121 | // 122 | template 123 | Line2 lineFromSE2(SE2 const& T_foo_line) { 124 | return Line2(normalFromSO2(T_foo_line.so2()), T_foo_line.translation()); 125 | } 126 | 127 | // Returns the pose ``T_foo_line``, given a line in reference frame ``foo``. 128 | // 129 | // Note: The line is defined by X-axis of the frame ``line``. 130 | // 131 | template 132 | SE2 SE2FromLine(Line2 const& line_foo) { 133 | T const d = line_foo.offset(); 134 | Vector2 const n = line_foo.normal(); 135 | SO2 const R_foo_plane = SO2FromNormal(n); 136 | return SE2(R_foo_plane, -d * n); 137 | } 138 | 139 | // Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in 140 | // reference frame ``foo``. 141 | // 142 | // Note: The plane is defined by XY-plane of the frame ``plane``. 143 | // 144 | template 145 | Plane3 planeFromSE3(SE3 const& T_foo_plane) { 146 | return Plane3(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation()); 147 | } 148 | 149 | // Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``. 150 | // 151 | // Note: The plane is defined by XY-plane of the frame ``plane``. 152 | // 153 | template 154 | SE3 SE3FromPlane(Plane3 const& plane_foo) { 155 | T const d = plane_foo.offset(); 156 | Vector3 const n = plane_foo.normal(); 157 | SO3 const R_foo_plane = SO3FromNormal(n); 158 | return SE3(R_foo_plane, -d * n); 159 | } 160 | 161 | // Takes in a hyperplane and returns unique representation by ensuring that the 162 | // ``offset`` is not negative. 163 | // 164 | template 165 | Eigen::Hyperplane makeHyperplaneUnique( 166 | Eigen::Hyperplane const& plane) { 167 | if (plane.offset() >= 0) { 168 | return plane; 169 | } 170 | 171 | return Eigen::Hyperplane(-plane.normal(), -plane.offset()); 172 | } 173 | 174 | } // namespace Sophus 175 | 176 | #endif // GEOMETRY_HPP 177 | -------------------------------------------------------------------------------- /sophus/interpolate.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_INTERPOLATE_HPP 2 | #define SOPHUS_INTERPOLATE_HPP 3 | 4 | #include 5 | 6 | #include "interpolate_details.hpp" 7 | 8 | namespace Sophus { 9 | 10 | // This function interpolates between two Lie group elements ``foo_T_bar`` 11 | // and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1]. 12 | // 13 | // It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar`` 14 | // and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns 15 | // ``foo_T_bar``. 16 | // 17 | // (Since interpolation on Lie groups is inverse-invariant, we can equivalently 18 | // think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the 19 | // return value being ``quiz_T_foo``.) 20 | // 21 | // Precondition: ``p`` must be in [0, 1]. 22 | // 23 | template 24 | enable_if_t::supported, G> interpolate( 25 | G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) { 26 | using Scalar = typename G::Scalar; 27 | Scalar inter_p(p); 28 | SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1), 29 | "p (%) must in [0, 1]."); 30 | return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log()); 31 | } 32 | 33 | } // namespace Sophus 34 | 35 | #endif // SOPHUS_INTERPOLATE_HPP 36 | -------------------------------------------------------------------------------- /sophus/interpolate_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_INTERPOLATE_DETAILS_HPP 2 | #define SOPHUS_INTERPOLATE_DETAILS_HPP 3 | 4 | #include "rxso2.hpp" 5 | #include "rxso3.hpp" 6 | #include "se2.hpp" 7 | #include "se3.hpp" 8 | #include "sim2.hpp" 9 | #include "sim3.hpp" 10 | #include "so2.hpp" 11 | #include "so3.hpp" 12 | 13 | namespace Sophus { 14 | namespace interp_details { 15 | 16 | template 17 | struct Traits; 18 | 19 | template 20 | struct Traits> { 21 | static bool constexpr supported = true; 22 | 23 | static bool hasShortestPathAmbiguity(SO2 const& foo_T_bar) { 24 | using std::abs; 25 | Scalar angle = foo_T_bar.log(); 26 | return abs(abs(angle) - Constants::pi()) < 27 | Constants::epsilon(); 28 | } 29 | }; 30 | 31 | template 32 | struct Traits> { 33 | static bool constexpr supported = true; 34 | 35 | static bool hasShortestPathAmbiguity(RxSO2 const& foo_T_bar) { 36 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 37 | } 38 | }; 39 | 40 | template 41 | struct Traits> { 42 | static bool constexpr supported = true; 43 | 44 | static bool hasShortestPathAmbiguity(SO3 const& foo_T_bar) { 45 | using std::abs; 46 | Scalar angle = foo_T_bar.logAndTheta().theta; 47 | return abs(abs(angle) - Constants::pi()) < 48 | Constants::epsilon(); 49 | } 50 | }; 51 | 52 | template 53 | struct Traits> { 54 | static bool constexpr supported = true; 55 | 56 | static bool hasShortestPathAmbiguity(RxSO3 const& foo_T_bar) { 57 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 58 | } 59 | }; 60 | 61 | template 62 | struct Traits> { 63 | static bool constexpr supported = true; 64 | 65 | static bool hasShortestPathAmbiguity(SE2 const& foo_T_bar) { 66 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 67 | } 68 | }; 69 | 70 | template 71 | struct Traits> { 72 | static bool constexpr supported = true; 73 | 74 | static bool hasShortestPathAmbiguity(SE3 const& foo_T_bar) { 75 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 76 | } 77 | }; 78 | 79 | template 80 | struct Traits> { 81 | static bool constexpr supported = true; 82 | 83 | static bool hasShortestPathAmbiguity(Sim2 const& foo_T_bar) { 84 | return Traits>::hasShortestPathAmbiguity( 85 | foo_T_bar.rxso2().so2()); 86 | ; 87 | } 88 | }; 89 | 90 | template 91 | struct Traits> { 92 | static bool constexpr supported = true; 93 | 94 | static bool hasShortestPathAmbiguity(Sim3 const& foo_T_bar) { 95 | return Traits>::hasShortestPathAmbiguity( 96 | foo_T_bar.rxso3().so3()); 97 | ; 98 | } 99 | }; 100 | 101 | } // namespace details 102 | } // namespace Sophus 103 | 104 | #endif // SOPHUS_INTERPOLATE_DETAILS_HPP 105 | -------------------------------------------------------------------------------- /sophus/num_diff.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_NUM_DIFF_HPP 2 | #define SOPHUS_NUM_DIFF_HPP 3 | // Numerical differentiation using finite differences 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "types.hpp" 10 | 11 | namespace Sophus { 12 | 13 | namespace details { 14 | template 15 | class Curve { 16 | public: 17 | template 18 | static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) { 19 | using ReturnType = decltype(curve(t)); 20 | static_assert(std::is_floating_point::value, 21 | "Scalar must be a floating point type."); 22 | static_assert(IsFloatingPoint::value, 23 | "ReturnType must be either a floating point scalar, " 24 | "vector or matrix."); 25 | 26 | return (curve(t + h) - curve(t - h)) / (Scalar(2) * h); 27 | } 28 | }; 29 | 30 | template 31 | class VectorField { 32 | public: 33 | static Eigen::Matrix num_diff( 34 | std::function(Sophus::Vector)> 35 | vector_field, 36 | Sophus::Vector const& a, Scalar eps) { 37 | static_assert(std::is_floating_point::value, 38 | "Scalar must be a floating point type."); 39 | Eigen::Matrix J; 40 | Sophus::Vector h; 41 | h.setZero(); 42 | for (int i = 0; i < M; ++i) { 43 | h[i] = eps; 44 | J.col(i) = 45 | (vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps); 46 | h[i] = Scalar(0); 47 | } 48 | 49 | return J; 50 | } 51 | }; 52 | 53 | template 54 | class VectorField { 55 | public: 56 | static Eigen::Matrix num_diff( 57 | std::function(Scalar)> vector_field, 58 | Scalar const& a, Scalar eps) { 59 | return details::Curve::num_diff(std::move(vector_field), a, eps); 60 | } 61 | }; 62 | } // namespace details 63 | 64 | // Calculates the derivative of a curve at a point ``t``. 65 | // 66 | // Here, a curve is a function from a Scalar to a Euclidean space. Thus, it 67 | // returns either a Scalar, a vector or a matrix. 68 | // 69 | template 70 | auto curveNumDiff(Fn curve, Scalar t, 71 | Scalar h = Constants::epsilonSqrt()) 72 | -> decltype(details::Curve::num_diff(std::move(curve), t, h)) { 73 | return details::Curve::num_diff(std::move(curve), t, h); 74 | } 75 | 76 | // Calculates the derivative of a vector field at a point ``a``. 77 | // 78 | // Here, a vector field is a function from a vector space to another vector 79 | // space. 80 | // 81 | template 82 | Eigen::Matrix vectorFieldNumDiff( 83 | Fn vector_field, ScalarOrVector const& a, 84 | Scalar eps = Constants::epsilonSqrt()) { 85 | return details::VectorField::num_diff(std::move(vector_field), 86 | a, eps); 87 | } 88 | 89 | } // namespace Sophus 90 | 91 | #endif // SOPHUS_NUM_DIFF_HPP 92 | -------------------------------------------------------------------------------- /sophus/rotation_matrix.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_ROTATION_MATRIX_HPP 2 | #define SOPHUS_ROTATION_MATRIX_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "types.hpp" 8 | 9 | namespace Sophus { 10 | 11 | // Takes in arbiray square matrix and returns true if it is 12 | // orthogonal. 13 | template 14 | SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase const& R) { 15 | using Scalar = typename D::Scalar; 16 | static int const N = D::RowsAtCompileTime; 17 | static int const M = D::ColsAtCompileTime; 18 | 19 | static_assert(N == M, "must be a square matrix"); 20 | static_assert(N >= 2, "must have compile time dimension >= 2"); 21 | 22 | return (R * R.transpose() - Matrix::Identity()).norm() < 23 | Constants::epsilon(); 24 | } 25 | 26 | // Takes in arbiray square matrix and returns true if it is 27 | // "scaled-orthogonal" with positive determinant. 28 | // 29 | template 30 | SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase const& sR) { 31 | using Scalar = typename D::Scalar; 32 | static int const N = D::RowsAtCompileTime; 33 | static int const M = D::ColsAtCompileTime; 34 | using std::pow; 35 | using std::sqrt; 36 | 37 | Scalar det = sR.determinant(); 38 | 39 | if (det <= Scalar(0)) { 40 | return false; 41 | } 42 | 43 | Scalar scale_sqr = pow(det, Scalar(2. / N)); 44 | 45 | static_assert(N == M, "must be a square matrix"); 46 | static_assert(N >= 2, "must have compile time dimension >= 2"); 47 | 48 | return (sR * sR.transpose() - scale_sqr * Matrix::Identity()) 49 | .template lpNorm() < 50 | sqrt(Constants::epsilon()); 51 | } 52 | 53 | // Takes in arbiray square matrix (2x2 or larger) and returns closest 54 | // orthogonal matrix with positive determinant. 55 | template 56 | SOPHUS_FUNC enable_if_t< 57 | std::is_floating_point::value, 58 | Matrix> 59 | makeRotationMatrix(Eigen::MatrixBase const& R) { 60 | using Scalar = typename D::Scalar; 61 | static int const N = D::RowsAtCompileTime; 62 | static int const M = D::ColsAtCompileTime; 63 | 64 | static_assert(N == M, "must be a square matrix"); 65 | static_assert(N >= 2, "must have compile time dimension >= 2"); 66 | 67 | Eigen::JacobiSVD> svd( 68 | R, Eigen::ComputeFullU | Eigen::ComputeFullV); 69 | 70 | // Determine determinant of orthogonal matrix U*V'. 71 | Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); 72 | // Starting from the identity matrix D, set the last entry to d (+1 or 73 | // -1), so that det(U*D*V') = 1. 74 | Matrix Diag = Matrix::Identity(); 75 | Diag(N - 1, N - 1) = d; 76 | return svd.matrixU() * Diag * svd.matrixV().transpose(); 77 | } 78 | 79 | } // namespace Sophus 80 | 81 | #endif // SOPHUS_ROTATION_MATRIX_HPP 82 | -------------------------------------------------------------------------------- /sophus/sim_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_SIM_DETAILS_HPP 2 | #define SOPHUS_SIM_DETAILS_HPP 3 | 4 | #include "types.hpp" 5 | 6 | namespace Sophus { 7 | namespace details { 8 | 9 | template 10 | Matrix calcW(Matrix const& Omega, 11 | Scalar const theta, Scalar const sigma) { 12 | using std::abs; 13 | using std::exp; 14 | using std::sin; 15 | using std::cos; 16 | static Matrix const I = Matrix::Identity(); 17 | static Scalar const one(1); 18 | static Scalar const half(0.5); 19 | Matrix const Omega2 = Omega * Omega; 20 | Scalar const scale = exp(sigma); 21 | Scalar A, B, C; 22 | if (abs(sigma) < Constants::epsilon()) { 23 | C = one; 24 | if (abs(theta) < Constants::epsilon()) { 25 | A = half; 26 | B = Scalar(1. / 6.); 27 | } else { 28 | Scalar theta_sq = theta * theta; 29 | A = (one - cos(theta)) / theta_sq; 30 | B = (theta - sin(theta)) / (theta_sq * theta); 31 | } 32 | } else { 33 | C = (scale - one) / sigma; 34 | if (abs(theta) < Constants::epsilon()) { 35 | Scalar sigma_sq = sigma * sigma; 36 | A = ((sigma - one) * scale + one) / sigma_sq; 37 | B = (scale * half * sigma_sq + scale - one - sigma * scale) / 38 | (sigma_sq * sigma); 39 | } else { 40 | Scalar theta_sq = theta * theta; 41 | Scalar a = scale * sin(theta); 42 | Scalar b = scale * cos(theta); 43 | Scalar c = theta_sq + sigma * sigma; 44 | A = (a * sigma + (one - b) * theta) / (theta * c); 45 | B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq); 46 | } 47 | } 48 | return A * Omega + B * Omega2 + C * I; 49 | } 50 | 51 | template 52 | Matrix calcWInv(Matrix const& Omega, 53 | Scalar const theta, Scalar const sigma, 54 | Scalar const scale) { 55 | using std::abs; 56 | using std::sin; 57 | using std::cos; 58 | static Matrix const I = Matrix::Identity(); 59 | static Scalar const half(0.5); 60 | static Scalar const one(1); 61 | static Scalar const two(2); 62 | Matrix const Omega2 = Omega * Omega; 63 | Scalar const scale_sq = scale * scale; 64 | Scalar const theta_sq = theta * theta; 65 | Scalar const sin_theta = sin(theta); 66 | Scalar const cos_theta = cos(theta); 67 | 68 | Scalar a, b, c; 69 | if (abs(sigma * sigma) < Constants::epsilon()) { 70 | c = one - half * sigma; 71 | a = -half; 72 | if (abs(theta_sq) < Constants::epsilon()) { 73 | b = Scalar(1. / 12.); 74 | } else { 75 | b = (theta * sin_theta + two * cos_theta - two) / 76 | (two * theta_sq * (cos_theta - one)); 77 | } 78 | } else { 79 | Scalar const scale_cu = scale_sq * scale; 80 | c = sigma / (scale - one); 81 | if (abs(theta_sq) < Constants::epsilon()) { 82 | a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one)); 83 | b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) / 84 | (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two); 85 | } else { 86 | Scalar const s_sin_theta = scale * sin_theta; 87 | Scalar const s_cos_theta = scale * cos_theta; 88 | a = (theta * s_cos_theta - theta - sigma * s_sin_theta) / 89 | (theta * (scale_sq - two * s_cos_theta + one)); 90 | b = -scale * 91 | (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta - 92 | scale * sigma + sigma * cos_theta - sigma) / 93 | (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq + 94 | two * s_cos_theta + scale - one)); 95 | } 96 | } 97 | return a * Omega + b * Omega2 + c * I; 98 | } 99 | 100 | } // details 101 | } // Sophus 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /sophus/test_macros.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPUHS_TESTS_MACROS_HPP 2 | #define SOPUHS_TESTS_MACROS_HPP 3 | 4 | #include 5 | 6 | namespace Sophus { 7 | namespace details { 8 | 9 | template 10 | class Pretty { 11 | public: 12 | static std::string impl(Scalar s) { return FormatString("%", s); } 13 | }; 14 | 15 | template 16 | class Pretty> { 17 | public: 18 | static std::string impl(Matrix const& v) { 19 | return FormatString("\n%\n", v); 20 | } 21 | }; 22 | 23 | template 24 | std::string pretty(T const& v) { 25 | return Pretty::impl(v); 26 | } 27 | 28 | template 29 | void testFailed(bool& passed, char const* func, char const* file, int line, 30 | std::string const& msg) { 31 | std::cerr << FormatString("Test failed in function %, file %, line %\n", func, 32 | file, line); 33 | std::cerr << msg << "\n\n"; 34 | passed = false; 35 | } 36 | } // namespace details 37 | 38 | void processTestResult(bool passed) { 39 | if (!passed) { 40 | // LCOV_EXCL_START 41 | std::cerr << "failed!" << std::endl << std::endl; 42 | exit(-1); 43 | // LCOV_EXCL_STOP 44 | } 45 | std::cerr << "passed." << std::endl << std::endl; 46 | } 47 | } // namespace Sophus 48 | 49 | #define SOPHUS_STRINGIFY(x) #x 50 | 51 | // GenericTests whether condition is true. 52 | // The in-out parameter passed will be set to false if test fails. 53 | #define SOPHUS_TEST(passed, condition, ...) \ 54 | do { \ 55 | if (!(condition)) { \ 56 | std::string msg = Sophus::details::FormatString( \ 57 | "condition ``%`` is false\n", SOPHUS_STRINGIFY(condition)); \ 58 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 59 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 60 | msg); \ 61 | } \ 62 | } while (false) 63 | 64 | // GenericTests whether left is equal to right given a threshold. 65 | // The in-out parameter passed will be set to false if test fails. 66 | #define SOPHUS_TEST_EQUAL(passed, left, right, ...) \ 67 | do { \ 68 | if (left != right) { \ 69 | std::string msg = Sophus::details::FormatString( \ 70 | "% (=%) is not equal to % (=%)\n", SOPHUS_STRINGIFY(left), \ 71 | Sophus::details::pretty(left), SOPHUS_STRINGIFY(right), \ 72 | Sophus::details::pretty(right)); \ 73 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 74 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 75 | msg); \ 76 | } \ 77 | } while (false) 78 | 79 | // GenericTests whether left is equal to right given a threshold. 80 | // The in-out parameter passed will be set to false if test fails. 81 | #define SOPHUS_TEST_NEQ(passed, left, right, ...) \ 82 | do { \ 83 | if (left == right) { \ 84 | std::string msg = Sophus::details::FormatString( \ 85 | "% (=%) shoudl not be equal to % (=%)\n", SOPHUS_STRINGIFY(left), \ 86 | Sophus::details::pretty(left), SOPHUS_STRINGIFY(right), \ 87 | Sophus::details::pretty(right)); \ 88 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 89 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 90 | msg); \ 91 | } \ 92 | } while (false) 93 | 94 | // GenericTests whether left is approximatly equal to right given a threshold. 95 | // The in-out parameter passed will be set to false if test fails. 96 | #define SOPHUS_TEST_APPROX(passed, left, right, thr, ...) \ 97 | do { \ 98 | auto nrm = Sophus::maxMetric((left), (right)); \ 99 | if (!(nrm < (thr))) { \ 100 | std::string msg = Sophus::details::FormatString( \ 101 | "% (=%) is not approx % (=%); % is %; nrm is %\n", \ 102 | SOPHUS_STRINGIFY(left), Sophus::details::pretty(left), \ 103 | SOPHUS_STRINGIFY(right), Sophus::details::pretty(right), \ 104 | SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm); \ 105 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 106 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 107 | msg); \ 108 | } \ 109 | } while (false) 110 | 111 | // GenericTests whether left is NOT approximatly equal to right given a 112 | // threshold. 113 | // The in-out parameter passed will be set to false if test fails. 114 | #define SOPHUS_TEST_NOT_APPROX(passed, left, right, thr, ...) \ 115 | do { \ 116 | auto nrm = Sophus::maxMetric((left), (right)); \ 117 | if (nrm < (thr)) { \ 118 | std::string msg = Sophus::details::FormatString( \ 119 | "% (=%) is approx % (=%), but it should not!\n % is %; nrm is %\n", \ 120 | SOPHUS_STRINGIFY(left), Sophus::details::pretty(left), \ 121 | SOPHUS_STRINGIFY(right), Sophus::details::pretty(right), \ 122 | SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm); \ 123 | msg += Sophus::details::FormatString(__VA_ARGS__); \ 124 | Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ 125 | msg); \ 126 | } \ 127 | } while (false) 128 | 129 | #endif // SOPUHS_TESTS_MACROS_HPP 130 | -------------------------------------------------------------------------------- /sophus/types.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_TYEPES_HPP 2 | #define SOPHUS_TYEPES_HPP 3 | 4 | #include 5 | #include "common.hpp" 6 | 7 | namespace Sophus { 8 | 9 | template 10 | using Vector = Eigen::Matrix; 11 | 12 | template 13 | using Vector2 = Vector; 14 | using Vector2f = Vector2; 15 | using Vector2d = Vector2; 16 | 17 | template 18 | using Vector3 = Vector; 19 | using Vector3f = Vector3; 20 | using Vector3d = Vector3; 21 | 22 | template 23 | using Vector4 = Vector; 24 | using Vector4f = Vector4; 25 | using Vector4d = Vector4; 26 | 27 | template 28 | using Vector6 = Vector; 29 | using Vector6f = Vector6; 30 | using Vector6d = Vector6; 31 | 32 | template 33 | using Vector7 = Vector; 34 | using Vector7f = Vector7; 35 | using Vector7d = Vector7; 36 | 37 | template 38 | using Matrix = Eigen::Matrix; 39 | 40 | template 41 | using Matrix2 = Matrix; 42 | using Matrix2f = Matrix2; 43 | using Matrix2d = Matrix2; 44 | 45 | template 46 | using Matrix3 = Matrix; 47 | using Matrix3f = Matrix3; 48 | using Matrix3d = Matrix3; 49 | 50 | template 51 | using Matrix4 = Matrix; 52 | using Matrix4f = Matrix4; 53 | using Matrix4d = Matrix4; 54 | 55 | template 56 | using Matrix6 = Matrix; 57 | using Matrix6f = Matrix6; 58 | using Matrix6d = Matrix6; 59 | 60 | template 61 | using Matrix7 = Matrix; 62 | using Matrix7f = Matrix7; 63 | using Matrix7d = Matrix7; 64 | 65 | template 66 | using ParametrizedLine = Eigen::ParametrizedLine; 67 | 68 | template 69 | using ParametrizedLine3 = ParametrizedLine; 70 | using ParametrizedLine3f = ParametrizedLine3; 71 | using ParametrizedLine3d = ParametrizedLine3; 72 | 73 | template 74 | using ParametrizedLine2 = ParametrizedLine; 75 | using ParametrizedLine2f = ParametrizedLine2; 76 | using ParametrizedLine2d = ParametrizedLine2; 77 | 78 | namespace details { 79 | template 80 | class MaxMetric { 81 | public: 82 | static Scalar impl(Scalar s0, Scalar s1) { 83 | using std::abs; 84 | return abs(s0 - s1); 85 | } 86 | }; 87 | 88 | template 89 | class MaxMetric> { 90 | public: 91 | static Scalar impl(Matrix const& p0, 92 | Matrix const& p1) { 93 | return (p0 - p1).template lpNorm(); 94 | } 95 | }; 96 | 97 | template 98 | class SetToZero { 99 | public: 100 | static void impl(Scalar& s) { s = Scalar(0); } 101 | }; 102 | 103 | template 104 | class SetToZero> { 105 | public: 106 | static void impl(Matrix& v) { v.setZero(); } 107 | }; 108 | 109 | template 110 | class SetElementAt; 111 | 112 | template 113 | class SetElementAt { 114 | public: 115 | static void impl(Scalar& s, Scalar value, int at) { 116 | SOPHUS_ENSURE(at == 0, "is %", at); 117 | s = value; 118 | } 119 | }; 120 | 121 | template 122 | class SetElementAt, Scalar> { 123 | public: 124 | static void impl(Vector& v, Scalar value, int at) { 125 | SOPHUS_ENSURE(at >= 0 && at < N, "is %", at); 126 | v[at] = value; 127 | } 128 | }; 129 | 130 | template 131 | class SquaredNorm { 132 | public: 133 | static Scalar impl(Scalar const& s) { return s * s; } 134 | }; 135 | 136 | template 137 | class SquaredNorm> { 138 | public: 139 | static Scalar impl(Matrix const& s) { return s.squaredNorm(); } 140 | }; 141 | 142 | template 143 | class Transpose { 144 | public: 145 | static Scalar impl(Scalar const& s) { return s; } 146 | }; 147 | 148 | template 149 | class Transpose> { 150 | public: 151 | static Matrix impl(Matrix const& s) { 152 | return s.transpose(); 153 | } 154 | }; 155 | } // namespace details 156 | 157 | // Returns maximum metric between two points ``p0`` and ``p1``, with ``p`` 158 | // being a matrix or a scalar. 159 | // 160 | template 161 | auto maxMetric(T const& p0, T const& p1) 162 | -> decltype(details::MaxMetric::impl(p0, p1)) { 163 | return details::MaxMetric::impl(p0, p1); 164 | } 165 | 166 | // Sets point ``p`` to zero, with ``p`` being a matrix or a scalar. 167 | // 168 | template 169 | void setToZero(T& p) { 170 | return details::SetToZero::impl(p); 171 | } 172 | 173 | // Sets point ``p`` to zero, with ``p`` being a matrix or a scalar. 174 | // 175 | template 176 | void setElementAt(T& p, Scalar value, int at) { 177 | return details::SetElementAt::impl(p, value, at); 178 | } 179 | 180 | // Returns the squared 2-norm of ``p``, with ``p`` being a vector or a scalar. 181 | // 182 | template 183 | auto squaredNorm(T const& p) -> decltype(details::SquaredNorm::impl(p)) { 184 | return details::SquaredNorm::impl(p); 185 | } 186 | 187 | // Returns ``p.transpose()`` if ``p`` is a matrix, and simply ``p`` if m is a 188 | // scalar. 189 | // 190 | template 191 | auto transpose(T const& p) -> decltype(details::Transpose::impl(T())) { 192 | return details::Transpose::impl(p); 193 | } 194 | 195 | template 196 | struct IsFloatingPoint { 197 | static bool const value = std::is_floating_point::value; 198 | }; 199 | 200 | template 201 | struct IsFloatingPoint> { 202 | static bool const value = std::is_floating_point::value; 203 | }; 204 | 205 | template 206 | struct GetScalar { 207 | using Scalar = Scalar_; 208 | }; 209 | 210 | template 211 | struct GetScalar> { 212 | using Scalar = Scalar_; 213 | }; 214 | 215 | // If the Vector type is a fixed size, then IsFixedSizeVector::value will be 216 | // true. 217 | template ::type> 221 | struct IsFixedSizeVector : std::true_type {}; 222 | 223 | // Planes in 3d are hyperplanes. 224 | template 225 | using Plane3 = Eigen::Hyperplane; 226 | using Plane3d = Plane3; 227 | using Plane3f = Plane3; 228 | 229 | // Lines in 2d are hyperplanes. 230 | template 231 | using Line2 = Eigen::Hyperplane; 232 | using Line2d = Line2; 233 | using Line2f = Line2; 234 | 235 | } // namespace Sophus 236 | 237 | #endif // SOPHUS_TYEPES_HPP 238 | -------------------------------------------------------------------------------- /sophus/velocities.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_VELOCITIES_HPP 2 | #define SOPHUS_VELOCITIES_HPP 3 | 4 | #include 5 | 6 | #include "num_diff.hpp" 7 | #include "se3.hpp" 8 | 9 | namespace Sophus { 10 | namespace experimental { 11 | // Experimental since the API will certainly change drastically in the future. 12 | 13 | // Transforms velocity vector by rotation ``foo_R_bar``. 14 | // 15 | // Note: vel_bar can be either a linear or a rotational velocity vector. 16 | // 17 | template 18 | Vector3 transformVelocity(SO3 const& foo_R_bar, 19 | Vector3 const& vel_bar) { 20 | // For rotational velocities note that: 21 | // 22 | // vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T) 23 | // = vee(hat(Adj(foo_R_bar) * vel_bar)) 24 | // = Adj(foo_R_bar) * vel_bar 25 | // = foo_R_bar * vel_bar. 26 | // 27 | return foo_R_bar * vel_bar; 28 | } 29 | 30 | // Transforms velocity vector by pose ``foo_T_bar``. 31 | // 32 | // Note: vel_bar can be either a linear or a rotational velocity vector. 33 | // 34 | template 35 | Vector3 transformVelocity(SE3 const& foo_T_bar, 36 | Vector3 const& vel_bar) { 37 | return transformVelocity(foo_T_bar.so3(), vel_bar); 38 | } 39 | 40 | // finite difference approximation of instantanious velocity in frame foo 41 | // 42 | template 43 | Vector3 finiteDifferenceRotationalVelocity( 44 | std::function(Scalar)> const& foo_R_bar, Scalar t, 45 | Scalar h = Constants::epsilon()) { 46 | // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor 47 | // 48 | // W = dR(t)/dt * R^{-1}(t) 49 | Matrix3 dR_dt_in_frame_foo = curveNumDiff( 50 | [&foo_R_bar](Scalar t0) -> Matrix3 { 51 | return foo_R_bar(t0).matrix(); 52 | }, 53 | t, h); 54 | // velocity tensor 55 | Matrix3 W_in_frame_foo = 56 | dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix(); 57 | return SO3::vee(W_in_frame_foo); 58 | } 59 | 60 | // finite difference approximation of instantanious velocity in frame foo 61 | // 62 | template 63 | Vector3 finiteDifferenceRotationalVelocity( 64 | std::function(Scalar)> const& foo_T_bar, Scalar t, 65 | Scalar h = Constants::epsilon()) { 66 | return finiteDifferenceRotationalVelocity( 67 | [&foo_T_bar](Scalar t) -> SO3 { return foo_T_bar(t).so3(); }, t, 68 | h); 69 | } 70 | 71 | } // namespace experimental 72 | } // namespace Sophus 73 | 74 | #endif // SOPHUS_VELOCITIES_HPP 75 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_SUBDIRECTORY(core) 2 | ADD_SUBDIRECTORY(ceres) 3 | -------------------------------------------------------------------------------- /test/ceres/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Make sure Ceres knows where to find Eigen 2 | list(APPEND SEARCH_HEADERS ${EIGEN3_INCLUDE_DIR}) 3 | 4 | # git clone https://ceres-solver.googlesource.com/ceres-solver 5 | #find_package( Ceres 1.6.0 QUIET ) 6 | find_package( Ceres QUIET ) 7 | if( Ceres_FOUND ) 8 | MESSAGE(STATUS "CERES found") 9 | 10 | # Tests to run 11 | SET( TEST_SOURCES test_ceres_se3 ) 12 | 13 | FOREACH(test_src ${TEST_SOURCES}) 14 | ADD_EXECUTABLE( ${test_src} ${test_src}.cpp local_parameterization_se3) 15 | TARGET_LINK_LIBRARIES( ${test_src} sophus ${CERES_LIBRARIES} ) 16 | TARGET_INCLUDE_DIRECTORIES( ${test_src} SYSTEM PRIVATE ${CERES_INCLUDE_DIRS}) 17 | ADD_TEST( ${test_src} ${test_src} ) 18 | ENDFOREACH(test_src) 19 | 20 | SET ( ROBUSTPCLRECONSTRUCTION_SOURCES cauchy_em_two_EM) 21 | 22 | FOREACH(robust_src ${ROBUSTPCLRECONSTRUCTION_SOURCES}) 23 | ADD_EXECUTABLE( ${robust_src} ${robust_src}.cpp local_parameterization_se3) 24 | TARGET_LINK_LIBRARIES( ${robust_src} sophus ${CERES_LIBRARIES} ) 25 | TARGET_INCLUDE_DIRECTORIES( ${robust_src} SYSTEM PRIVATE ${CERES_INCLUDE_DIRS}) 26 | ENDFOREACH(robust_src) 27 | 28 | endif( Ceres_FOUND ) 29 | -------------------------------------------------------------------------------- /test/ceres/local_parameterization_se3.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP 2 | #define SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace Sophus { 8 | namespace test { 9 | 10 | class LocalParameterizationSE3 : public ceres::LocalParameterization { 11 | public: 12 | virtual ~LocalParameterizationSE3() {} 13 | 14 | // SE3 plus operation for Ceres 15 | // 16 | // T * exp(x) 17 | // 18 | virtual bool Plus(double const* T_raw, double const* delta_raw, 19 | double* T_plus_delta_raw) const { 20 | Eigen::Map const T(T_raw); 21 | Eigen::Map const delta(delta_raw); 22 | Eigen::Map T_plus_delta(T_plus_delta_raw); 23 | T_plus_delta = T * SE3d::exp(delta); 24 | return true; 25 | } 26 | 27 | // Jacobian of SE3 plus operation for Ceres 28 | // 29 | // Dx T * exp(x) with x=0 30 | // 31 | virtual bool ComputeJacobian(double const* T_raw, 32 | double* jacobian_raw) const { 33 | Eigen::Map T(T_raw); 34 | Eigen::Map> jacobian( 35 | jacobian_raw); 36 | jacobian = T.Dx_this_mul_exp_x_at_0(); 37 | return true; 38 | } 39 | 40 | virtual int GlobalSize() const { return SE3d::num_parameters; } 41 | 42 | virtual int LocalSize() const { return SE3d::DoF; } 43 | }; 44 | } // namespace test 45 | } // namespace Sophus 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /test/ceres/test_ceres_se3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "local_parameterization_se3.hpp" 6 | 7 | // Eigen's ostream operator is not compatible with ceres::Jet types. 8 | // In particular, Eigen assumes that the scalar type (here Jet) can be 9 | // casted to an arithmetic type, which is not true for ceres::Jet. 10 | // Unfortunatly, the ceres::Jet class does not define a conversion 11 | // operator (http://en.cppreference.com/w/cpp/language/cast_operator). 12 | // 13 | // This workaround creates a template specilization for Eigen's cast_impl, 14 | // when casting from a ceres::Jet type. It relies on Eigen's internal API and 15 | // might break with future versions of Eigen. 16 | namespace Eigen { 17 | namespace internal { 18 | 19 | template 20 | struct cast_impl, NewType> { 21 | EIGEN_DEVICE_FUNC 22 | static inline NewType run(ceres::Jet const& x) { 23 | return static_cast(x.a); 24 | } 25 | }; 26 | 27 | } // namespace internal 28 | } // namespace Eigen 29 | 30 | struct TestSE3CostFunctor { 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | TestSE3CostFunctor(Sophus::SE3d T_aw) : T_aw(T_aw) {} 33 | 34 | template 35 | bool operator()(T const* const sT_wa, T* sResiduals) const { 36 | Eigen::Map const> const T_wa(sT_wa); 37 | Eigen::Map > residuals(sResiduals); 38 | 39 | // We are able to mix Sophus types with doubles and Jet types withou needing 40 | // to cast to T. 41 | residuals = (T_aw * T_wa).log(); 42 | // Reverse order of multiplication. This forces the compiler to verify that 43 | // (Jet, double) and (double, Jet) SE3 multiplication work correctly. 44 | residuals = (T_wa * T_aw).log(); 45 | // Finally, ensure that Jet-to-Jet multiplication works. 46 | residuals = (T_wa * T_aw.cast()).log(); 47 | return true; 48 | } 49 | 50 | Sophus::SE3d T_aw; 51 | }; 52 | 53 | struct TestPointCostFunctor { 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 55 | TestPointCostFunctor(Sophus::SE3d T_aw, Eigen::Vector3d point_a) 56 | : T_aw(T_aw), point_a(point_a) {} 57 | 58 | template 59 | bool operator()(T const* const sT_wa, T const* const spoint_b, 60 | T* sResiduals) const { 61 | using Vector3T = Eigen::Matrix; 62 | Eigen::Map const> const T_wa(sT_wa); 63 | Eigen::Map point_b(spoint_b); 64 | Eigen::Map residuals(sResiduals); 65 | 66 | // Multiply SE3d by Jet Vector3. 67 | Vector3T point_b_prime = T_aw * point_b; 68 | // Ensure Jet SE3 multiplication with Jet Vector3. 69 | point_b_prime = T_aw.cast() * point_b; 70 | 71 | // Multiply Jet SE3 with Vector3d. 72 | Vector3T point_a_prime = T_wa * point_a; 73 | // Ensure Jet SE3 multiplication with Jet Vector3. 74 | point_a_prime = T_wa * point_a.cast(); 75 | 76 | residuals = point_b_prime - point_a_prime; 77 | return true; 78 | } 79 | 80 | Sophus::SE3d T_aw; 81 | Eigen::Vector3d point_a; 82 | }; 83 | 84 | bool test(Sophus::SE3d const& T_w_targ, Sophus::SE3d const& T_w_init, 85 | Sophus::SE3d::Point const& point_a_init, 86 | Sophus::SE3d::Point const& point_b) { 87 | static constexpr int kNumPointParameters = 3; 88 | 89 | // Optimisation parameters. 90 | Sophus::SE3d T_wr = T_w_init; 91 | Sophus::SE3d::Point point_a = point_a_init; 92 | 93 | // Build the problem. 94 | ceres::Problem problem; 95 | 96 | // Specify local update rule for our parameter 97 | problem.AddParameterBlock(T_wr.data(), Sophus::SE3d::num_parameters, 98 | new Sophus::test::LocalParameterizationSE3); 99 | 100 | // Create and add cost functions. Derivatives will be evaluated via 101 | // automatic differentiation 102 | ceres::CostFunction* cost_function1 = 103 | new ceres::AutoDiffCostFunction( 105 | new TestSE3CostFunctor(T_w_targ.inverse())); 106 | problem.AddResidualBlock(cost_function1, NULL, T_wr.data()); 107 | ceres::CostFunction* cost_function2 = 108 | new ceres::AutoDiffCostFunction( 111 | new TestPointCostFunctor(T_w_targ.inverse(), point_b)); 112 | problem.AddResidualBlock(cost_function2, NULL, T_wr.data(), point_a.data()); 113 | 114 | // Set solver options (precision / method) 115 | ceres::Solver::Options options; 116 | options.gradient_tolerance = 0.01 * Sophus::Constants::epsilon(); 117 | options.function_tolerance = 0.01 * Sophus::Constants::epsilon(); 118 | options.linear_solver_type = ceres::DENSE_QR; 119 | 120 | // Solve 121 | ceres::Solver::Summary summary; 122 | Solve(options, &problem, &summary); 123 | std::cout << summary.BriefReport() << std::endl; 124 | 125 | // Difference between target and parameter 126 | double const mse = (T_w_targ.inverse() * T_wr).log().squaredNorm(); 127 | bool const passed = mse < 10. * Sophus::Constants::epsilon(); 128 | return passed; 129 | } 130 | 131 | template 132 | bool CreateSE3FromMatrix(Eigen::Matrix mat) { 133 | auto se3 = Sophus::SE3(mat); 134 | se3 = se3; 135 | return true; 136 | } 137 | 138 | int main(int, char**) { 139 | using SE3Type = Sophus::SE3; 140 | using SO3Type = Sophus::SO3; 141 | using Point = SE3Type::Point; 142 | double const kPi = Sophus::Constants::pi(); 143 | 144 | std::vector se3_vec; 145 | se3_vec.push_back( 146 | SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0))); 147 | se3_vec.push_back( 148 | SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), Point(10, 0, 0))); 149 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)), Point(0, 100, 5))); 150 | se3_vec.push_back( 151 | SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0, 0, 0))); 152 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), 153 | Point(0, -0.00000001, 0.0000000001))); 154 | se3_vec.push_back( 155 | SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0.01, 0, 0))); 156 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(4, -5, 0))); 157 | se3_vec.push_back( 158 | SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)) * 159 | SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) * 160 | SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), Point(0, 0, 0))); 161 | se3_vec.push_back( 162 | SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), Point(2, 0, -7)) * 163 | SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) * 164 | SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), Point(0, 6, 0))); 165 | 166 | std::vector point_vec; 167 | point_vec.emplace_back(1.012, 2.73, -1.4); 168 | point_vec.emplace_back(9.2, -7.3, -4.4); 169 | point_vec.emplace_back(2.5, 0.1, 9.1); 170 | point_vec.emplace_back(12.3, 1.9, 3.8); 171 | point_vec.emplace_back(-3.21, 3.42, 2.3); 172 | point_vec.emplace_back(-8.0, 6.1, -1.1); 173 | point_vec.emplace_back(0.0, 2.5, 5.9); 174 | point_vec.emplace_back(7.1, 7.8, -14); 175 | point_vec.emplace_back(5.8, 9.2, 0.0); 176 | 177 | for (size_t i = 0; i < se3_vec.size(); ++i) { 178 | const int other_index = (i + 3) % se3_vec.size(); 179 | bool const passed = test(se3_vec[i], se3_vec[other_index], point_vec[i], 180 | point_vec[other_index]); 181 | if (!passed) { 182 | std::cerr << "failed!" << std::endl << std::endl; 183 | exit(-1); 184 | } 185 | } 186 | 187 | Eigen::Matrix, 4, 4> mat; 188 | mat.setIdentity(); 189 | std::cout << CreateSE3FromMatrix(mat) << std::endl; 190 | 191 | return 0; 192 | } 193 | -------------------------------------------------------------------------------- /test/core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Tests to run 2 | SET( TEST_SOURCES test_common test_so2 test_se2 test_rxso2 test_sim2 test_so3 test_se3 test_rxso3 test_sim3 test_velocities test_geometry) 3 | find_package( Ceres 1.6.0 QUIET ) 4 | 5 | FOREACH(test_src ${TEST_SOURCES}) 6 | ADD_EXECUTABLE( ${test_src} ${test_src}.cpp tests.hpp ../../sophus/test_macros.hpp) 7 | TARGET_LINK_LIBRARIES( ${test_src} sophus ) 8 | if( Ceres_FOUND ) 9 | TARGET_INCLUDE_DIRECTORIES( ${test_src} SYSTEM PRIVATE ${CERES_INCLUDE_DIRS}) 10 | ADD_DEFINITIONS(-DSOPHUS_CERES) 11 | endif(Ceres_FOUND) 12 | ADD_TEST( ${test_src} ${test_src} ) 13 | ENDFOREACH(test_src) 14 | 15 | 16 | -------------------------------------------------------------------------------- /test/core/test_common.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace Sophus { 6 | 7 | namespace { 8 | 9 | bool testFormatString() { 10 | bool passed = true; 11 | SOPHUS_TEST_EQUAL(passed, details::FormatString(), std::string()); 12 | std::string test_str = "Hello World!"; 13 | SOPHUS_TEST_EQUAL(passed, details::FormatString(test_str.c_str()), test_str); 14 | SOPHUS_TEST_EQUAL(passed, details::FormatString("Number: %", 5), 15 | std::string("Number: 5")); 16 | SOPHUS_TEST_EQUAL(passed, 17 | details::FormatString("Real: % msg %", 1.5, test_str), 18 | std::string("Real: 1.5 msg Hello World!")); 19 | SOPHUS_TEST_EQUAL(passed, 20 | details::FormatString( 21 | "vec: %", Eigen::Vector3f(0.f, 1.f, 1.5f).transpose()), 22 | std::string("vec: 0 1 1.5")); 23 | SOPHUS_TEST_EQUAL( 24 | passed, details::FormatString("Number: %", 1, 2), 25 | std::string("Number: 1\nFormat-Warning: There are 1 args unused.")); 26 | return passed; 27 | } 28 | 29 | bool testSmokeDetails() { 30 | bool passed = true; 31 | std::cout << details::pretty(4.2) << std::endl; 32 | std::cout << details::pretty(Vector2f(1, 2)) << std::endl; 33 | bool dummy = true; 34 | details::testFailed(dummy, "dummyFunc", "dummyFile", 99, 35 | "This is just a pratice alarm!"); 36 | SOPHUS_TEST_EQUAL(passed, dummy, false); 37 | 38 | double val = transpose(42.0); 39 | SOPHUS_TEST_EQUAL(passed, val, 42.0); 40 | Matrix row = transpose(Vector2f(1, 7)); 41 | Matrix expected_row(1, 7); 42 | SOPHUS_TEST_EQUAL(passed, row, expected_row); 43 | 44 | optional opt(nullopt); 45 | SOPHUS_TEST(passed, !opt); 46 | 47 | return passed; 48 | } 49 | 50 | void runAll() { 51 | std::cerr << "Common tests:" << std::endl; 52 | bool passed = testFormatString(); 53 | passed &= testSmokeDetails(); 54 | processTestResult(passed); 55 | } 56 | 57 | } // namespace 58 | } // namespace Sophus 59 | 60 | int main() { Sophus::runAll(); } 61 | -------------------------------------------------------------------------------- /test/core/test_geometry.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include "tests.hpp" 6 | 7 | namespace Sophus { 8 | 9 | namespace { 10 | 11 | template 12 | bool test2dGeometry() { 13 | bool passed = true; 14 | T const eps = Constants::epsilon(); 15 | 16 | for (int i = 0; i < 20; ++i) { 17 | // Roundtrip test: 18 | Vector2 normal_foo = Vector2::Random().normalized(); 19 | Sophus::SO2 R_foo_plane = SO2FromNormal(normal_foo); 20 | Vector2 resultNormal_foo = normalFromSO2(R_foo_plane); 21 | SOPHUS_TEST_APPROX(passed, normal_foo, resultNormal_foo, eps); 22 | } 23 | 24 | for (int i = 0; i < 20; ++i) { 25 | // Roundtrip test: 26 | Line2 line_foo = makeHyperplaneUnique( 27 | Line2(Vector2::Random().normalized(), Vector2::Random())); 28 | Sophus::SE2 T_foo_plane = SE2FromLine(line_foo); 29 | Line2 resultPlane_foo = lineFromSE2(T_foo_plane); 30 | SOPHUS_TEST_APPROX(passed, line_foo.normal().eval(), 31 | resultPlane_foo.normal().eval(), eps); 32 | SOPHUS_TEST_APPROX(passed, line_foo.offset(), resultPlane_foo.offset(), 33 | eps); 34 | } 35 | 36 | std::vector, Eigen::aligned_allocator>> Ts_foo_line = 37 | getTestSE2s(); 38 | 39 | for (SE2 const& T_foo_line : Ts_foo_line) { 40 | Line2 line_foo = lineFromSE2(T_foo_line); 41 | SE2 T2_foo_line = SE2FromLine(line_foo); 42 | Line2 line2_foo = lineFromSE2(T2_foo_line); 43 | SOPHUS_TEST_APPROX(passed, line_foo.normal().eval(), 44 | line2_foo.normal().eval(), eps); 45 | SOPHUS_TEST_APPROX(passed, line_foo.offset(), line2_foo.offset(), eps); 46 | } 47 | 48 | return passed; 49 | } 50 | 51 | template 52 | bool test3dGeometry() { 53 | bool passed = true; 54 | T const eps = Constants::epsilon(); 55 | 56 | Vector3 normal_foo = Vector3(1, 2, 0).normalized(); 57 | Matrix3 R_foo_plane = rotationFromNormal(normal_foo); 58 | SOPHUS_TEST_APPROX(passed, normal_foo, R_foo_plane.col(2).eval(), eps); 59 | // Just testing that the function normalizes the input normal and hint 60 | // direction correctly: 61 | Matrix3 R2_foo_plane = rotationFromNormal((T(0.9) * normal_foo).eval()); 62 | SOPHUS_TEST_APPROX(passed, normal_foo, R2_foo_plane.col(2).eval(), eps); 63 | Matrix3 R3_foo_plane = 64 | rotationFromNormal(normal_foo, Vector3(T(0.9), T(0), T(0)), 65 | Vector3(T(0), T(1.1), T(0))); 66 | SOPHUS_TEST_APPROX(passed, normal_foo, R3_foo_plane.col(2).eval(), eps); 67 | 68 | normal_foo = Vector3(1, 0, 0); 69 | R_foo_plane = rotationFromNormal(normal_foo); 70 | SOPHUS_TEST_APPROX(passed, normal_foo, R_foo_plane.col(2).eval(), eps); 71 | SOPHUS_TEST_APPROX(passed, Vector3(0, 1, 0), R_foo_plane.col(1).eval(), 72 | eps); 73 | 74 | normal_foo = Vector3(0, 1, 0); 75 | R_foo_plane = rotationFromNormal(normal_foo); 76 | SOPHUS_TEST_APPROX(passed, normal_foo, R_foo_plane.col(2).eval(), eps); 77 | SOPHUS_TEST_APPROX(passed, Vector3(1, 0, 0), R_foo_plane.col(0).eval(), 78 | eps); 79 | 80 | for (int i = 0; i < 20; ++i) { 81 | // Roundtrip test: 82 | Vector3 normal_foo = Vector3::Random().normalized(); 83 | Sophus::SO3 R_foo_plane = SO3FromNormal(normal_foo); 84 | Vector3 resultNormal_foo = normalFromSO3(R_foo_plane); 85 | SOPHUS_TEST_APPROX(passed, normal_foo, resultNormal_foo, eps); 86 | } 87 | 88 | for (int i = 0; i < 20; ++i) { 89 | // Roundtrip test: 90 | Plane3 plane_foo = makeHyperplaneUnique( 91 | Plane3(Vector3::Random().normalized(), Vector3::Random())); 92 | Sophus::SE3 T_foo_plane = SE3FromPlane(plane_foo); 93 | Plane3 resultPlane_foo = planeFromSE3(T_foo_plane); 94 | SOPHUS_TEST_APPROX(passed, plane_foo.normal().eval(), 95 | resultPlane_foo.normal().eval(), eps); 96 | SOPHUS_TEST_APPROX(passed, plane_foo.offset(), resultPlane_foo.offset(), 97 | eps); 98 | } 99 | 100 | std::vector, Eigen::aligned_allocator>> Ts_foo_plane = 101 | getTestSE3s(); 102 | 103 | for (SE3 const& T_foo_plane : Ts_foo_plane) { 104 | Plane3 plane_foo = planeFromSE3(T_foo_plane); 105 | SE3 T2_foo_plane = SE3FromPlane(plane_foo); 106 | Plane3 plane2_foo = planeFromSE3(T2_foo_plane); 107 | SOPHUS_TEST_APPROX(passed, plane_foo.normal().eval(), 108 | plane2_foo.normal().eval(), eps); 109 | SOPHUS_TEST_APPROX(passed, plane_foo.offset(), plane2_foo.offset(), eps); 110 | } 111 | 112 | return passed; 113 | } 114 | 115 | void runAll() { 116 | std::cerr << "Geometry (Lines/Planes) tests:" << std::endl; 117 | std::cerr << "Double tests: " << std::endl; 118 | bool passed = test2dGeometry(); 119 | passed = test3dGeometry(); 120 | processTestResult(passed); 121 | std::cerr << "Float tests: " << std::endl; 122 | passed = test2dGeometry(); 123 | passed = test3dGeometry(); 124 | processTestResult(passed); 125 | } 126 | 127 | } // namespace 128 | } // namespace Sophus 129 | 130 | int main() { Sophus::runAll(); } 131 | -------------------------------------------------------------------------------- /test/core/test_rxso2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include "tests.hpp" 5 | 6 | // Explicit instantiate all class templates so that all member methods 7 | // get compiled and for code coverage analysis. 8 | namespace Eigen { 9 | template class Map>; 10 | template class Map const>; 11 | } // namespace Eigen 12 | 13 | namespace Sophus { 14 | 15 | template class RxSO2; 16 | template class RxSO2; 17 | #if SOPHUS_CERES 18 | template class RxSO2>; 19 | #endif 20 | 21 | template 22 | class Tests { 23 | public: 24 | using SO2Type = SO2; 25 | using RxSO2Type = RxSO2; 26 | using Point = typename RxSO2::Point; 27 | using Tangent = typename RxSO2::Tangent; 28 | Scalar const kPi = Constants::pi(); 29 | 30 | Tests() { 31 | rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.2, 1.))); 32 | rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.2, 1.1))); 33 | rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0., 1.1))); 34 | rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.00001, 0.))); 35 | rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.00001, 0.00001))); 36 | rxso2_vec_.push_back(RxSO2Type::exp(Tangent(kPi, 0.9))); 37 | rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.2, 0)) * 38 | RxSO2Type::exp(Tangent(kPi, 0.0)) * 39 | RxSO2Type::exp(Tangent(-0.2, 0))); 40 | rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.3, 0)) * 41 | RxSO2Type::exp(Tangent(kPi, 0.001)) * 42 | RxSO2Type::exp(Tangent(-0.3, 0))); 43 | 44 | Tangent tmp; 45 | tmp << Scalar(0), Scalar(0); 46 | tangent_vec_.push_back(tmp); 47 | tmp << Scalar(1), Scalar(0); 48 | tangent_vec_.push_back(tmp); 49 | tmp << Scalar(1), Scalar(0.1); 50 | tangent_vec_.push_back(tmp); 51 | tmp << Scalar(0), Scalar(0.1); 52 | tangent_vec_.push_back(tmp); 53 | tmp << Scalar(0), Scalar(-0.1); 54 | tangent_vec_.push_back(tmp); 55 | tmp << Scalar(-1), Scalar(-0.1); 56 | tangent_vec_.push_back(tmp); 57 | tmp << Scalar(20), Scalar(2); 58 | tangent_vec_.push_back(tmp); 59 | 60 | point_vec_.push_back(Point(Scalar(1), Scalar(4))); 61 | point_vec_.push_back(Point(Scalar(1), Scalar(-3))); 62 | } 63 | 64 | template 65 | enable_if_t::value, bool> testFit() { 66 | bool passed = true; 67 | for (int i = 0; i < 10; ++i) { 68 | Matrix2 M = Matrix2::Random(); 69 | for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) { 70 | Matrix2 R = makeRotationMatrix(M); 71 | Matrix2 sR = scale * R; 72 | SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR), 73 | "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R); 74 | Matrix2 sR_cols_swapped; 75 | sR_cols_swapped << sR.col(1), sR.col(0); 76 | SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped), 77 | "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R); 78 | } 79 | } 80 | return passed; 81 | } 82 | 83 | template 84 | enable_if_t::value, bool> testFit() { 85 | return true; 86 | } 87 | 88 | void runAll() { 89 | bool passed = testLieProperties(); 90 | passed &= testSaturation(); 91 | passed &= testRawDataAcces(); 92 | passed &= testConstructors(); 93 | passed &= testFit(); 94 | processTestResult(passed); 95 | } 96 | 97 | private: 98 | bool testLieProperties() { 99 | LieGroupTests tests(rxso2_vec_, tangent_vec_, point_vec_); 100 | return tests.doAllTestsPass(); 101 | } 102 | 103 | bool testSaturation() { 104 | bool passed = true; 105 | RxSO2Type small1(Scalar(1.1) * Constants::epsilon(), SO2Type()); 106 | RxSO2Type small2(Scalar(1.1) * Constants::epsilon(), 107 | SO2Type::exp(Constants::pi())); 108 | RxSO2Type saturated_product = small1 * small2; 109 | SOPHUS_TEST_APPROX(passed, saturated_product.scale(), 110 | Constants::epsilon(), 111 | Constants::epsilon()); 112 | SOPHUS_TEST_APPROX(passed, saturated_product.so2().matrix(), 113 | (small1.so2() * small2.so2()).matrix(), 114 | Constants::epsilon()); 115 | return passed; 116 | } 117 | 118 | bool testRawDataAcces() { 119 | bool passed = true; 120 | Eigen::Matrix raw = {0, 1}; 121 | Eigen::Map map_of_const_rxso2(raw.data()); 122 | SOPHUS_TEST_APPROX(passed, map_of_const_rxso2.complex().eval(), raw, 123 | Constants::epsilon()); 124 | SOPHUS_TEST_EQUAL(passed, map_of_const_rxso2.complex().data(), raw.data()); 125 | Eigen::Map const_shallow_copy = map_of_const_rxso2; 126 | SOPHUS_TEST_EQUAL(passed, const_shallow_copy.complex().eval(), 127 | map_of_const_rxso2.complex().eval()); 128 | 129 | Eigen::Matrix raw2 = {1, 0}; 130 | Eigen::Map map_of_rxso2(raw2.data()); 131 | SOPHUS_TEST_APPROX(passed, map_of_rxso2.complex().eval(), raw2, 132 | Constants::epsilon()); 133 | SOPHUS_TEST_EQUAL(passed, map_of_rxso2.complex().data(), raw2.data()); 134 | Eigen::Map shallow_copy = map_of_rxso2; 135 | SOPHUS_TEST_EQUAL(passed, shallow_copy.complex().eval(), 136 | map_of_rxso2.complex().eval()); 137 | 138 | RxSO2Type const const_so2(raw2); 139 | for (int i = 0; i < 2; ++i) { 140 | SOPHUS_TEST_EQUAL(passed, const_so2.data()[i], raw2.data()[i]); 141 | } 142 | 143 | RxSO2Type so2(raw2); 144 | for (int i = 0; i < 2; ++i) { 145 | so2.data()[i] = raw[i]; 146 | } 147 | 148 | for (int i = 0; i < 2; ++i) { 149 | SOPHUS_TEST_EQUAL(passed, so2.data()[i], raw.data()[i]); 150 | } 151 | return passed; 152 | } 153 | 154 | bool testConstructors() { 155 | bool passed = true; 156 | RxSO2Type rxso2; 157 | Scalar scale(1.2); 158 | rxso2.setScale(scale); 159 | SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(), 160 | Constants::epsilon(), "setScale"); 161 | Scalar angle(0.2); 162 | rxso2.setAngle(angle); 163 | SOPHUS_TEST_APPROX(passed, angle, rxso2.angle(), 164 | Constants::epsilon(), "setAngle"); 165 | SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(), 166 | Constants::epsilon(), 167 | "setAngle leaves scale as is"); 168 | 169 | auto so2 = rxso2_vec_[0].so2(); 170 | rxso2.setSO2(so2); 171 | SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(), 172 | Constants::epsilon(), "setSO2"); 173 | SOPHUS_TEST_APPROX(passed, RxSO2Type(scale, so2).matrix(), rxso2.matrix(), 174 | Constants::epsilon(), "RxSO2(scale, SO2)"); 175 | SOPHUS_TEST_APPROX(passed, RxSO2Type(scale, so2.matrix()).matrix(), 176 | rxso2.matrix(), Constants::epsilon(), 177 | "RxSO2(scale, SO2)"); 178 | Matrix2 R = SO2::exp(Scalar(0.2)).matrix(); 179 | Matrix2 sR = R * Scalar(1.3); 180 | SOPHUS_TEST_APPROX(passed, RxSO2Type(sR).matrix(), sR, 181 | Constants::epsilon(), "RxSO2(sR)"); 182 | rxso2.setScaledRotationMatrix(sR); 183 | SOPHUS_TEST_APPROX(passed, sR, rxso2.matrix(), Constants::epsilon(), 184 | "setScaleRotationMatrix"); 185 | rxso2.setScale(scale); 186 | rxso2.setRotationMatrix(R); 187 | SOPHUS_TEST_APPROX(passed, R, rxso2.rotationMatrix(), 188 | Constants::epsilon(), "setRotationMatrix"); 189 | SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(), 190 | Constants::epsilon(), "setScale"); 191 | 192 | return passed; 193 | } 194 | 195 | std::vector> rxso2_vec_; 196 | std::vector> tangent_vec_; 197 | std::vector> point_vec_; 198 | }; 199 | 200 | int test_rxso2() { 201 | using std::cerr; 202 | using std::endl; 203 | 204 | cerr << "Test RxSO2" << endl << endl; 205 | cerr << "Double tests: " << endl; 206 | Tests().runAll(); 207 | cerr << "Float tests: " << endl; 208 | Tests().runAll(); 209 | 210 | #if SOPHUS_CERES 211 | cerr << "ceres::Jet tests: " << endl; 212 | Tests>().runAll(); 213 | #endif 214 | return 0; 215 | } 216 | 217 | } // namespace Sophus 218 | 219 | int main() { return Sophus::test_rxso2(); } 220 | -------------------------------------------------------------------------------- /test/core/test_rxso3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include "tests.hpp" 5 | 6 | // Explicit instantiate all class templates so that all member methods 7 | // get compiled and for code coverage analysis. 8 | namespace Eigen { 9 | template class Map>; 10 | template class Map const>; 11 | } // namespace Eigen 12 | 13 | namespace Sophus { 14 | 15 | template class RxSO3; 16 | template class RxSO3; 17 | #if SOPHUS_CERES 18 | template class RxSO3>; 19 | #endif 20 | 21 | template 22 | class Tests { 23 | public: 24 | using Scalar = Scalar_; 25 | using SO3Type = SO3; 26 | using RxSO3Type = RxSO3; 27 | using Point = typename RxSO3::Point; 28 | using Tangent = typename RxSO3::Tangent; 29 | Scalar const kPi = Constants::pi(); 30 | 31 | Tests() { 32 | rxso3_vec_.push_back(RxSO3Type::exp( 33 | Tangent(Scalar(0.2), Scalar(0.5), Scalar(0.0), Scalar(1.)))); 34 | rxso3_vec_.push_back(RxSO3Type::exp( 35 | Tangent(Scalar(0.2), Scalar(0.5), Scalar(-1.0), Scalar(1.1)))); 36 | rxso3_vec_.push_back(RxSO3Type::exp( 37 | Tangent(Scalar(0.), Scalar(0.), Scalar(0.), Scalar(1.1)))); 38 | rxso3_vec_.push_back(RxSO3Type::exp( 39 | Tangent(Scalar(0.), Scalar(0.), Scalar(0.00001), Scalar(0.)))); 40 | rxso3_vec_.push_back(RxSO3Type::exp( 41 | Tangent(Scalar(0.), Scalar(0.), Scalar(0.00001), Scalar(0.00001)))); 42 | rxso3_vec_.push_back(RxSO3Type::exp( 43 | Tangent(Scalar(0.), Scalar(0.), Scalar(0.00001), Scalar(0)))); 44 | rxso3_vec_.push_back( 45 | RxSO3Type::exp(Tangent(kPi, Scalar(0), Scalar(0), Scalar(0.9)))); 46 | rxso3_vec_.push_back( 47 | RxSO3Type::exp( 48 | Tangent(Scalar(0.2), Scalar(-0.5), Scalar(0), Scalar(0))) * 49 | RxSO3Type::exp(Tangent(kPi, Scalar(0), Scalar(0), Scalar(0))) * 50 | RxSO3Type::exp( 51 | Tangent(-Scalar(0.2), Scalar(-0.5), Scalar(0), Scalar(0)))); 52 | rxso3_vec_.push_back( 53 | RxSO3Type::exp( 54 | Tangent(Scalar(0.3), Scalar(0.5), Scalar(0.1), Scalar(0))) * 55 | RxSO3Type::exp(Tangent(kPi, Scalar(0), Scalar(0), Scalar(0))) * 56 | RxSO3Type::exp( 57 | Tangent(Scalar(-0.3), Scalar(-0.5), Scalar(-0.1), Scalar(0)))); 58 | 59 | Tangent tmp; 60 | tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0); 61 | tangent_vec_.push_back(tmp); 62 | tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0); 63 | tangent_vec_.push_back(tmp); 64 | tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0.1); 65 | tangent_vec_.push_back(tmp); 66 | tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(0.1); 67 | tangent_vec_.push_back(tmp); 68 | tmp << Scalar(0), Scalar(0), Scalar(1), Scalar(-0.1); 69 | tangent_vec_.push_back(tmp); 70 | tmp << Scalar(-1), Scalar(1), Scalar(0), Scalar(-0.1); 71 | tangent_vec_.push_back(tmp); 72 | tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(2); 73 | tangent_vec_.push_back(tmp); 74 | 75 | point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4))); 76 | point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5))); 77 | } 78 | 79 | void runAll() { 80 | bool passed = testLieProperties(); 81 | passed &= testSaturation(); 82 | passed &= testRawDataAcces(); 83 | passed &= testConstructors(); 84 | passed &= testFit(); 85 | processTestResult(passed); 86 | } 87 | 88 | private: 89 | bool testLieProperties() { 90 | LieGroupTests tests(rxso3_vec_, tangent_vec_, point_vec_); 91 | return tests.doAllTestsPass(); 92 | } 93 | 94 | bool testSaturation() { 95 | bool passed = true; 96 | RxSO3Type small1(Constants::epsilon(), SO3Type()); 97 | RxSO3Type small2(Constants::epsilon(), 98 | SO3Type::exp(Vector3(Constants::pi(), 99 | Scalar(0), Scalar(0)))); 100 | RxSO3Type saturated_product = small1 * small2; 101 | SOPHUS_TEST_APPROX(passed, saturated_product.scale(), 102 | Constants::epsilon(), 103 | Constants::epsilon()); 104 | SOPHUS_TEST_APPROX(passed, saturated_product.so3().matrix(), 105 | (small1.so3() * small2.so3()).matrix(), 106 | Constants::epsilon()); 107 | return passed; 108 | } 109 | 110 | bool testRawDataAcces() { 111 | bool passed = true; 112 | Eigen::Matrix raw = {Scalar(0), Scalar(1), Scalar(0), 113 | Scalar(0)}; 114 | Eigen::Map map_of_const_rxso3(raw.data()); 115 | SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(), 116 | raw, Constants::epsilon()); 117 | SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(), 118 | raw.data()); 119 | Eigen::Map const_shallow_copy = map_of_const_rxso3; 120 | SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(), 121 | map_of_const_rxso3.quaternion().coeffs().eval()); 122 | 123 | Eigen::Matrix raw2 = {Scalar(1), Scalar(0), Scalar(0), 124 | Scalar(0)}; 125 | Eigen::Map map_of_rxso3(raw.data()); 126 | Eigen::Quaternion quat; 127 | quat.coeffs() = raw2; 128 | map_of_rxso3.setQuaternion(quat); 129 | SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2, 130 | Constants::epsilon()); 131 | SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(), 132 | raw.data()); 133 | SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(), 134 | quat.coeffs().data()); 135 | Eigen::Map shallow_copy = map_of_rxso3; 136 | SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(), 137 | map_of_rxso3.quaternion().coeffs().eval()); 138 | 139 | RxSO3Type const const_so3(quat); 140 | for (int i = 0; i < 4; ++i) { 141 | SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]); 142 | } 143 | 144 | RxSO3Type so3(quat); 145 | for (int i = 0; i < 4; ++i) { 146 | so3.data()[i] = raw[i]; 147 | } 148 | 149 | for (int i = 0; i < 4; ++i) { 150 | SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]); 151 | } 152 | 153 | return passed; 154 | } 155 | 156 | bool testConstructors() { 157 | bool passed = true; 158 | RxSO3Type rxso3; 159 | Scalar scale(1.2); 160 | rxso3.setScale(scale); 161 | SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(), 162 | Constants::epsilon(), "setScale"); 163 | auto so3 = rxso3_vec_[0].so3(); 164 | rxso3.setSO3(so3); 165 | SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(), 166 | Constants::epsilon(), "setScale"); 167 | SOPHUS_TEST_APPROX(passed, RxSO3Type(scale, so3).matrix(), rxso3.matrix(), 168 | Constants::epsilon(), "RxSO3(scale, SO3)"); 169 | SOPHUS_TEST_APPROX(passed, RxSO3Type(scale, so3.matrix()).matrix(), 170 | rxso3.matrix(), Constants::epsilon(), 171 | "RxSO3(scale, SO3)"); 172 | Matrix3 R = 173 | SO3::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(-1.0))) 174 | .matrix(); 175 | Matrix3 sR = R * Scalar(1.3); 176 | SOPHUS_TEST_APPROX(passed, RxSO3Type(sR).matrix(), sR, 177 | Constants::epsilon(), "RxSO3(sR)"); 178 | rxso3.setScaledRotationMatrix(sR); 179 | SOPHUS_TEST_APPROX(passed, sR, rxso3.matrix(), Constants::epsilon(), 180 | "setScaleRotationMatrix"); 181 | rxso3.setScale(scale); 182 | rxso3.setRotationMatrix(R); 183 | SOPHUS_TEST_APPROX(passed, R, rxso3.rotationMatrix(), 184 | Constants::epsilon(), "setRotationMatrix"); 185 | SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(), 186 | Constants::epsilon(), "setScale"); 187 | 188 | return passed; 189 | } 190 | 191 | template 192 | enable_if_t::value, bool> testFit() { 193 | bool passed = true; 194 | for (int i = 0; i < 10; ++i) { 195 | Matrix3 M = Matrix3::Random(); 196 | for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) { 197 | Matrix3 R = makeRotationMatrix(M); 198 | Matrix3 sR = scale * R; 199 | SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR), 200 | "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R); 201 | Matrix3 sR_cols_swapped; 202 | sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2); 203 | SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped), 204 | "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R); 205 | } 206 | } 207 | return passed; 208 | } 209 | 210 | template 211 | enable_if_t::value, bool> testFit() { 212 | return true; 213 | } 214 | 215 | std::vector> rxso3_vec_; 216 | std::vector> tangent_vec_; 217 | std::vector> point_vec_; 218 | }; 219 | 220 | int test_rxso3() { 221 | using std::cerr; 222 | using std::endl; 223 | 224 | cerr << "Test RxSO3" << endl << endl; 225 | cerr << "Double tests: " << endl; 226 | Tests().runAll(); 227 | cerr << "Float tests: " << endl; 228 | Tests().runAll(); 229 | 230 | #if SOPHUS_CERES 231 | cerr << "ceres::Jet tests: " << endl; 232 | Tests>().runAll(); 233 | #endif 234 | 235 | return 0; 236 | } 237 | 238 | } // namespace Sophus 239 | 240 | int main() { return Sophus::test_rxso3(); } 241 | -------------------------------------------------------------------------------- /test/core/test_se2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include "tests.hpp" 6 | 7 | // Explicit instantiate all class templates so that all member methods 8 | // get compiled and for code coverage analysis. 9 | namespace Eigen { 10 | template class Map>; 11 | template class Map const>; 12 | } // namespace Eigen 13 | 14 | namespace Sophus { 15 | 16 | template class SE2; 17 | template class SE2; 18 | #if SOPHUS_CERES 19 | template class SE2>; 20 | #endif 21 | 22 | template 23 | class Tests { 24 | public: 25 | using SE2Type = SE2; 26 | using SO2Type = SO2; 27 | using Point = typename SE2::Point; 28 | using Tangent = typename SE2::Tangent; 29 | Scalar const kPi = Constants::pi(); 30 | 31 | Tests() { 32 | se2_vec_.push_back( 33 | SE2Type(SO2Type(Scalar(0.0)), Point(Scalar(0), Scalar(0)))); 34 | se2_vec_.push_back( 35 | SE2Type(SO2Type(Scalar(0.2)), Point(Scalar(10), Scalar(0)))); 36 | se2_vec_.push_back( 37 | SE2Type(SO2Type(Scalar(0.)), Point(Scalar(0), Scalar(100)))); 38 | se2_vec_.push_back( 39 | SE2Type(SO2Type(Scalar(-1.)), Point(Scalar(20), -Scalar(1)))); 40 | se2_vec_.push_back( 41 | SE2Type(SO2Type(Scalar(0.00001)), 42 | Point(Scalar(-0.00000001), Scalar(0.0000000001)))); 43 | se2_vec_.push_back( 44 | SE2Type(SO2Type(Scalar(0.2)), Point(Scalar(0), Scalar(0))) * 45 | SE2Type(SO2Type(kPi), Point(Scalar(0), Scalar(0))) * 46 | SE2Type(SO2Type(Scalar(-0.2)), Point(Scalar(0), Scalar(0)))); 47 | se2_vec_.push_back( 48 | SE2Type(SO2Type(Scalar(0.3)), Point(Scalar(2), Scalar(0))) * 49 | SE2Type(SO2Type(kPi), Point(Scalar(0), Scalar(0))) * 50 | SE2Type(SO2Type(Scalar(-0.3)), Point(Scalar(0), Scalar(6)))); 51 | 52 | Tangent tmp; 53 | tmp << Scalar(0), Scalar(0), Scalar(0); 54 | tangent_vec_.push_back(tmp); 55 | tmp << Scalar(1), Scalar(0), Scalar(0); 56 | tangent_vec_.push_back(tmp); 57 | tmp << Scalar(0), Scalar(1), Scalar(1); 58 | tangent_vec_.push_back(tmp); 59 | tmp << Scalar(-1), Scalar(1), Scalar(0); 60 | tangent_vec_.push_back(tmp); 61 | tmp << Scalar(20), Scalar(-1), Scalar(-1); 62 | tangent_vec_.push_back(tmp); 63 | tmp << Scalar(30), Scalar(5), Scalar(20); 64 | tangent_vec_.push_back(tmp); 65 | 66 | point_vec_.push_back(Point(1, 2)); 67 | point_vec_.push_back(Point(1, -3)); 68 | } 69 | 70 | void runAll() { 71 | bool passed = testLieProperties(); 72 | passed &= testRawDataAcces(); 73 | passed &= testConstructors(); 74 | passed &= testFit(); 75 | processTestResult(passed); 76 | } 77 | 78 | private: 79 | bool testLieProperties() { 80 | LieGroupTests tests(se2_vec_, tangent_vec_, point_vec_); 81 | return tests.doAllTestsPass(); 82 | } 83 | 84 | bool testRawDataAcces() { 85 | bool passed = true; 86 | Eigen::Matrix raw; 87 | raw << Scalar(0), Scalar(1), Scalar(0), Scalar(3); 88 | Eigen::Map const_se2_map(raw.data()); 89 | SOPHUS_TEST_APPROX(passed, const_se2_map.unit_complex().eval(), 90 | raw.template head<2>().eval(), 91 | Constants::epsilon()); 92 | SOPHUS_TEST_APPROX(passed, const_se2_map.translation().eval(), 93 | raw.template tail<2>().eval(), 94 | Constants::epsilon()); 95 | SOPHUS_TEST_EQUAL(passed, const_se2_map.unit_complex().data(), raw.data()); 96 | SOPHUS_TEST_EQUAL(passed, const_se2_map.translation().data(), 97 | raw.data() + 2); 98 | Eigen::Map const_shallow_copy = const_se2_map; 99 | SOPHUS_TEST_EQUAL(passed, const_shallow_copy.unit_complex().eval(), 100 | const_se2_map.unit_complex().eval()); 101 | SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(), 102 | const_se2_map.translation().eval()); 103 | 104 | Eigen::Matrix raw2; 105 | raw2 << Scalar(1), Scalar(0), Scalar(3), Scalar(1); 106 | Eigen::Map map_of_se3(raw.data()); 107 | map_of_se3.setComplex(raw2.template head<2>()); 108 | map_of_se3.translation() = raw2.template tail<2>(); 109 | SOPHUS_TEST_APPROX(passed, map_of_se3.unit_complex().eval(), 110 | raw2.template head<2>().eval(), 111 | Constants::epsilon()); 112 | SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(), 113 | raw2.template tail<2>().eval(), 114 | Constants::epsilon()); 115 | SOPHUS_TEST_EQUAL(passed, map_of_se3.unit_complex().data(), raw.data()); 116 | SOPHUS_TEST_EQUAL(passed, map_of_se3.translation().data(), raw.data() + 2); 117 | SOPHUS_TEST_NEQ(passed, map_of_se3.unit_complex().data(), raw2.data()); 118 | Eigen::Map shallow_copy = map_of_se3; 119 | SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_complex().eval(), 120 | map_of_se3.unit_complex().eval()); 121 | SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(), 122 | map_of_se3.translation().eval()); 123 | Eigen::Map const const_map_of_se2 = map_of_se3; 124 | SOPHUS_TEST_EQUAL(passed, const_map_of_se2.unit_complex().eval(), 125 | map_of_se3.unit_complex().eval()); 126 | SOPHUS_TEST_EQUAL(passed, const_map_of_se2.translation().eval(), 127 | map_of_se3.translation().eval()); 128 | 129 | SE2Type const const_se2(raw2.template head<2>().eval(), 130 | raw2.template tail<2>().eval()); 131 | for (int i = 0; i < 4; ++i) { 132 | SOPHUS_TEST_EQUAL(passed, const_se2.data()[i], raw2.data()[i]); 133 | } 134 | 135 | SE2Type se2(raw2.template head<2>().eval(), raw2.template tail<2>().eval()); 136 | for (int i = 0; i < 4; ++i) { 137 | SOPHUS_TEST_EQUAL(passed, se2.data()[i], raw2.data()[i]); 138 | } 139 | 140 | for (int i = 0; i < 4; ++i) { 141 | SOPHUS_TEST_EQUAL(passed, se2.data()[i], raw.data()[i]); 142 | } 143 | 144 | SE2Type trans = SE2Type::transX(Scalar(0.2)); 145 | SOPHUS_TEST_APPROX(passed, trans.translation().x(), Scalar(0.2), 146 | Constants::epsilon()); 147 | trans = SE2Type::transY(Scalar(0.7)); 148 | SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7), 149 | Constants::epsilon()); 150 | return passed; 151 | } 152 | 153 | bool testConstructors() { 154 | bool passed = true; 155 | Matrix3 I = Matrix3::Identity(); 156 | SOPHUS_TEST_EQUAL(passed, SE2Type().matrix(), I); 157 | 158 | SE2Type se2 = se2_vec_.front(); 159 | Point translation = se2.translation(); 160 | SO2Type so2 = se2.so2(); 161 | 162 | SOPHUS_TEST_APPROX(passed, SE2Type(so2.log(), translation).matrix(), 163 | se2.matrix(), Constants::epsilon()); 164 | SOPHUS_TEST_APPROX(passed, SE2Type(so2, translation).matrix(), se2.matrix(), 165 | Constants::epsilon()); 166 | SOPHUS_TEST_APPROX(passed, SE2Type(so2.matrix(), translation).matrix(), 167 | se2.matrix(), Constants::epsilon()); 168 | SOPHUS_TEST_APPROX(passed, 169 | SE2Type(so2.unit_complex(), translation).matrix(), 170 | se2.matrix(), Constants::epsilon()); 171 | SOPHUS_TEST_APPROX(passed, SE2Type(se2.matrix()).matrix(), se2.matrix(), 172 | Constants::epsilon()); 173 | 174 | return passed; 175 | } 176 | 177 | template 178 | enable_if_t::value, bool> testFit() { 179 | bool passed = true; 180 | for (int i = 0; i < 100; ++i) { 181 | Matrix3 T = Matrix3::Random(); 182 | SE2Type se2 = SE2Type::fitToSE2(T); 183 | SE2Type se2_2 = SE2Type::fitToSE2(se2.matrix()); 184 | 185 | SOPHUS_TEST_APPROX(passed, se2.matrix(), se2_2.matrix(), 186 | Constants::epsilon()); 187 | } 188 | return passed; 189 | } 190 | 191 | template 192 | enable_if_t::value, bool> testFit() { 193 | return true; 194 | } 195 | 196 | std::vector> se2_vec_; 197 | std::vector> tangent_vec_; 198 | std::vector> point_vec_; 199 | }; 200 | 201 | int test_se2() { 202 | using std::cerr; 203 | using std::endl; 204 | 205 | cerr << "Test SE2" << endl << endl; 206 | cerr << "Double tests: " << endl; 207 | Tests().runAll(); 208 | cerr << "Float tests: " << endl; 209 | Tests().runAll(); 210 | 211 | #if SOPHUS_CERES 212 | cerr << "ceres::Jet tests: " << endl; 213 | Tests>().runAll(); 214 | #endif 215 | 216 | return 0; 217 | } 218 | } // namespace Sophus 219 | 220 | int main() { return Sophus::test_se2(); } 221 | -------------------------------------------------------------------------------- /test/core/test_se3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include "tests.hpp" 5 | 6 | // Explicit instantiate all class templates so that all member methods 7 | // get compiled and for code coverage analysis. 8 | namespace Eigen { 9 | template class Map>; 10 | template class Map const>; 11 | } // namespace Eigen 12 | 13 | namespace Sophus { 14 | 15 | template class SE3; 16 | template class SE3; 17 | #if SOPHUS_CERES 18 | template class SE3>; 19 | #endif 20 | 21 | template 22 | class Tests { 23 | public: 24 | using SE3Type = SE3; 25 | using SO3Type = SO3; 26 | using Point = typename SE3::Point; 27 | using Tangent = typename SE3::Tangent; 28 | Scalar const kPi = Constants::pi(); 29 | 30 | Tests() { 31 | se3_vec_ = getTestSE3s(); 32 | 33 | Tangent tmp; 34 | tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0); 35 | tangent_vec_.push_back(tmp); 36 | tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0); 37 | tangent_vec_.push_back(tmp); 38 | tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0); 39 | tangent_vec_.push_back(tmp); 40 | tmp << Scalar(0), Scalar(-5), Scalar(10), Scalar(0), Scalar(0), Scalar(0); 41 | tangent_vec_.push_back(tmp); 42 | tmp << Scalar(-1), Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(1); 43 | tangent_vec_.push_back(tmp); 44 | tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(-1), Scalar(1), Scalar(0); 45 | tangent_vec_.push_back(tmp); 46 | tmp << Scalar(30), Scalar(5), Scalar(-1), Scalar(20), Scalar(-1), Scalar(0); 47 | tangent_vec_.push_back(tmp); 48 | 49 | point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4))); 50 | point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5))); 51 | } 52 | 53 | void runAll() { 54 | bool passed = testLieProperties(); 55 | passed &= testRawDataAcces(); 56 | passed &= testConstructors(); 57 | passed &= testFit(); 58 | processTestResult(passed); 59 | } 60 | 61 | private: 62 | bool testLieProperties() { 63 | LieGroupTests tests(se3_vec_, tangent_vec_, point_vec_); 64 | return tests.doAllTestsPass(); 65 | } 66 | 67 | bool testRawDataAcces() { 68 | bool passed = true; 69 | Eigen::Matrix raw; 70 | raw << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3), 71 | Scalar(2); 72 | Eigen::Map map_of_const_se3(raw.data()); 73 | SOPHUS_TEST_APPROX( 74 | passed, map_of_const_se3.unit_quaternion().coeffs().eval(), 75 | raw.template head<4>().eval(), Constants::epsilon()); 76 | SOPHUS_TEST_APPROX(passed, map_of_const_se3.translation().eval(), 77 | raw.template tail<3>().eval(), 78 | Constants::epsilon()); 79 | SOPHUS_TEST_EQUAL( 80 | passed, map_of_const_se3.unit_quaternion().coeffs().data(), raw.data()); 81 | SOPHUS_TEST_EQUAL(passed, map_of_const_se3.translation().data(), 82 | raw.data() + 4); 83 | Eigen::Map const_shallow_copy = map_of_const_se3; 84 | SOPHUS_TEST_EQUAL(passed, 85 | const_shallow_copy.unit_quaternion().coeffs().eval(), 86 | map_of_const_se3.unit_quaternion().coeffs().eval()); 87 | SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(), 88 | map_of_const_se3.translation().eval()); 89 | 90 | Eigen::Matrix raw2; 91 | raw2 << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2), 92 | Scalar(1); 93 | Eigen::Map map_of_se3(raw.data()); 94 | Eigen::Quaternion quat; 95 | quat.coeffs() = raw2.template head<4>(); 96 | map_of_se3.setQuaternion(quat); 97 | map_of_se3.translation() = raw2.template tail<3>(); 98 | SOPHUS_TEST_APPROX(passed, map_of_se3.unit_quaternion().coeffs().eval(), 99 | raw2.template head<4>().eval(), 100 | Constants::epsilon()); 101 | SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(), 102 | raw2.template tail<3>().eval(), 103 | Constants::epsilon()); 104 | SOPHUS_TEST_EQUAL(passed, map_of_se3.unit_quaternion().coeffs().data(), 105 | raw.data()); 106 | SOPHUS_TEST_EQUAL(passed, map_of_se3.translation().data(), raw.data() + 4); 107 | SOPHUS_TEST_NEQ(passed, map_of_se3.unit_quaternion().coeffs().data(), 108 | quat.coeffs().data()); 109 | Eigen::Map shallow_copy = map_of_se3; 110 | SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_quaternion().coeffs().eval(), 111 | map_of_se3.unit_quaternion().coeffs().eval()); 112 | SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(), 113 | map_of_se3.translation().eval()); 114 | Eigen::Map const const_map_of_se3 = map_of_se3; 115 | SOPHUS_TEST_EQUAL(passed, 116 | const_map_of_se3.unit_quaternion().coeffs().eval(), 117 | map_of_se3.unit_quaternion().coeffs().eval()); 118 | SOPHUS_TEST_EQUAL(passed, const_map_of_se3.translation().eval(), 119 | map_of_se3.translation().eval()); 120 | 121 | SE3Type const const_se3(quat, raw2.template tail<3>().eval()); 122 | for (int i = 0; i < 7; ++i) { 123 | SOPHUS_TEST_EQUAL(passed, const_se3.data()[i], raw2.data()[i]); 124 | } 125 | 126 | SE3Type se3(quat, raw2.template tail<3>().eval()); 127 | for (int i = 0; i < 7; ++i) { 128 | SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]); 129 | } 130 | 131 | for (int i = 0; i < 7; ++i) { 132 | SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]); 133 | } 134 | SE3Type trans = SE3Type::transX(Scalar(0.2)); 135 | SOPHUS_TEST_APPROX(passed, trans.translation().x(), Scalar(0.2), 136 | Constants::epsilon()); 137 | trans = SE3Type::transY(Scalar(0.7)); 138 | SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7), 139 | Constants::epsilon()); 140 | trans = SE3Type::transZ(Scalar(-0.2)); 141 | SOPHUS_TEST_APPROX(passed, trans.translation().z(), Scalar(-0.2), 142 | Constants::epsilon()); 143 | Tangent t; 144 | t << Scalar(0), Scalar(0), Scalar(0), Scalar(0.2), Scalar(0), Scalar(0); 145 | SOPHUS_TEST_EQUAL(passed, SE3Type::rotX(Scalar(0.2)).matrix(), 146 | SE3Type::exp(t).matrix()); 147 | t << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(-0.2), Scalar(0); 148 | SOPHUS_TEST_EQUAL(passed, SE3Type::rotY(Scalar(-0.2)).matrix(), 149 | SE3Type::exp(t).matrix()); 150 | t << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1.1); 151 | SOPHUS_TEST_EQUAL(passed, SE3Type::rotZ(Scalar(1.1)).matrix(), 152 | SE3Type::exp(t).matrix()); 153 | 154 | return passed; 155 | } 156 | 157 | bool testConstructors() { 158 | bool passed = true; 159 | Eigen::Matrix I = Eigen::Matrix::Identity(); 160 | SOPHUS_TEST_EQUAL(passed, SE3Type().matrix(), I); 161 | 162 | SE3Type se3 = se3_vec_.front(); 163 | Point translation = se3.translation(); 164 | SO3Type so3 = se3.so3(); 165 | 166 | SOPHUS_TEST_APPROX(passed, SE3Type(so3, translation).matrix(), se3.matrix(), 167 | Constants::epsilon()); 168 | SOPHUS_TEST_APPROX(passed, SE3Type(so3.matrix(), translation).matrix(), 169 | se3.matrix(), Constants::epsilon()); 170 | SOPHUS_TEST_APPROX(passed, 171 | SE3Type(so3.unit_quaternion(), translation).matrix(), 172 | se3.matrix(), Constants::epsilon()); 173 | SOPHUS_TEST_APPROX(passed, SE3Type(se3.matrix()).matrix(), se3.matrix(), 174 | Constants::epsilon()); 175 | 176 | return passed; 177 | } 178 | 179 | template 180 | enable_if_t::value, bool> testFit() { 181 | bool passed = true; 182 | 183 | for (int i = 0; i < 100; ++i) { 184 | Matrix4 T = Matrix4::Random(); 185 | SE3Type se3 = SE3Type::fitToSE3(T); 186 | SE3Type se3_2 = SE3Type::fitToSE3(se3.matrix()); 187 | 188 | SOPHUS_TEST_APPROX(passed, se3.matrix(), se3_2.matrix(), 189 | Constants::epsilon()); 190 | } 191 | for (Scalar const angle : 192 | {Scalar(0.0), Scalar(0.1), Scalar(0.3), Scalar(-0.7)}) { 193 | SOPHUS_TEST_APPROX(passed, SE3Type::rotX(angle).angleX(), angle, 194 | Constants::epsilon()); 195 | SOPHUS_TEST_APPROX(passed, SE3Type::rotY(angle).angleY(), angle, 196 | Constants::epsilon()); 197 | SOPHUS_TEST_APPROX(passed, SE3Type::rotZ(angle).angleZ(), angle, 198 | Constants::epsilon()); 199 | } 200 | return passed; 201 | } 202 | 203 | template 204 | enable_if_t::value, bool> testFit() { 205 | return true; 206 | } 207 | 208 | std::vector> se3_vec_; 209 | std::vector> tangent_vec_; 210 | std::vector> point_vec_; 211 | }; 212 | 213 | int test_se3() { 214 | using std::cerr; 215 | using std::endl; 216 | 217 | cerr << "Test SE3" << endl << endl; 218 | cerr << "Double tests: " << endl; 219 | Tests().runAll(); 220 | cerr << "Float tests: " << endl; 221 | Tests().runAll(); 222 | 223 | #if SOPHUS_CERES 224 | cerr << "ceres::Jet tests: " << endl; 225 | Tests>().runAll(); 226 | #endif 227 | 228 | return 0; 229 | } 230 | } // namespace Sophus 231 | 232 | int main() { return Sophus::test_se3(); } 233 | -------------------------------------------------------------------------------- /test/core/test_sim2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include "tests.hpp" 7 | 8 | // Explicit instantiate all class templates so that all member methods 9 | // get compiled and for code coverage analysis. 10 | namespace Eigen { 11 | template class Map>; 12 | template class Map const>; 13 | } // namespace Eigen 14 | 15 | namespace Sophus { 16 | 17 | template class Sim2; 18 | template class Sim2; 19 | #if SOPHUS_CERES 20 | template class Sim2>; 21 | #endif 22 | 23 | template 24 | class Tests { 25 | public: 26 | using Sim2Type = Sim2; 27 | using RxSO2Type = RxSO2; 28 | using Point = typename Sim2::Point; 29 | using Vector2Type = Vector2; 30 | using Tangent = typename Sim2::Tangent; 31 | Scalar const kPi = Constants::pi(); 32 | 33 | Tests() { 34 | sim2_vec_.push_back( 35 | Sim2Type(RxSO2Type::exp(Vector2Type(0.2, 1.)), Point(0, 0))); 36 | sim2_vec_.push_back( 37 | Sim2Type(RxSO2Type::exp(Vector2Type(0.2, 1.1)), Point(10, 0))); 38 | sim2_vec_.push_back( 39 | Sim2Type(RxSO2Type::exp(Vector2Type(0., 0.)), Point(0, 10))); 40 | sim2_vec_.push_back( 41 | Sim2Type(RxSO2Type::exp(Vector2Type(0.00001, 0.)), Point(0, 0))); 42 | sim2_vec_.push_back( 43 | Sim2Type(RxSO2Type::exp(Vector2Type(0.00001, 0.0000001)), 44 | Point(1, -1.00000001))); 45 | sim2_vec_.push_back( 46 | Sim2Type(RxSO2Type::exp(Vector2Type(0., 0.)), Point(0.01, 0))); 47 | sim2_vec_.push_back( 48 | Sim2Type(RxSO2Type::exp(Vector2Type(kPi, 0.9)), Point(4, 0))); 49 | sim2_vec_.push_back( 50 | Sim2Type(RxSO2Type::exp(Vector2Type(0.2, 0)), Point(0, 0)) * 51 | Sim2Type(RxSO2Type::exp(Vector2Type(kPi, 0)), Point(0, 0)) * 52 | Sim2Type(RxSO2Type::exp(Vector2Type(-0.2, 0)), Point(0, 0))); 53 | sim2_vec_.push_back( 54 | Sim2Type(RxSO2Type::exp(Vector2Type(0.3, 0)), Point(2, -7)) * 55 | Sim2Type(RxSO2Type::exp(Vector2Type(kPi, 0)), Point(0, 0)) * 56 | Sim2Type(RxSO2Type::exp(Vector2Type(-0.3, 0)), Point(0, 6))); 57 | Tangent tmp; 58 | tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0); 59 | tangent_vec_.push_back(tmp); 60 | tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0); 61 | tangent_vec_.push_back(tmp); 62 | tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(0.1); 63 | tangent_vec_.push_back(tmp); 64 | tmp << Scalar(-1), Scalar(1), Scalar(1), Scalar(-0.1); 65 | tangent_vec_.push_back(tmp); 66 | tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(-0.1); 67 | tangent_vec_.push_back(tmp); 68 | tmp << Scalar(30), Scalar(5), Scalar(-1), Scalar(1.5); 69 | tangent_vec_.push_back(tmp); 70 | 71 | point_vec_.push_back(Point(Scalar(1), Scalar(4))); 72 | point_vec_.push_back(Point(Scalar(1), Scalar(-3))); 73 | } 74 | 75 | void runAll() { 76 | bool passed = testLieProperties(); 77 | passed &= testRawDataAcces(); 78 | passed &= testConstructors(); 79 | processTestResult(passed); 80 | } 81 | 82 | private: 83 | bool testLieProperties() { 84 | LieGroupTests tests(sim2_vec_, tangent_vec_, point_vec_); 85 | return tests.doAllTestsPass(); 86 | } 87 | 88 | bool testRawDataAcces() { 89 | bool passed = true; 90 | Eigen::Matrix raw; 91 | raw << Scalar(0), Scalar(1), Scalar(3), Scalar(2); 92 | Eigen::Map map_of_const_sim2(raw.data()); 93 | SOPHUS_TEST_APPROX(passed, map_of_const_sim2.complex().eval(), 94 | raw.template head<2>().eval(), 95 | Constants::epsilon()); 96 | SOPHUS_TEST_APPROX(passed, map_of_const_sim2.translation().eval(), 97 | raw.template tail<2>().eval(), 98 | Constants::epsilon()); 99 | SOPHUS_TEST_EQUAL(passed, map_of_const_sim2.complex().data(), raw.data()); 100 | SOPHUS_TEST_EQUAL(passed, map_of_const_sim2.translation().data(), 101 | raw.data() + 2); 102 | Eigen::Map const_shallow_copy = map_of_const_sim2; 103 | SOPHUS_TEST_EQUAL(passed, const_shallow_copy.complex().eval(), 104 | map_of_const_sim2.complex().eval()); 105 | SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(), 106 | map_of_const_sim2.translation().eval()); 107 | 108 | Eigen::Matrix raw2; 109 | raw2 << Scalar(1), Scalar(0), Scalar(2), Scalar(1); 110 | Eigen::Map map_of_sim2(raw.data()); 111 | Vector2 z; 112 | z = raw2.template head<2>(); 113 | map_of_sim2.setComplex(z); 114 | map_of_sim2.translation() = raw2.template tail<2>(); 115 | SOPHUS_TEST_APPROX(passed, map_of_sim2.complex().eval(), 116 | raw2.template head<2>().eval(), 117 | Constants::epsilon()); 118 | SOPHUS_TEST_APPROX(passed, map_of_sim2.translation().eval(), 119 | raw2.template tail<2>().eval(), 120 | Constants::epsilon()); 121 | SOPHUS_TEST_EQUAL(passed, map_of_sim2.complex().data(), raw.data()); 122 | SOPHUS_TEST_EQUAL(passed, map_of_sim2.translation().data(), raw.data() + 2); 123 | SOPHUS_TEST_NEQ(passed, map_of_sim2.complex().data(), z.data()); 124 | Eigen::Map shallow_copy = map_of_sim2; 125 | SOPHUS_TEST_EQUAL(passed, shallow_copy.complex().eval(), 126 | map_of_sim2.complex().eval()); 127 | SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(), 128 | map_of_sim2.translation().eval()); 129 | Eigen::Map const const_map_of_sim3 = map_of_sim2; 130 | SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.complex().eval(), 131 | map_of_sim2.complex().eval()); 132 | SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.translation().eval(), 133 | map_of_sim2.translation().eval()); 134 | 135 | Sim2Type const const_sim2(z, raw2.template tail<2>().eval()); 136 | for (int i = 0; i < 4; ++i) { 137 | SOPHUS_TEST_EQUAL(passed, const_sim2.data()[i], raw2.data()[i]); 138 | } 139 | 140 | Sim2Type se3(z, raw2.template tail<2>().eval()); 141 | for (int i = 0; i < 4; ++i) { 142 | SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]); 143 | } 144 | 145 | for (int i = 0; i < 4; ++i) { 146 | SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]); 147 | } 148 | return passed; 149 | } 150 | 151 | bool testConstructors() { 152 | bool passed = true; 153 | Eigen::Matrix I = Eigen::Matrix::Identity(); 154 | SOPHUS_TEST_EQUAL(passed, Sim2Type().matrix(), I); 155 | 156 | Sim2Type sim2 = sim2_vec_.front(); 157 | Point translation = sim2.translation(); 158 | RxSO2Type rxso2 = sim2.rxso2(); 159 | 160 | SOPHUS_TEST_APPROX(passed, Sim2Type(rxso2, translation).matrix(), 161 | sim2.matrix(), Constants::epsilon()); 162 | SOPHUS_TEST_APPROX(passed, Sim2Type(rxso2.complex(), translation).matrix(), 163 | sim2.matrix(), Constants::epsilon()); 164 | SOPHUS_TEST_APPROX(passed, Sim2Type(sim2.matrix()).matrix(), sim2.matrix(), 165 | Constants::epsilon()); 166 | 167 | Scalar scale(1.2); 168 | sim2.setScale(scale); 169 | SOPHUS_TEST_APPROX(passed, scale, sim2.scale(), 170 | Constants::epsilon(), "setScale"); 171 | 172 | sim2.setComplex(sim2_vec_[0].rxso2().complex()); 173 | SOPHUS_TEST_APPROX(passed, sim2_vec_[0].rxso2().complex(), 174 | sim2_vec_[0].rxso2().complex(), 175 | Constants::epsilon(), "setComplex"); 176 | return passed; 177 | } 178 | 179 | std::vector> sim2_vec_; 180 | std::vector> tangent_vec_; 181 | std::vector> point_vec_; 182 | }; 183 | 184 | int test_sim3() { 185 | using std::cerr; 186 | using std::endl; 187 | 188 | cerr << "Test Sim2" << endl << endl; 189 | cerr << "Double tests: " << endl; 190 | Tests().runAll(); 191 | cerr << "Float tests: " << endl; 192 | Tests().runAll(); 193 | 194 | #if SOPHUS_CERES 195 | cerr << "ceres::Jet tests: " << endl; 196 | Tests>().runAll(); 197 | #endif 198 | 199 | return 0; 200 | } 201 | } // namespace Sophus 202 | 203 | int main() { return Sophus::test_sim3(); } 204 | -------------------------------------------------------------------------------- /test/core/test_so2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include "tests.hpp" 5 | 6 | // Explicit instantiate all class templates so that all member methods 7 | // get compiled and for code coverage analysis. 8 | namespace Eigen { 9 | template class Map>; 10 | template class Map const>; 11 | } // namespace Eigen 12 | 13 | namespace Sophus { 14 | 15 | template class SO2; 16 | template class SO2; 17 | #if SOPHUS_CERES 18 | template class SO2>; 19 | #endif 20 | 21 | template 22 | class Tests { 23 | public: 24 | using SO2Type = SO2; 25 | using Point = typename SO2::Point; 26 | using Tangent = typename SO2::Tangent; 27 | Scalar const kPi = Constants::pi(); 28 | 29 | Tests() { 30 | so2_vec_.push_back(SO2Type::exp(Scalar(0.0))); 31 | so2_vec_.push_back(SO2Type::exp(Scalar(0.2))); 32 | so2_vec_.push_back(SO2Type::exp(Scalar(10.))); 33 | so2_vec_.push_back(SO2Type::exp(Scalar(0.00001))); 34 | so2_vec_.push_back(SO2Type::exp(kPi)); 35 | so2_vec_.push_back(SO2Type::exp(Scalar(0.2)) * SO2Type::exp(kPi) * 36 | SO2Type::exp(Scalar(-0.2))); 37 | so2_vec_.push_back(SO2Type::exp(Scalar(-0.3)) * SO2Type::exp(kPi) * 38 | SO2Type::exp(Scalar(0.3))); 39 | 40 | tangent_vec_.push_back(Tangent(Scalar(0))); 41 | tangent_vec_.push_back(Tangent(Scalar(1))); 42 | tangent_vec_.push_back(Tangent(Scalar(kPi / 2.))); 43 | tangent_vec_.push_back(Tangent(Scalar(-1))); 44 | tangent_vec_.push_back(Tangent(Scalar(20))); 45 | tangent_vec_.push_back(Tangent(Scalar(kPi / 2. + 0.0001))); 46 | 47 | point_vec_.push_back(Point(Scalar(1), Scalar(2))); 48 | point_vec_.push_back(Point(Scalar(1), Scalar(-3))); 49 | } 50 | 51 | void runAll() { 52 | bool passed = testLieProperties(); 53 | passed &= testUnity(); 54 | passed &= testRawDataAcces(); 55 | passed &= testConstructors(); 56 | passed &= testFit(); 57 | processTestResult(passed); 58 | } 59 | 60 | private: 61 | bool testLieProperties() { 62 | LieGroupTests tests(so2_vec_, tangent_vec_, point_vec_); 63 | return tests.doAllTestsPass(); 64 | } 65 | 66 | bool testUnity() { 67 | bool passed = true; 68 | // Test that the complex number magnitude stays close to one. 69 | SO2Type current_q; 70 | for (std::size_t i = 0; i < 1000; ++i) { 71 | for (SO2Type const& q : so2_vec_) { 72 | current_q *= q; 73 | } 74 | } 75 | SOPHUS_TEST_APPROX(passed, current_q.unit_complex().norm(), Scalar(1), 76 | Constants::epsilon(), "Magnitude drift"); 77 | return passed; 78 | } 79 | 80 | bool testRawDataAcces() { 81 | bool passed = true; 82 | Vector2 raw = {0, 1}; 83 | Eigen::Map map_of_const_so2(raw.data()); 84 | SOPHUS_TEST_APPROX(passed, map_of_const_so2.unit_complex().eval(), raw, 85 | Constants::epsilon()); 86 | SOPHUS_TEST_EQUAL(passed, map_of_const_so2.unit_complex().data(), 87 | raw.data()); 88 | Eigen::Map const_shallow_copy = map_of_const_so2; 89 | SOPHUS_TEST_EQUAL(passed, const_shallow_copy.unit_complex().eval(), 90 | map_of_const_so2.unit_complex().eval()); 91 | 92 | Vector2 raw2 = {1, 0}; 93 | Eigen::Map map_of_so2(raw.data()); 94 | map_of_so2.setComplex(raw2); 95 | SOPHUS_TEST_APPROX(passed, map_of_so2.unit_complex().eval(), raw2, 96 | Constants::epsilon()); 97 | SOPHUS_TEST_EQUAL(passed, map_of_so2.unit_complex().data(), raw.data()); 98 | SOPHUS_TEST_NEQ(passed, map_of_so2.unit_complex().data(), raw2.data()); 99 | Eigen::Map shallow_copy = map_of_so2; 100 | SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_complex().eval(), 101 | map_of_so2.unit_complex().eval()); 102 | 103 | SO2Type const const_so2(raw2); 104 | for (int i = 0; i < 2; ++i) { 105 | SOPHUS_TEST_EQUAL(passed, const_so2.data()[i], raw2.data()[i]); 106 | } 107 | 108 | SO2Type so2(raw2); 109 | for (int i = 0; i < 2; ++i) { 110 | so2.data()[i] = raw[i]; 111 | } 112 | 113 | for (int i = 0; i < 2; ++i) { 114 | SOPHUS_TEST_EQUAL(passed, so2.data()[i], raw.data()[i]); 115 | } 116 | return passed; 117 | } 118 | 119 | bool testConstructors() { 120 | bool passed = true; 121 | Matrix2 R = so2_vec_.front().matrix(); 122 | SO2Type so2(R); 123 | SOPHUS_TEST_APPROX(passed, R, so2.matrix(), Constants::epsilon()); 124 | 125 | return passed; 126 | } 127 | 128 | template 129 | enable_if_t::value, bool> testFit() { 130 | bool passed = true; 131 | 132 | for (int i = 0; i < 100; ++i) { 133 | Matrix2 R = Matrix2::Random(); 134 | SO2Type so2 = SO2Type::fitToSO2(R); 135 | SO2Type so2_2 = SO2Type::fitToSO2(so2.matrix()); 136 | 137 | SOPHUS_TEST_APPROX(passed, so2.matrix(), so2_2.matrix(), 138 | Constants::epsilon()); 139 | } 140 | return passed; 141 | } 142 | 143 | template 144 | enable_if_t::value, bool> testFit() { 145 | return true; 146 | } 147 | 148 | std::vector> so2_vec_; 149 | std::vector> tangent_vec_; 150 | std::vector> point_vec_; 151 | }; 152 | 153 | int test_so2() { 154 | using std::cerr; 155 | using std::endl; 156 | 157 | cerr << "Test SO2" << endl << endl; 158 | cerr << "Double tests: " << endl; 159 | Tests().runAll(); 160 | cerr << "Float tests: " << endl; 161 | Tests().runAll(); 162 | 163 | #if SOPHUS_CERES 164 | cerr << "ceres::Jet tests: " << endl; 165 | Tests>().runAll(); 166 | #endif 167 | 168 | return 0; 169 | } 170 | } // namespace Sophus 171 | 172 | int main() { return Sophus::test_so2(); } 173 | -------------------------------------------------------------------------------- /test/core/test_so3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include "tests.hpp" 6 | 7 | // Explicit instantiate all class templates so that all member methods 8 | // get compiled and for code coverage analysis. 9 | namespace Eigen { 10 | template class Map>; 11 | template class Map const>; 12 | } // namespace Eigen 13 | 14 | namespace Sophus { 15 | 16 | template class SO3; 17 | template class SO3; 18 | #if SOPHUS_CERES 19 | template class SO3>; 20 | #endif 21 | 22 | template 23 | class Tests { 24 | public: 25 | using SO3Type = SO3; 26 | using Point = typename SO3::Point; 27 | using Tangent = typename SO3::Tangent; 28 | Scalar const kPi = Constants::pi(); 29 | 30 | Tests() { 31 | so3_vec_.push_back(SO3Type(Eigen::Quaternion( 32 | Scalar(0.1e-11), Scalar(0.), Scalar(1.), Scalar(0.)))); 33 | so3_vec_.push_back(SO3Type(Eigen::Quaternion( 34 | Scalar(-1), Scalar(0.00001), Scalar(0.0), Scalar(0.0)))); 35 | so3_vec_.push_back( 36 | SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0)))); 37 | so3_vec_.push_back( 38 | SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(-1.0)))); 39 | so3_vec_.push_back(SO3Type::exp(Point(Scalar(0.), Scalar(0.), Scalar(0.)))); 40 | so3_vec_.push_back( 41 | SO3Type::exp(Point(Scalar(0.), Scalar(0.), Scalar(0.00001)))); 42 | so3_vec_.push_back(SO3Type::exp(Point(kPi, Scalar(0), Scalar(0)))); 43 | so3_vec_.push_back( 44 | SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0))) * 45 | SO3Type::exp(Point(kPi, Scalar(0), Scalar(0))) * 46 | SO3Type::exp(Point(Scalar(-0.2), Scalar(-0.5), Scalar(-0.0)))); 47 | so3_vec_.push_back( 48 | SO3Type::exp(Point(Scalar(0.3), Scalar(0.5), Scalar(0.1))) * 49 | SO3Type::exp(Point(kPi, Scalar(0), Scalar(0))) * 50 | SO3Type::exp(Point(Scalar(-0.3), Scalar(-0.5), Scalar(-0.1)))); 51 | tangent_vec_.push_back(Tangent(Scalar(0), Scalar(0), Scalar(0))); 52 | tangent_vec_.push_back(Tangent(Scalar(1), Scalar(0), Scalar(0))); 53 | tangent_vec_.push_back(Tangent(Scalar(0), Scalar(1), Scalar(0))); 54 | tangent_vec_.push_back( 55 | Tangent(Scalar(kPi / 2.), Scalar(kPi / 2.), Scalar(0))); 56 | tangent_vec_.push_back(Tangent(Scalar(-1), Scalar(1), Scalar(0))); 57 | tangent_vec_.push_back(Tangent(Scalar(20), Scalar(-1), Scalar(0))); 58 | tangent_vec_.push_back(Tangent(Scalar(30), Scalar(5), Scalar(-1))); 59 | 60 | point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4))); 61 | point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5))); 62 | } 63 | 64 | void runAll() { 65 | bool passed = testLieProperties(); 66 | passed &= testUnity(); 67 | passed &= testRawDataAcces(); 68 | passed &= testConstructors(); 69 | passed &= testFit(); 70 | processTestResult(passed); 71 | } 72 | 73 | private: 74 | bool testLieProperties() { 75 | LieGroupTests tests(so3_vec_, tangent_vec_, point_vec_); 76 | return tests.doAllTestsPass(); 77 | } 78 | 79 | bool testUnity() { 80 | bool passed = true; 81 | // Test that the complex number magnitude stays close to one. 82 | SO3Type current_q; 83 | for (std::size_t i = 0; i < 1000; ++i) { 84 | for (SO3Type const& q : so3_vec_) { 85 | current_q *= q; 86 | } 87 | } 88 | SOPHUS_TEST_APPROX(passed, current_q.unit_quaternion().norm(), Scalar(1), 89 | Constants::epsilon(), "Magnitude drift"); 90 | return passed; 91 | } 92 | 93 | bool testRawDataAcces() { 94 | bool passed = true; 95 | Eigen::Matrix raw = {Scalar(0), Scalar(1), Scalar(0), 96 | Scalar(0)}; 97 | Eigen::Map map_of_const_so3(raw.data()); 98 | SOPHUS_TEST_APPROX(passed, 99 | map_of_const_so3.unit_quaternion().coeffs().eval(), raw, 100 | Constants::epsilon()); 101 | SOPHUS_TEST_EQUAL( 102 | passed, map_of_const_so3.unit_quaternion().coeffs().data(), raw.data()); 103 | Eigen::Map const_shallow_copy = map_of_const_so3; 104 | SOPHUS_TEST_EQUAL(passed, 105 | const_shallow_copy.unit_quaternion().coeffs().eval(), 106 | map_of_const_so3.unit_quaternion().coeffs().eval()); 107 | 108 | Eigen::Matrix raw2 = {Scalar(1), Scalar(0), Scalar(0), 109 | Scalar(0)}; 110 | Eigen::Map map_of_so3(raw.data()); 111 | Eigen::Quaternion quat; 112 | quat.coeffs() = raw2; 113 | map_of_so3.setQuaternion(quat); 114 | SOPHUS_TEST_APPROX(passed, map_of_so3.unit_quaternion().coeffs().eval(), 115 | raw2, Constants::epsilon()); 116 | SOPHUS_TEST_EQUAL(passed, map_of_so3.unit_quaternion().coeffs().data(), 117 | raw.data()); 118 | SOPHUS_TEST_NEQ(passed, map_of_so3.unit_quaternion().coeffs().data(), 119 | quat.coeffs().data()); 120 | Eigen::Map shallow_copy = map_of_so3; 121 | SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_quaternion().coeffs().eval(), 122 | map_of_so3.unit_quaternion().coeffs().eval()); 123 | 124 | SO3Type const const_so3(quat); 125 | for (int i = 0; i < 4; ++i) { 126 | SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]); 127 | } 128 | 129 | SO3Type so3(quat); 130 | for (int i = 0; i < 4; ++i) { 131 | so3.data()[i] = raw[i]; 132 | } 133 | 134 | for (int i = 0; i < 4; ++i) { 135 | SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]); 136 | } 137 | 138 | SOPHUS_TEST_EQUAL( 139 | passed, SO3Type::rotX(Scalar(0.2)).matrix(), 140 | SO3Type::exp(Point(Scalar(0.2), Scalar(0), Scalar(0))).matrix()); 141 | SOPHUS_TEST_EQUAL( 142 | passed, SO3Type::rotY(Scalar(-0.2)).matrix(), 143 | SO3Type::exp(Point(Scalar(0), Scalar(-0.2), Scalar(0))).matrix()); 144 | SOPHUS_TEST_EQUAL( 145 | passed, SO3Type::rotZ(Scalar(1.1)).matrix(), 146 | SO3Type::exp(Point(Scalar(0), Scalar(0), Scalar(1.1))).matrix()); 147 | 148 | return passed; 149 | } 150 | 151 | bool testConstructors() { 152 | bool passed = true; 153 | Matrix3 R = so3_vec_.front().matrix(); 154 | SO3Type so3(R); 155 | SOPHUS_TEST_APPROX(passed, R, so3.matrix(), Constants::epsilon()); 156 | 157 | return passed; 158 | } 159 | 160 | template 161 | enable_if_t::value, bool> testFit() { 162 | bool passed = true; 163 | 164 | for (int i = 0; i < 100; ++i) { 165 | Matrix3 R = Matrix3::Random(); 166 | SO3Type so3 = SO3Type::fitToSO3(R); 167 | SO3Type so3_2 = SO3Type::fitToSO3(so3.matrix()); 168 | 169 | SOPHUS_TEST_APPROX(passed, so3.matrix(), so3_2.matrix(), 170 | Constants::epsilon()); 171 | } 172 | 173 | for (Scalar const angle : 174 | {Scalar(0.0), Scalar(0.1), Scalar(0.3), Scalar(-0.7)}) { 175 | SOPHUS_TEST_APPROX(passed, SO3Type::rotX(angle).angleX(), angle, 176 | Constants::epsilon()); 177 | SOPHUS_TEST_APPROX(passed, SO3Type::rotY(angle).angleY(), angle, 178 | Constants::epsilon()); 179 | SOPHUS_TEST_APPROX(passed, SO3Type::rotZ(angle).angleZ(), angle, 180 | Constants::epsilon()); 181 | } 182 | return passed; 183 | } 184 | 185 | template 186 | enable_if_t::value, bool> testFit() { 187 | return true; 188 | } 189 | 190 | std::vector> so3_vec_; 191 | std::vector> tangent_vec_; 192 | std::vector> point_vec_; 193 | }; 194 | 195 | int test_so3() { 196 | using std::cerr; 197 | using std::endl; 198 | 199 | cerr << "Test SO3" << endl << endl; 200 | cerr << "Double tests: " << endl; 201 | Tests().runAll(); 202 | cerr << "Float tests: " << endl; 203 | Tests().runAll(); 204 | 205 | #if SOPHUS_CERES 206 | cerr << "ceres::Jet tests: " << endl; 207 | Tests>().runAll(); 208 | #endif 209 | 210 | return 0; 211 | } 212 | } // namespace Sophus 213 | 214 | int main() { return Sophus::test_so3(); } 215 | -------------------------------------------------------------------------------- /test/core/test_velocities.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "tests.hpp" 4 | 5 | #include 6 | 7 | namespace Sophus { 8 | namespace experimental { 9 | 10 | template 11 | bool tests_linear_velocities() { 12 | bool passed = true; 13 | std::vector, Eigen::aligned_allocator>> bar_Ts_baz; 14 | 15 | for (size_t i = 0; i < 10; ++i) { 16 | bar_Ts_baz.push_back(SE3::rotX(i * 0.001) * 17 | SE3::rotY(i * 0.001) * 18 | SE3::transX(0.01 * i)); 19 | } 20 | 21 | SE3 foo_T_bar = 22 | SE3::rotX(0.5) * SE3::rotZ(0.2) * SE3::transY(2); 23 | 24 | std::vector, Eigen::aligned_allocator>> foo_Ts_baz; 25 | for (auto const& bar_T_baz : bar_Ts_baz) { 26 | foo_Ts_baz.push_back(foo_T_bar * bar_T_baz); 27 | } 28 | 29 | auto gen_linear_vels = []( 30 | std::vector, Eigen::aligned_allocator>> const& 31 | a_Ts_b) { 32 | std::vector, Eigen::aligned_allocator>> 33 | linearVels_a; 34 | for (size_t i = 0; i < a_Ts_b.size() - 1; ++i) { 35 | linearVels_a.push_back(a_Ts_b[i + 1].translation() - 36 | a_Ts_b[i].translation()); 37 | } 38 | return linearVels_a; 39 | }; 40 | 41 | // linear velocities in frame bar 42 | std::vector, Eigen::aligned_allocator>> 43 | linearVels_bar = gen_linear_vels(bar_Ts_baz); 44 | // linear velocities in frame foo 45 | std::vector, Eigen::aligned_allocator>> 46 | linearVels_foo = gen_linear_vels(foo_Ts_baz); 47 | 48 | for (size_t i = 0; i < linearVels_bar.size(); ++i) { 49 | SOPHUS_TEST_APPROX(passed, linearVels_foo[i], 50 | transformVelocity(foo_T_bar, linearVels_bar[i]), 51 | sqrt(Constants::epsilon())); 52 | } 53 | return passed; 54 | } 55 | 56 | template 57 | bool tests_rotational_velocities() { 58 | bool passed = true; 59 | 60 | SE3 foo_T_bar = 61 | SE3::rotX(0.5) * SE3::rotZ(0.2) * SE3::transY(2); 62 | 63 | // One parameter subgroup of SE3, motion through space given time t. 64 | auto bar_T_baz = [](Scalar t) -> SE3 { 65 | return SE3::rotX(t * Scalar(0.01)) * 66 | SE3::rotY(t * Scalar(0.0001)) * 67 | SE3::transX(t * Scalar(0.0001)); 68 | }; 69 | 70 | std::vector ts = {Scalar(0), Scalar(0.3), Scalar(1)}; 71 | 72 | Scalar h = Constants::epsilon(); 73 | for (Scalar t : ts) { 74 | // finite difference approximiation of instantanious velocity in frame bar 75 | Vector3 rotVel_in_frame_bar = 76 | finiteDifferenceRotationalVelocity(bar_T_baz, t, h); 77 | 78 | // finite difference approximiation of instantanious velocity in frame foo 79 | Vector3 rotVel_in_frame_foo = 80 | finiteDifferenceRotationalVelocity( 81 | [&foo_T_bar, bar_T_baz](Scalar t) -> SE3 { 82 | return foo_T_bar * bar_T_baz(t); 83 | }, 84 | t, h); 85 | 86 | Vector3 rotVel_in_frame_bar2 = 87 | transformVelocity(foo_T_bar.inverse(), rotVel_in_frame_foo); 88 | SOPHUS_TEST_APPROX( 89 | passed, rotVel_in_frame_bar, rotVel_in_frame_bar2, 90 | // not too tight threshold, because of finit difference approximation 91 | std::sqrt(Constants::epsilon())); 92 | 93 | // The rotational velocities rotVel_in_frame_foo and rotVel_in_frame_bar 94 | // should not be equal since they are in different frames (foo != bar). 95 | SOPHUS_TEST_NOT_APPROX(passed, rotVel_in_frame_foo, rotVel_in_frame_bar, 96 | Scalar(1e-3)); 97 | 98 | // Expect same result when using adjoint instead since: 99 | // vee(bar_R_foo * hat(vel_foo) * bar_R_foo^T = bar_R_foo 8 vel_foo. 100 | SOPHUS_TEST_APPROX( 101 | passed, transformVelocity(foo_T_bar.inverse(), rotVel_in_frame_foo), 102 | SO3::vee(foo_T_bar.so3().inverse().matrix() * 103 | SO3::hat(rotVel_in_frame_foo) * 104 | foo_T_bar.so3().matrix()), 105 | Constants::epsilon()); 106 | } 107 | return passed; 108 | } 109 | 110 | int test_velocities() { 111 | using std::cerr; 112 | using std::endl; 113 | 114 | cerr << "Test Velocities" << endl << endl; 115 | cerr << "Double tests: " << endl; 116 | bool passed = tests_linear_velocities(); 117 | passed &= tests_rotational_velocities(); 118 | processTestResult(passed); 119 | 120 | cerr << "Float tests: " << endl; 121 | passed = tests_linear_velocities(); 122 | passed &= tests_rotational_velocities(); 123 | processTestResult(passed); 124 | 125 | return 0; 126 | } 127 | } // namespace experimental 128 | } // namespace Sophus 129 | 130 | int main() { return Sophus::experimental::test_velocities(); } 131 | --------------------------------------------------------------------------------