diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml deleted file mode 100644 index 12d5935fd..000000000 --- a/.github/workflows/coverage.yml +++ /dev/null @@ -1,59 +0,0 @@ -name: Coverage Analysis - -on: - push: - branches: [ master ] - pull_request: - - workflow_dispatch: - -jobs: - build: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-20.04] - fail-fast: false - env: - BUILD_TYPE: Coverage - - - steps: - - uses: actions/checkout@v2 - - - name: ccache - uses: hendrikmuhs/ccache-action@v1 - - - name: Install dependencies (Linux) - run: | - ./scripts/install_linux_deps.sh - ./scripts/install_linux_fmt_deps.sh - - - name: Install Coverage - run: | - DEBIAN_FRONTEND=noninteractive sudo apt-get install -y \ - lcov - - - name: Run tests for coverage - run: | - mkdir build - cd build - cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=$BUILD_TYPE .. - make - make CTEST_OUTPUT_ON_FAILURE=1 test - - - name: Run lcov - run: | - set -x - cd build - lcov --directory . --capture --output-file coverage.info - lcov --remove coverage.info 'test/*' '/usr/*' --output-file coverage.info - lcov --list coverage.info - ls - pwd - - - name: Coveralls - uses: coverallsapp/github-action@master - with: - github-token: ${{ secrets.GITHUB_TOKEN }} - path-to-lcov: /home/runner/work/Sophus/Sophus/build/coverage.info diff --git a/.github/workflows/gh_pages.yml b/.github/workflows/gh_pages.yml deleted file mode 100644 index 429067c1c..000000000 --- a/.github/workflows/gh_pages.yml +++ /dev/null @@ -1,34 +0,0 @@ -name: Build Docs - -on: - push: - branches: [ master ] - - workflow_dispatch: - -jobs: - build: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-20.04] - fail-fast: false - - - steps: - - uses: actions/checkout@v2 - - - name: build docs - run: | - scripts/install_docs_deps.sh - ./make_docs.sh - cd html-dir - touch .nojekyll - cd ../.. - - - name: Deploy - uses: peaceiris/actions-gh-pages@v3 - with: - github_token: ${{ secrets.GITHUB_TOKEN }} - publish_dir: html-dir - publish_branch: gh_pages diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 2e4988f1b..5da823229 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -12,51 +12,43 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - os: [ubuntu-20.04] - build_type: [Debug, Release] - row_major: [ON, OFF] - basic_logging: [ON, OFF] - exclude: - - build_type: Release + include: + - os: ubuntu-22.04 + build_type: Release row_major: ON - - build_type: Release - basic_logging: ON - - build_type: Debug - row_major: ON - basic_logging: ON + - os: ubuntu-22.04 + build_type: RelWithDebInfo + row_major: OFF + - os: ubuntu-24.04 + build_type: RelWithDebInfo + row_major: OFF + - os: macos-13 + build_type: RelWithDebInfo + row_major: OFF + - os: macos-14 + build_type: RelWithDebInfo + row_major: OFF fail-fast: false env: - BUILD_TYPE: ${{ matrix.build_type }} ROW_MAJOR_DEFAULT: ${{ matrix.row_major }} - USE_BASIC_LOGGING: ${{ matrix.basic_logging }} - steps: - uses: actions/checkout@v2 - - name: Format - uses: DoozyX/clang-format-lint-action@v0.12 - with: - source: '.' - extensions: 'hpp,cpp' - exclude: './sympy ./doxyrest_b' - clangFormatVersion: 9 - if: matrix.os == 'ubuntu-20.04' - - name: ccache uses: hendrikmuhs/ccache-action@v1 - - name: Install dependencies (Linux) - run: ./scripts/install_linux_deps.sh - if: matrix.os == 'ubuntu-20.04' + - name: Install dependencies (Linux incl. Ceres) + run: ./scripts/install_ubuntu22.04_deps.sh + if: matrix.os == 'ubuntu-22.04' - - name: Install fmt dependency (Linux) - run: ./scripts/install_linux_fmt_deps.sh - if: matrix.os == 'ubuntu-20.04' && matrix.basic_logging == 'OFF' + - name: Install dependencies (Linux NO Ceres) + run: ./scripts/install_ubuntu24.04_deps.sh + if: matrix.os == 'ubuntu-24.04' - name: Install dependencies (Mac OS) run: ./scripts/install_osx_deps.sh - if: matrix.os == 'macos-10.15' + if: matrix.os == 'macos-14' || matrix.os == 'macos-13' - name: Run tests run: ./scripts/run_cpp_tests.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index eed097231..a22ffead7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.4) -project(Sophus VERSION 1.22.10) +cmake_minimum_required(VERSION 3.16) +project(Sophus VERSION 1.24.06) include(CMakePackageConfigHelpers) include(GNUInstallDirs) @@ -15,7 +15,6 @@ if (NOT DEFINED SOPHUS_MASTER_PROJECT) endif () option(SOPHUS_INSTALL "Generate the install target." ${SOPHUS_MASTER_PROJECT}) -option(SOPHUS_USE_BASIC_LOGGING "Use basic logging (in ensure and test macros)" OFF) if(SOPHUS_MASTER_PROJECT) # Release by default @@ -24,7 +23,7 @@ if(SOPHUS_MASTER_PROJECT) set(CMAKE_BUILD_TYPE Release) endif() - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) # Set compiler specific settings (FixMe: Should not cmake do this for us automatically?) IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") @@ -35,9 +34,6 @@ if(SOPHUS_MASTER_PROJECT) SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") SET(CMAKE_CXX_FLAGS_RELEASE "-O3") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++14 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0") - SET(CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_DEBUG} --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") - SET(CMAKE_EXE_LINKER_FLAGS_COVERAGE "${CMAKE_EXE_LINKER_FLAGS_DEBUG} --coverage") - SET(CMAKE_SHARED_LINKER_FLAGS_COVERAGE "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} --coverage") ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES "^MSVC$") ADD_DEFINITIONS("-D _USE_MATH_DEFINES /bigobj /wd4305 /wd4244 /MP") ENDIF() @@ -46,10 +42,6 @@ if(SOPHUS_MASTER_PROJECT) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules") endif() -if(SOPHUS_USE_BASIC_LOGGING) - set (CMAKE_DISABLE_FIND_PACKAGE_fmt ON) -endif() - # Find public dependencies if targets are not yet defined. (Targets might be for example # defined by a parent project including Sophus via `add_subdirectory`.) @@ -89,7 +81,6 @@ set(SOPHUS_HEADER_FILES sophus/so3.hpp sophus/spline.hpp sophus/types.hpp - sophus/velocities.hpp ) set(SOPHUS_OTHER_FILES @@ -110,16 +101,10 @@ else() target_include_directories (sophus SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR}) endif() -if(SOPHUS_USE_BASIC_LOGGING OR NOT TARGET fmt::fmt) - # NOTE fmt_FOUND does not seem to be defined even though the package config - # was found. - target_compile_definitions(sophus INTERFACE SOPHUS_USE_BASIC_LOGGING=1) - message(STATUS "Turning basic logging ON") -else() - target_link_libraries(sophus INTERFACE fmt::fmt) - set(fmt_DEPENDENCY "find_dependency (fmt ${fmt_VERSION})") - message(STATUS "Turning basic logging OFF") -endif() + +target_link_libraries(sophus INTERFACE fmt::fmt) +set(fmt_DEPENDENCY "find_dependency (fmt ${fmt_VERSION})") +message(STATUS "Turning basic logging OFF") # Associate target with include directory target_include_directories(sophus INTERFACE diff --git a/README.rst b/README.rst index e7aab639b..f5a34e22a 100644 --- a/README.rst +++ b/README.rst @@ -24,21 +24,23 @@ The specific compiler and operating system versions which are supported are the ones which are used in the Continuous Integration (CI): See GitHubCI_ and AppVeyor_ for details. -However, it should work (with no to minor modification) on many other -modern configurations as long they support c++14, CMake, Eigen 3.3.X and -(optionally) fmt. The fmt dependency can be eliminated by passing -"-DUSE_BASIC_LOGGING=ON" to cmake when configuring Sophus. - .. _GitHubCI: https://github.com/strasdat/Sophus/actions -.. |AppVeyor| image:: https://ci.appveyor.com/api/projects/status/um4285lwhs8ci7pt/branch/master?svg=true -.. _AppVeyor: https://ci.appveyor.com/project/strasdat/sophus/branch/master - -.. |ci_cov| image:: https://coveralls.io/repos/github/strasdat/Sophus/badge.svg?branch=master -.. _ci_cov: https://coveralls.io/github/strasdat/Sophus?branch=master - .. |GithubCICpp| image:: https://github.com/strasdat/Sophus/actions/workflows/main.yml/badge.svg?branch=master .. _GithubCICpp: https://github.com/strasdat/Sophus/actions/workflows/main.yml?query=branch%3Amaster .. |GithubCISympy| image:: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml/badge.svg?branch=master .. _GithubCISympy: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml?query=branch%3Amaster + + +Install Ubuntu 22.04 +-------------------- + +sudo apt install libeigen3-dev +sudo apt install libfmt-dev +sudo apt install libceres-dev + +mkdir build +cd build +cmake .. +make -j4 \ No newline at end of file diff --git a/Sophus.code-workspace b/Sophus.code-workspace index 876a1499c..8fef94ca5 100644 --- a/Sophus.code-workspace +++ b/Sophus.code-workspace @@ -4,5 +4,67 @@ "path": "." } ], - "settings": {} + "settings": { + "files.associations": { + "type_traits": "cpp", + "array": "cpp", + "optional": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "cctype": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "utility": "cpp", + "initializer_list": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp" + } + } } \ No newline at end of file diff --git a/appveyor.yml b/appveyor.yml deleted file mode 100644 index 6fe6813fb..000000000 --- a/appveyor.yml +++ /dev/null @@ -1,35 +0,0 @@ -branches: - only: - - master - -os: Visual Studio 2015 - -clone_folder: c:\projects\sophus - -platform: x64 -configuration: Debug - -build: - project: c:\projects\sophus\build\Sophus.sln - -install: - - ps: wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.zip -outfile eigen3.zip - - cmd: 7z x eigen3.zip -o"C:\projects" -y > nul - - git clone https://github.com/fmtlib/fmt.git - - cd fmt - - git checkout 5.3.0 - - mkdir build - - cd build - - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug .. - - cmake --build . - - cmake --build . --target install - - cd ../.. - -before_build: - - cd c:\projects\sophus - - mkdir build - - cd build - - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug -D EIGEN3_INCLUDE_DIR=C:\projects\eigen-3.3.4 .. - -after_build: - - ctest --output-on-failure diff --git a/doxyfile b/doxyfile deleted file mode 100644 index 995ed8991..000000000 --- a/doxyfile +++ /dev/null @@ -1,24 +0,0 @@ -DOXYFILE_ENCODING = UTF-8 -PROJECT_NAME = "Sophus" -INPUT = sophus -EXTRACT_ALL = YES -ENABLE_PREPROCESSING = YES -MACRO_EXPANSION = YES -WARN_AS_ERROR = YES -EXPAND_ONLY_PREDEF = NO -SKIP_FUNCTION_MACROS = NO -AUTOLINK_SUPPORT = YES -MULTILINE_CPP_IS_BRIEF = YES -MARKDOWN_SUPPORT = YES -INLINE_INHERITED_MEMB = NO -EXCLUDE_SYMBOLS = Eigen::internal Sophus::details Sophus::interp_details Sophus::experimental -GENERATE_LATEX = NO -STRIP_CODE_COMMENTS = NO - -GENERATE_XML = YES -GENERATE_HTML = NO -XML_OUTPUT = xml-dir -XML_PROGRAMLISTING = NO -CASE_SENSE_NAMES = NO -HIDE_UNDOC_RELATIONS = YES -EXTRACT_ALL = YES \ No newline at end of file diff --git a/doxyrest-config.lua b/doxyrest-config.lua deleted file mode 100644 index c4e8bc864..000000000 --- a/doxyrest-config.lua +++ /dev/null @@ -1,20 +0,0 @@ -FRAME_DIR_LIST = { "doxyrest_b/doxyrest/frame/cfamily", "doxyrest_b/doxyrest/frame/common" } -FRAME_FILE = "index.rst.in" -INPUT_FILE = "xml-dir/index.xml" -OUTPUT_FILE = "rst-dir/index.rst" -INTRO_FILE = "page_index.rst" -SORT_GROUPS_BY = "title" -GLOBAL_AUX_COMPOUND_ID = "group_global" -LANGUAGE = cpp -VERBATIM_TO_CODE_BLOCK = "cpp" -ESCAPE_ASTERISKS = true -ESCAPE_PIPES = true -ESCAPE_TRAILING_UNDERSCORES = true -PROTECTION_FILTER = "protected" -EXCLUDE_EMPTY_DEFINES = true -EXCLUDE_DEFAULT_CONSTRUCTORS = false -EXCLUDE_DESTRUCTORS = false -EXCLUDE_PRIMITIVE_TYPEDEFS = true -SHOW_DIRECT_DESCENDANTS = true -TYPEDEF_TO_USING = true -ML_PARAM_LIST_LENGTH_THRESHOLD = 80 \ No newline at end of file diff --git a/make_docs.sh b/make_docs.sh deleted file mode 100755 index 6393cef3d..000000000 --- a/make_docs.sh +++ /dev/null @@ -1,3 +0,0 @@ -doxygen doxyfile -doxyrest_b/build/doxyrest/bin/Release/doxyrest -c doxyrest-config.lua -sphinx-build -b html rst-dir html-dir \ No newline at end of file diff --git a/rst-dir/conf.py b/rst-dir/conf.py deleted file mode 100644 index 76ed90a84..000000000 --- a/rst-dir/conf.py +++ /dev/null @@ -1,54 +0,0 @@ -# Configuration file for the Sphinx documentation builder. -# -# This file only contains a selection of the most common options. For a full -# list see the documentation: -# http://www.sphinx-doc.org/en/master/config - -# -- Path setup -------------------------------------------------------------- - -# If extensions (or modules to document with autodoc) are in another directory, -# add these directories to sys.path here. If the directory is relative to the -# documentation root, use os.path.abspath to make it absolute, like shown here. -# -import os -import sys -sys.path.insert(0, os.path.abspath('../sympy')) - - -sys.path.insert(1, os.path.abspath('../doxyrest_b/doxyrest/sphinx')) -extensions = ['doxyrest', 'cpplexer', 'sphinx.ext.autodoc'] - -# -- Project information ----------------------------------------------------- - -project = 'Sophus' -copyright = '2019, Hauke Strasdat' -author = 'Hauke Strasdat' - - -# Tell sphinx what the primary language being documented is. -primary_domain = 'cpp' - -# Tell sphinx what the pygments highlight language should be. -highlight_language = 'cpp' - - -# Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] - -# List of patterns, relative to source directory, that match files and -# directories to ignore when looking for source files. -# This pattern also affects html_static_path and html_extra_path. -exclude_patterns = [] - - -# -- Options for HTML output ------------------------------------------------- - -# The theme to use for HTML and HTML Help pages. See the documentation for -# a list of builtin themes. -# -html_theme = "sphinx_rtd_theme" - -# Add any paths that contain custom static files (such as style sheets) here, -# relative to this directory. They are copied after the builtin static files, -# so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] diff --git a/rst-dir/page_index.rst b/rst-dir/page_index.rst deleted file mode 100644 index 9955aca10..000000000 --- a/rst-dir/page_index.rst +++ /dev/null @@ -1,9 +0,0 @@ -Sophus - Lie groups for 2d/3d Geometry -======================================= - -.. toctree:: - :maxdepth: 2 - :caption: Contents: - - GitHub Page - pysophus \ No newline at end of file diff --git a/rst-dir/pysophus.rst b/rst-dir/pysophus.rst deleted file mode 100644 index 3e532a0ac..000000000 --- a/rst-dir/pysophus.rst +++ /dev/null @@ -1,23 +0,0 @@ -Python API -========== - -.. automodule:: sophus.matrix - :members: - -.. automodule:: sophus.complex - :members: - -.. automodule:: sophus.quaternion - :members: - -.. automodule:: sophus.so2 - :members: - -.. automodule:: sophus.so3 - :members: - -.. automodule:: sophus.se2 - :members: - -.. automodule:: sophus.se3 - :members: \ No newline at end of file diff --git a/run_format.sh b/run_format.sh deleted file mode 100755 index ef6b7403d..000000000 --- a/run_format.sh +++ /dev/null @@ -1 +0,0 @@ -find . -type d \( -path ./sympy -o -path ./doxyrest_b -o -path "./*/CMakeFiles/*" \) -prune -o \( -iname "*.hpp" -o -iname "*.cpp" \) -print | xargs clang-format -i diff --git a/scripts/install_docs_deps.sh b/scripts/install_docs_deps.sh deleted file mode 100755 index c947a41e8..000000000 --- a/scripts/install_docs_deps.sh +++ /dev/null @@ -1,21 +0,0 @@ -#!/bin/bash - -set -x # echo on -set -e # exit on error - -sudo apt-get -qq update -sudo apt-get install doxygen liblua5.3-dev ragel -pip3 install 'sphinx==2.0.1' -pip3 install sphinx_rtd_theme -pip3 install sympy - -git clone https://github.com/vovkos/doxyrest_b -cd doxyrest_b -git reset --hard ad45c064d1199e71b8cae5aa66d4251c4228b958 -git submodule update --init -mkdir build -cd build -cmake .. -cmake --build . - -cd ../.. diff --git a/scripts/install_linux_deps.sh b/scripts/install_linux_deps.sh deleted file mode 100755 index 39c188de4..000000000 --- a/scripts/install_linux_deps.sh +++ /dev/null @@ -1,27 +0,0 @@ -#!/bin/bash - -set -x # echo on -set -e # exit on error - -cmake --version - -sudo apt-get -qq update -sudo apt-get install gfortran libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev libceres-dev ccache -wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.tar.bz2 -tar xvf eigen-3.3.4.tar.bz2 -mkdir build-eigen -cd build-eigen -cmake ../eigen-3.3.4 -DEIGEN_DEFAULT_TO_ROW_MAJOR=$ROW_MAJOR_DEFAULT -sudo make install -cd .. - -git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver -cd ceres-solver -git reset --hard b0aef211db734379319c19c030e734d6e23436b0 -mkdir build -cd build -ccache -s -cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. -make -j8 -sudo make install -cd ../.. diff --git a/scripts/install_linux_fmt_deps.sh b/scripts/install_linux_fmt_deps.sh deleted file mode 100755 index acbfda2ca..000000000 --- a/scripts/install_linux_fmt_deps.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/bin/bash - -set -x # echo on -set -e # exit on error - -git clone https://github.com/fmtlib/fmt.git -cd fmt -git checkout 5.3.0 -mkdir build -cd build -cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. -make -j8 -sudo make install -cd ../.. diff --git a/scripts/install_osx_deps.sh b/scripts/install_osx_deps.sh index e2125dff7..1eec3ca26 100755 --- a/scripts/install_osx_deps.sh +++ b/scripts/install_osx_deps.sh @@ -7,46 +7,24 @@ brew update brew install fmt brew install ccache -# Build a specific version of ceres-solver instead of one shipped over brew -curl https://raw.githubusercontent.com/Homebrew/homebrew-core/b0792ccba6e71cd028263ca7621db894afc602d2/Formula/ceres-solver.rb -o ceres-solver.rb -patch < -optional iterativeMean( +std::optional iterativeMean( SequenceContainer const& foo_Ts_bar, int max_num_iterations) { size_t N = foo_Ts_bar.size(); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); @@ -52,23 +52,16 @@ optional iterativeMean( foo_T_average = foo_T_newaverage; } // LCOV_EXCL_START - return nullopt; + return std::nullopt; // LCOV_EXCL_STOP } -#ifdef DOXYGEN_SHOULD_SKIP_THIS -/// Mean implementation for any Lie group. -template -optional average( - SequenceContainer const& foo_Ts_bar); -#else - // Mean implementation for Cartesian. template -enable_if_t >::value, - optional > +std::enable_if_t >::value, + std::optional > average(SequenceContainer const& foo_Ts_bar) { size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); @@ -84,9 +77,9 @@ average(SequenceContainer const& foo_Ts_bar) { // Mean implementation for SO(2). template -enable_if_t< +std::enable_if_t< std::is_same >::value, - optional > + std::optional > average(SequenceContainer const& foo_Ts_bar) { // This implements rotational part of Proposition 12 from Sec. 6.2 of // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. @@ -105,9 +98,9 @@ average(SequenceContainer const& foo_Ts_bar) { // Mean implementation for RxSO(2). template -enable_if_t< +std::enable_if_t< std::is_same >::value, - optional > + std::optional > average(SequenceContainer const& foo_Ts_bar) { size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); @@ -179,9 +172,9 @@ Eigen::Quaternion averageUnitQuaternion( // TODO: Detect degenerated cases and return nullopt. template -enable_if_t< +std::enable_if_t< std::is_same >::value, - optional > + std::optional > average(SequenceContainer const& foo_Ts_bar) { return SO3(details::averageUnitQuaternion(foo_Ts_bar)); } @@ -189,9 +182,9 @@ average(SequenceContainer const& foo_Ts_bar) { // Mean implementation for R x SO(3). template -enable_if_t< +std::enable_if_t< std::is_same >::value, - optional > + std::optional > average(SequenceContainer const& foo_Ts_bar) { size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); @@ -208,9 +201,9 @@ average(SequenceContainer const& foo_Ts_bar) { template -enable_if_t< +std::enable_if_t< std::is_same >::value, - optional > + std::optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { // TODO: Implement Proposition 12 from Sec. 6.2 of // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. @@ -219,31 +212,29 @@ average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { template -enable_if_t< +std::enable_if_t< std::is_same >::value, - optional > + std::optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { return iterativeMean(foo_Ts_bar, max_num_iterations); } template -enable_if_t< +std::enable_if_t< std::is_same >::value, - optional > + std::optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { return iterativeMean(foo_Ts_bar, max_num_iterations); } template -enable_if_t< +std::enable_if_t< std::is_same >::value, - optional > + std::optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { return iterativeMean(foo_Ts_bar, max_num_iterations); } -#endif // DOXYGEN_SHOULD_SKIP_THIS - } // namespace Sophus diff --git a/sophus/cartesian.hpp b/sophus/cartesian.hpp index 8ade57eae..ee3e210a5 100644 --- a/sophus/cartesian.hpp +++ b/sophus/cartesian.hpp @@ -191,8 +191,8 @@ class CartesianBase { /// Group action on points, again just vector addition. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return PointProduct(params() + p); @@ -201,8 +201,8 @@ class CartesianBase { /// Group action on homogeneous points. See above for more details. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const auto rp = *this * p.template head(); @@ -228,8 +228,8 @@ class CartesianBase { /// type. /// template >::value>::type> + typename = typename std::enable_if_t< + std::is_same>::value>> SOPHUS_FUNC CartesianBase& operator*=( CartesianBase const& other) { *static_cast(this) = *this * other; diff --git a/sophus/ceres_typetraits.hpp b/sophus/ceres_typetraits.hpp index 8bf63159e..ed541b8f8 100644 --- a/sophus/ceres_typetraits.hpp +++ b/sophus/ceres_typetraits.hpp @@ -42,7 +42,7 @@ struct Mapper { }; template -struct Mapper>::type> { +struct Mapper>::type> { using Scalar = typename T::Scalar; using Map = Eigen::Map; using ConstMap = Eigen::Map; diff --git a/sophus/common.hpp b/sophus/common.hpp index 5634d35b0..7098a720f 100644 --- a/sophus/common.hpp +++ b/sophus/common.hpp @@ -6,33 +6,12 @@ #include #include #include +#include #include #include #include -#undef SOPHUS_COMPILE_TIME_FMT - -#ifdef SOPHUS_USE_BASIC_LOGGING - -#define SOPHUS_FMT_CSTR(description, ...) description -#define SOPHUS_FMT_STR(description, ...) std::string(description) -#define SOPHUS_FMT_PRINT(description, ...) std::printf("%s\n", description) -#define SOPHUS_FMT_ARG(arg) - -#else // !SOPHUS_USE_BASIC_LOGGING - -#ifdef __linux__ -#define SOPHUS_COMPILE_TIME_FMT -#endif - -#ifdef __APPLE__ -#include "TargetConditionals.h" -#ifdef TARGET_OS_MAC -#define SOPHUS_COMPILE_TIME_FMT -#endif -#endif - #include #include @@ -42,33 +21,16 @@ #define SOPHUS_FMT_ARG(arg) arg #endif -#ifdef SOPHUS_COMPILE_TIME_FMT -// To keep compatibility with older libfmt versions, -// disable the compile time check if FMT_STRING is not available. -#ifdef FMT_STRING -// compile-time format check on x -#define SOPHUS_FMT_STRING(x) FMT_STRING(x) -#else -// identity, hence no compile-time check on x -#define SOPHUS_FMT_STRING(x) x -#endif -#else // ! SOPHUS_COMPILE_TIME_FMT -// identity, hence no compile-time check on x -#define SOPHUS_FMT_STRING(x) x -#endif // ! SOPHUS_COMPILE_TIME_FMT - #define SOPHUS_FMT_CSTR(description, ...) \ - fmt::format(SOPHUS_FMT_STRING(description), ##__VA_ARGS__).c_str() + fmt::format(description, ##__VA_ARGS__).c_str() #define SOPHUS_FMT_STR(description, ...) \ - fmt::format(SOPHUS_FMT_STRING(description), ##__VA_ARGS__) + fmt::format(description, ##__VA_ARGS__) #define SOPHUS_FMT_PRINT(description, ...) \ - fmt::print(SOPHUS_FMT_STRING(description), ##__VA_ARGS__); \ + fmt::print(description, ##__VA_ARGS__); \ fmt::print("\n") -#endif // !SOPHUS_USE_BASIC_LOGGING - // following boost's assert.hpp #undef SOPHUS_ENSURE @@ -186,61 +148,11 @@ struct Constants { } }; -/// Nullopt type of lightweight optional class. -struct nullopt_t { - explicit constexpr nullopt_t() {} -}; - -constexpr nullopt_t nullopt{}; - -/// Lightweight optional implementation which requires ``T`` to have a -/// default constructor. -/// -/// TODO: Replace with std::optional once Sophus moves to c++17. -/// -template -class optional { - public: - optional() : is_valid_(false) {} - - optional(nullopt_t) : is_valid_(false) {} - - optional(T const& type) : type_(type), is_valid_(true) {} - - explicit operator bool() const { return is_valid_; } - - T const* operator->() const { - SOPHUS_ENSURE(is_valid_, "must be valid"); - return &type_; - } - - T* operator->() { - SOPHUS_ENSURE(is_valid_, "must be valid"); - return &type_; - } - - T const& operator*() const { - SOPHUS_ENSURE(is_valid_, "must be valid"); - return type_; - } - - T& operator*() { - SOPHUS_ENSURE(is_valid_, "must be valid"); - return type_; - } - - private: - T type_; - bool is_valid_; -}; - -template -using enable_if_t = typename std::enable_if::type; - template struct IsUniformRandomBitGenerator { static const bool value = std::is_unsigned::value && std::is_unsigned::value && std::is_unsigned::value; }; + } // namespace Sophus diff --git a/sophus/interpolate.hpp b/sophus/interpolate.hpp index ff509e4bf..fd224e166 100644 --- a/sophus/interpolate.hpp +++ b/sophus/interpolate.hpp @@ -23,7 +23,7 @@ namespace Sophus { /// Precondition: ``p`` must be in [0, 1]. /// template -enable_if_t::supported, G> interpolate( +std::enable_if_t::supported, G> interpolate( G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) { using Scalar = typename G::Scalar; Scalar inter_p(p); diff --git a/sophus/rotation_matrix.hpp b/sophus/rotation_matrix.hpp index 358ce7a4a..547ee0cb6 100644 --- a/sophus/rotation_matrix.hpp +++ b/sophus/rotation_matrix.hpp @@ -55,7 +55,7 @@ SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase const& sR) { /// Takes in arbitrary square matrix (2x2 or larger) and returns closest /// orthogonal matrix with positive determinant. template -SOPHUS_FUNC enable_if_t< +SOPHUS_FUNC std::enable_if_t< std::is_floating_point::value, Matrix> makeRotationMatrix(Eigen::MatrixBase const& R) { diff --git a/sophus/rxso2.hpp b/sophus/rxso2.hpp index 60d0b8a03..d9a81f5fa 100644 --- a/sophus/rxso2.hpp +++ b/sophus/rxso2.hpp @@ -249,8 +249,8 @@ class RxSO2Base { /// ``p_bar = s * (bar_R_foo * p_foo)``. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return matrix() * p; @@ -259,8 +259,8 @@ class RxSO2Base { /// Group action on homogeneous 2-points. See above for more details. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const auto rsp = *this * p.template head<2>(); @@ -303,8 +303,8 @@ class RxSO2Base { /// order to ensure the class invariant. /// template >::value>::type> + typename = typename std::enable_if_t< + std::is_same>::value>> SOPHUS_FUNC RxSO2Base& operator*=( RxSO2Base const& other) { *static_cast(this) = *this * other; diff --git a/sophus/rxso3.hpp b/sophus/rxso3.hpp index d2c70ba72..65907e0f8 100644 --- a/sophus/rxso3.hpp +++ b/sophus/rxso3.hpp @@ -272,8 +272,8 @@ class RxSO3Base { /// ``p_bar = s * (bar_R_foo * p_foo)``. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { // Follows http:///eigen.tuxfamily.org/bz/show_bug.cgi?id=459 @@ -287,8 +287,8 @@ class RxSO3Base { /// Group action on homogeneous 3-points. See above for more details. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const auto rsp = *this * p.template head<3>(); @@ -329,8 +329,8 @@ class RxSO3Base { /// order to ensure the class invariant. /// template >::value>::type> + typename = typename std::enable_if_t< + std::is_same>::value>> SOPHUS_FUNC RxSO3Base& operator*=( RxSO3Base const& other) { *static_cast(this) = *this * other; diff --git a/sophus/se2.hpp b/sophus/se2.hpp index 5851411b7..bfab776c5 100644 --- a/sophus/se2.hpp +++ b/sophus/se2.hpp @@ -255,8 +255,8 @@ class SE2Base { /// ``p_bar = bar_R_foo * p_foo + t_bar``. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return so2() * p + translation(); @@ -265,8 +265,8 @@ class SE2Base { /// Group action on homogeneous 2-points. See above for more details. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const PointProduct tp = @@ -307,8 +307,8 @@ class SE2Base { /// type of the multiplication is compatible with this SO2's Scalar type. /// template >::value>::type> + typename = typename std::enable_if_t< + std::is_same>::value>> SOPHUS_FUNC SE2Base& operator*=(SE2Base const& other) { *static_cast(this) = *this * other; return *this; @@ -633,7 +633,7 @@ class SE2 : public SE2Base> { /// Returns closest SE3 given arbitrary 4x4 matrix. /// template - static SOPHUS_FUNC enable_if_t::value, SE2> + static SOPHUS_FUNC std::enable_if_t::value, SE2> fitToSE2(Matrix3 const& T) { return SE2(SO2::fitToSO2(T.template block<2, 2>(0, 0)), T.template block<2, 1>(0, 2)); diff --git a/sophus/se3.hpp b/sophus/se3.hpp index f65a13e0f..49c8d5a79 100644 --- a/sophus/se3.hpp +++ b/sophus/se3.hpp @@ -314,8 +314,8 @@ class SE3Base { /// ``p_bar = bar_R_foo * p_foo + t_bar``. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return so3() * p + translation(); @@ -324,8 +324,8 @@ class SE3Base { /// Group action on homogeneous 3-points. See above for more details. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const PointProduct tp = @@ -363,8 +363,8 @@ class SE3Base { /// type of the multiplication is compatible with this SE3's Scalar type. /// template >::value>::type> + typename = typename std::enable_if_t< + std::is_same>::value>> SOPHUS_FUNC SE3Base& operator*=(SE3Base const& other) { *static_cast(this) = *this * other; return *this; @@ -863,7 +863,7 @@ class SE3 : public SE3Base> { /// Returns closest SE3 given arbitrary 4x4 matrix. /// template - SOPHUS_FUNC static enable_if_t::value, SE3> + SOPHUS_FUNC static std::enable_if_t::value, SE3> fitToSE3(Matrix4 const& T) { return SE3(SO3::fitToSO3(T.template block<3, 3>(0, 0)), T.template block<3, 1>(0, 3)); diff --git a/sophus/sim2.hpp b/sophus/sim2.hpp index c69cb8703..edb48775d 100644 --- a/sophus/sim2.hpp +++ b/sophus/sim2.hpp @@ -221,8 +221,8 @@ class Sim2Base { /// ``p_bar = bar_sR_foo * p_foo + t_bar``. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return rxso2() * p + translation(); @@ -231,8 +231,8 @@ class Sim2Base { /// Group action on homogeneous 2-points. See above for more details. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const PointProduct tp = @@ -285,8 +285,8 @@ class Sim2Base { /// type of the multiplication is compatible with this SO2's Scalar type. /// template >::value>::type> + typename = typename std::enable_if_t< + std::is_same>::value>> SOPHUS_FUNC Sim2Base& operator*=( Sim2Base const& other) { *static_cast(this) = *this * other; diff --git a/sophus/sim3.hpp b/sophus/sim3.hpp index a7a6eb3a1..e66e2b53b 100644 --- a/sophus/sim3.hpp +++ b/sophus/sim3.hpp @@ -223,8 +223,8 @@ class Sim3Base { /// ``p_bar = bar_sR_foo * p_foo + t_bar``. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return rxso3() * p + translation(); @@ -233,8 +233,8 @@ class Sim3Base { /// Group action on homogeneous 3-points. See above for more details. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const PointProduct tp = @@ -273,8 +273,8 @@ class Sim3Base { /// type of the multiplication is compatible with this SO3's Scalar type. /// template >::value>::type> + typename = typename std::enable_if_t< + std::is_same>::value>> SOPHUS_FUNC Sim3Base& operator*=( Sim3Base const& other) { *static_cast(this) = *this * other; diff --git a/sophus/so2.hpp b/sophus/so2.hpp index dcdabd733..ef6e5cf9c 100644 --- a/sophus/so2.hpp +++ b/sophus/so2.hpp @@ -240,8 +240,8 @@ class SO2Base { /// ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { Scalar const& real = unit_complex().x(); @@ -256,8 +256,8 @@ class SO2Base { /// element ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { Scalar const& real = unit_complex().x(); @@ -296,8 +296,8 @@ class SO2Base { /// type of the multiplication is compatible with this SO2's Scalar type. /// template >::value>::type> + typename = typename std::enable_if_t< + std::is_same>::value>> SOPHUS_FUNC SO2Base operator*=(SO2Base const& other) { *static_cast(this) = *this * other; return *this; @@ -520,7 +520,7 @@ class SO2 : public SO2Base> { /// Returns closed SO2 given arbitrary 2x2 matrix. /// template - static SOPHUS_FUNC enable_if_t::value, SO2> + static SOPHUS_FUNC std::enable_if_t::value, SO2> fitToSO2(Transformation const& R) { return SO2(makeRotationMatrix(R)); } diff --git a/sophus/so3.hpp b/sophus/so3.hpp index 67dd852fd..3bdb9b2f2 100644 --- a/sophus/so3.hpp +++ b/sophus/so3.hpp @@ -135,7 +135,8 @@ class SO3Base { /// Extract rotation angle about canonical X-axis /// template - SOPHUS_FUNC enable_if_t::value, S> angleX() const { + SOPHUS_FUNC std::enable_if_t::value, S> angleX() + const { Sophus::Matrix3 R = matrix(); Sophus::Matrix2 Rx = R.template block<2, 2>(1, 1); return SO2(makeRotationMatrix(Rx)).log(); @@ -144,7 +145,8 @@ class SO3Base { /// Extract rotation angle about canonical Y-axis /// template - SOPHUS_FUNC enable_if_t::value, S> angleY() const { + SOPHUS_FUNC std::enable_if_t::value, S> angleY() + const { Sophus::Matrix3 R = matrix(); Sophus::Matrix2 Ry; // clang-format off @@ -158,7 +160,8 @@ class SO3Base { /// Extract rotation angle about canonical Z-axis /// template - SOPHUS_FUNC enable_if_t::value, S> angleZ() const { + SOPHUS_FUNC std::enable_if_t::value, S> angleZ() + const { Sophus::Matrix3 R = matrix(); Sophus::Matrix2 Rz = R.template block<2, 2>(0, 0); return SO2(makeRotationMatrix(Rz)).log(); @@ -383,8 +386,8 @@ class SO3Base { /// For ``vee``-operator, see below. /// template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { /// NOTE: We cannot use Eigen's Quaternion transformVector because it always @@ -398,8 +401,8 @@ class SO3Base { /// Group action on homogeneous 3-points. See above for more details. template ::value>::type> + typename = typename std::enable_if_t< + IsFixedSizeVector::value>> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const auto rp = *this * p.template head<3>(); @@ -433,8 +436,8 @@ class SO3Base { /// type of the multiplication is compatible with this SO3's Scalar type. /// template >::value>::type> + typename = typename std::enable_if_t< + std::is_same>::value>> SOPHUS_FUNC SO3Base& operator*=(SO3Base const& other) { *static_cast(this) = *this * other; return *this; @@ -548,7 +551,7 @@ class SO3 : public SO3Base> { /// internal quaternion representation of SO3 wrt the tangent vector /// SOPHUS_FUNC static Sophus::Matrix leftJacobian( - Tangent const& omega, optional const& theta_o = nullopt) { + Tangent const& omega, std::optional const& theta_o = std::nullopt) { using std::cos; using std::sin; using std::sqrt; @@ -571,7 +574,8 @@ class SO3 : public SO3Base> { } SOPHUS_FUNC static Sophus::Matrix leftJacobianInverse( - Tangent const& omega, optional const& theta_o = nullopt) { + Tangent const& omega, + std::optional const& theta_o = std::nullopt) { using std::cos; using std::sin; using std::sqrt; @@ -733,7 +737,7 @@ class SO3 : public SO3Base> { /// Returns closest SO3 given arbitrary 3x3 matrix. /// template - static SOPHUS_FUNC enable_if_t::value, SO3> + static SOPHUS_FUNC std::enable_if_t::value, SO3> fitToSO3(Transformation const& R) { return SO3(::Sophus::makeRotationMatrix(R)); } diff --git a/sophus/test_macros.hpp b/sophus/test_macros.hpp index c48d43a43..219a87838 100644 --- a/sophus/test_macros.hpp +++ b/sophus/test_macros.hpp @@ -19,7 +19,7 @@ class Pretty { }; template -class Pretty::value>> { +class Pretty::value>> { public: static std::string impl(Ptr ptr) { std::stringstream sstr; diff --git a/sophus/types.hpp b/sophus/types.hpp index da2f969f7..a4638f7ae 100644 --- a/sophus/types.hpp +++ b/sophus/types.hpp @@ -231,9 +231,9 @@ struct GetScalar> { /// If the Vector type is of fixed size, then IsFixedSizeVector::value will be /// true. template ::type> + Vector::ColsAtCompileTime == 1>> struct IsFixedSizeVector : std::true_type {}; /// Planes in 3d are hyperplanes. diff --git a/sophus/velocities.hpp b/sophus/velocities.hpp deleted file mode 100644 index 4c0d373de..000000000 --- a/sophus/velocities.hpp +++ /dev/null @@ -1,71 +0,0 @@ -#pragma once - -#include - -#include "num_diff.hpp" -#include "se3.hpp" - -namespace Sophus { -namespace experimental { -// Experimental since the API will certainly change drastically in the future. - -// Transforms velocity vector by rotation ``foo_R_bar``. -// -// Note: vel_bar can be either a linear or a rotational velocity vector. -// -template -Vector3 transformVelocity(SO3 const& foo_R_bar, - Vector3 const& vel_bar) { - // For rotational velocities note that: - // - // vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T) - // = vee(hat(Adj(foo_R_bar) * vel_bar)) - // = Adj(foo_R_bar) * vel_bar - // = foo_R_bar * vel_bar. - // - return foo_R_bar * vel_bar; -} - -// Transforms velocity vector by pose ``foo_T_bar``. -// -// Note: vel_bar can be either a linear or a rotational velocity vector. -// -template -Vector3 transformVelocity(SE3 const& foo_T_bar, - Vector3 const& vel_bar) { - return transformVelocity(foo_T_bar.so3(), vel_bar); -} - -// finite difference approximation of instantaneous velocity in frame foo -// -template -Vector3 finiteDifferenceRotationalVelocity( - std::function(Scalar)> const& foo_R_bar, Scalar t, - Scalar h = Constants::epsilon()) { - // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor - // - // W = dR(t)/dt * R^{-1}(t) - Matrix3 dR_dt_in_frame_foo = curveNumDiff( - [&foo_R_bar](Scalar t0) -> Matrix3 { - return foo_R_bar(t0).matrix(); - }, - t, h); - // velocity tensor - Matrix3 W_in_frame_foo = - dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix(); - return SO3::vee(W_in_frame_foo); -} - -// finite difference approximation of instantaneous velocity in frame foo -// -template -Vector3 finiteDifferenceRotationalVelocity( - std::function(Scalar)> const& foo_T_bar, Scalar t, - Scalar h = Constants::epsilon()) { - return finiteDifferenceRotationalVelocity( - [&foo_T_bar](Scalar t) -> SO3 { return foo_T_bar(t).so3(); }, t, - h); -} - -} // namespace experimental -} // namespace Sophus diff --git a/test/core/CMakeLists.txt b/test/core/CMakeLists.txt index f7381498b..b982736ec 100644 --- a/test/core/CMakeLists.txt +++ b/test/core/CMakeLists.txt @@ -10,9 +10,8 @@ SET( TEST_SOURCES test_common test_se3 test_rxso3 test_sim3 - test_velocities test_geometry) -find_package( Ceres 2.1.0 QUIET ) +find_package( Ceres 2 ) FOREACH(test_src ${TEST_SOURCES}) ADD_EXECUTABLE( ${test_src} ${test_src}.cpp tests.hpp ../../sophus/test_macros.hpp) diff --git a/test/core/test_common.cpp b/test/core/test_common.cpp index 435bc59c6..2625b56c7 100644 --- a/test/core/test_common.cpp +++ b/test/core/test_common.cpp @@ -25,9 +25,6 @@ bool testSmokeDetails() { Matrix expected_row(1, 7); SOPHUS_TEST_EQUAL(passed, row, expected_row, ""); - optional opt(nullopt); - SOPHUS_TEST(passed, !opt, ""); - return passed; } diff --git a/test/core/test_rxso2.cpp b/test/core/test_rxso2.cpp index 161a78ef1..19cc643ce 100644 --- a/test/core/test_rxso2.cpp +++ b/test/core/test_rxso2.cpp @@ -63,7 +63,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { bool passed = true; for (int i = 0; i < 10; ++i) { Matrix2 M = Matrix2::Random(); @@ -84,7 +84,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { return true; } diff --git a/test/core/test_rxso3.cpp b/test/core/test_rxso3.cpp index 942030000..37b376fc9 100644 --- a/test/core/test_rxso3.cpp +++ b/test/core/test_rxso3.cpp @@ -273,7 +273,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { bool passed = true; for (int i = 0; i < 10; ++i) { Matrix3 M = Matrix3::Random(); @@ -294,7 +294,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { return true; } diff --git a/test/core/test_se2.cpp b/test/core/test_se2.cpp index 55a12b2d5..c0e5d8f6a 100644 --- a/test/core/test_se2.cpp +++ b/test/core/test_se2.cpp @@ -217,7 +217,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { bool passed = true; for (int i = 0; i < 100; ++i) { Matrix3 T = Matrix3::Random(); @@ -231,7 +231,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { return true; } diff --git a/test/core/test_se3.cpp b/test/core/test_se3.cpp index 5dffc5599..4be39b812 100644 --- a/test/core/test_se3.cpp +++ b/test/core/test_se3.cpp @@ -215,7 +215,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { bool passed = true; for (int i = 0; i < 100; ++i) { @@ -239,7 +239,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { return true; } diff --git a/test/core/test_so2.cpp b/test/core/test_so2.cpp index 4f0835dda..9039c3a61 100644 --- a/test/core/test_so2.cpp +++ b/test/core/test_so2.cpp @@ -145,7 +145,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { bool passed = true; for (int i = 0; i < 100; ++i) { @@ -160,7 +160,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { return true; } diff --git a/test/core/test_so3.cpp b/test/core/test_so3.cpp index 98a034d45..4e303be39 100644 --- a/test/core/test_so3.cpp +++ b/test/core/test_so3.cpp @@ -180,7 +180,7 @@ class Tests { } template - enable_if_t::value, bool> + std::enable_if_t::value, bool> testSampleUniformSymmetry() { bool passed = true; std::default_random_engine generator(0); @@ -230,13 +230,13 @@ class Tests { } template - enable_if_t::value, bool> + std::enable_if_t::value, bool> testSampleUniformSymmetry() { return true; } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { bool passed = true; for (int i = 0; i < 100; ++i) { @@ -261,7 +261,7 @@ class Tests { } template - enable_if_t::value, bool> testFit() { + std::enable_if_t::value, bool> testFit() { return true; } diff --git a/test/core/test_velocities.cpp b/test/core/test_velocities.cpp deleted file mode 100644 index a46aabda3..000000000 --- a/test/core/test_velocities.cpp +++ /dev/null @@ -1,130 +0,0 @@ -#include - -#include "tests.hpp" - -#include - -namespace Sophus { -namespace experimental { - -template -bool tests_linear_velocities() { - bool passed = true; - std::vector, Eigen::aligned_allocator>> bar_Ts_baz; - - for (size_t i = 0; i < 10; ++i) { - bar_Ts_baz.push_back(SE3::rotX(i * 0.001) * - SE3::rotY(i * 0.001) * - SE3::transX(0.01 * i)); - } - - SE3 foo_T_bar = - SE3::rotX(0.5) * SE3::rotZ(0.2) * SE3::transY(2); - - std::vector, Eigen::aligned_allocator>> foo_Ts_baz; - for (auto const& bar_T_baz : bar_Ts_baz) { - foo_Ts_baz.push_back(foo_T_bar * bar_T_baz); - } - - auto gen_linear_vels = - [](std::vector, Eigen::aligned_allocator>> const& - a_Ts_b) { - std::vector, Eigen::aligned_allocator>> - linearVels_a; - for (size_t i = 0; i < a_Ts_b.size() - 1; ++i) { - linearVels_a.push_back(a_Ts_b[i + 1].translation() - - a_Ts_b[i].translation()); - } - return linearVels_a; - }; - - // linear velocities in frame bar - std::vector, Eigen::aligned_allocator>> - linearVels_bar = gen_linear_vels(bar_Ts_baz); - // linear velocities in frame foo - std::vector, Eigen::aligned_allocator>> - linearVels_foo = gen_linear_vels(foo_Ts_baz); - - for (size_t i = 0; i < linearVels_bar.size(); ++i) { - SOPHUS_TEST_APPROX(passed, linearVels_foo[i], - transformVelocity(foo_T_bar, linearVels_bar[i]), - sqrt(Constants::epsilon()), ""); - } - return passed; -} - -template -bool tests_rotational_velocities() { - bool passed = true; - - SE3 foo_T_bar = - SE3::rotX(0.5) * SE3::rotZ(0.2) * SE3::transY(2); - - // One parameter subgroup of SE3, motion through space given time t. - auto bar_T_baz = [](Scalar t) -> SE3 { - return SE3::rotX(t * Scalar(0.01)) * - SE3::rotY(t * Scalar(0.0001)) * - SE3::transX(t * Scalar(0.0001)); - }; - - std::vector ts = {Scalar(0), Scalar(0.3), Scalar(1)}; - - Scalar h = Constants::epsilon(); - for (Scalar t : ts) { - // finite difference approximiation of instantanious velocity in frame bar - Vector3 rotVel_in_frame_bar = - finiteDifferenceRotationalVelocity(bar_T_baz, t, h); - - // finite difference approximiation of instantanious velocity in frame foo - Vector3 rotVel_in_frame_foo = - finiteDifferenceRotationalVelocity( - [&foo_T_bar, bar_T_baz](Scalar t) -> SE3 { - return foo_T_bar * bar_T_baz(t); - }, - t, h); - - Vector3 rotVel_in_frame_bar2 = - transformVelocity(foo_T_bar.inverse(), rotVel_in_frame_foo); - SOPHUS_TEST_APPROX( - passed, rotVel_in_frame_bar, rotVel_in_frame_bar2, - // not too tight threshold, because of finit difference approximation - std::sqrt(Constants::epsilon()), ""); - - // The rotational velocities rotVel_in_frame_foo and rotVel_in_frame_bar - // should not be equal since they are in different frames (foo != bar). - SOPHUS_TEST_NOT_APPROX(passed, rotVel_in_frame_foo, rotVel_in_frame_bar, - Scalar(1e-3), ""); - - // Expect same result when using adjoint instead since: - // vee(bar_R_foo * hat(vel_foo) * bar_R_foo^T = bar_R_foo 8 vel_foo. - SOPHUS_TEST_APPROX( - passed, transformVelocity(foo_T_bar.inverse(), rotVel_in_frame_foo), - SO3::vee(foo_T_bar.so3().inverse().matrix() * - SO3::hat(rotVel_in_frame_foo) * - foo_T_bar.so3().matrix()), - Constants::epsilon(), ""); - } - return passed; -} - -int test_velocities() { - using std::cerr; - using std::endl; - - cerr << "Test Velocities" << endl << endl; - cerr << "Double tests: " << endl; - bool passed = tests_linear_velocities(); - passed &= tests_rotational_velocities(); - processTestResult(passed); - - cerr << "Float tests: " << endl; - passed = tests_linear_velocities(); - passed &= tests_rotational_velocities(); - processTestResult(passed); - - return 0; -} -} // namespace experimental -} // namespace Sophus - -int main() { return Sophus::experimental::test_velocities(); } diff --git a/test/core/tests.hpp b/test/core/tests.hpp index 7adad3ba1..2164e8620 100644 --- a/test/core/tests.hpp +++ b/test/core/tests.hpp @@ -86,7 +86,7 @@ class LieGroupTests { // For the time being, leftJacobian and leftJacobianInverse are only // implemented for SO3 and SE3 template - enable_if_t>::value || + std::enable_if_t>::value || std::is_same>::value, bool> leftJacobianTest() { @@ -119,7 +119,7 @@ class LieGroupTests { } template - enable_if_t>::value || + std::enable_if_t>::value || std::is_same>::value), bool> leftJacobianTest() { @@ -516,9 +516,9 @@ class LieGroupTests { // test average({A, B}) == interp(A, B): LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, 0.5); - optional foo_T_iaverage = iterativeMean( + std:: optional foo_T_iaverage = iterativeMean( std::array({{foo_T_bar, foo_T_baz}}), 20); - optional foo_T_average = + std:: optional foo_T_average = average(std::array({{foo_T_bar, foo_T_baz}})); SOPHUS_TEST(passed, bool(foo_T_average), "log(foo_T_bar): {}\nlog(foo_T_baz): {}", @@ -565,13 +565,13 @@ class LieGroupTests { } template - enable_if_t::value, bool> testSpline() { + std::enable_if_t::value, bool> testSpline() { // skip tests for Scalar == float return true; } template - enable_if_t::value, bool> testSpline() { + std::enable_if_t::value, bool> testSpline() { // run tests for Scalar != float bool passed = true; @@ -642,12 +642,12 @@ class LieGroupTests { } template - enable_if_t::value, bool> doAllTestsPass() { + std::enable_if_t::value, bool> doAllTestsPass() { return doesLargeTestSetPass(); } template - enable_if_t::value, bool> doAllTestsPass() { + std::enable_if_t::value, bool> doAllTestsPass() { return doesSmallTestSetPass(); }