diff --git a/.clang-tidy b/.clang-tidy index 91b2d817..6206a54c 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,17 +1,25 @@ Checks: '-*, readability-*, + -readability-identifier-length, + -readability-identifier-naming, -readability-magic-numbers, + -readability-function-cognitive-complexity, + -readability-redundant-member-init, + -readability-inconsistent-declaration-parameter-name, cppcoreguidelines-*, + -cppcoreguidelines-init-variables, + -cppcoreguidelines-explicit-virtual-functions, -cppcoreguidelines-macro-usage, -cppcoreguidelines-avoid-*, -cppcoreguidelines-narrowing-conversions, -cppcoreguidelines-pro-*, -cppcoreguidelines-slicing, -cppcoreguidelines-special-member-functions, + -cppcoreguidelines-prefer-member-initializer, -cppcoreguidelines-owning-memory, + -bugprone-easily-swappable-parameters, bugprone-dynamic-static-initializers, - bugprone-easily-swappable-parameters, bugprone-integer-division, bugprone-use-after-move, performance-noexcept-move-constructor, @@ -20,7 +28,6 @@ Checks: modernize-make-shared, modernize-use-bool-literals, modernize-use-default-member-init, - modernize-use-nodiscard, modernize-use-using' WarningsAsErrors: '*' HeaderFilterRegex: cpp/farm_ng/.* diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 00000000..6b6ac5f0 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,28 @@ +// For format details, see https://aka.ms/devcontainer.json. +// For config options, see https://github.com/devcontainers/templates/tree/main/src/ubuntu +{ + "name": "Ubuntu", + "image": "mcr.microsoft.com/devcontainers/base:jammy", + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools", + "ms-vscode.cpptools-extension-pack", + "twxs.cmake", + "ms-vscode.cmake-tools", + "ms-vscode.cpptools-themes" + ] + } + }, + + // Reference: https://containers.dev/features. + "features": { + "ghcr.io/devcontainers/features/python:1": {}, + "ghcr.io/rocker-org/devcontainer-features/apt-packages:1": { + "packages": "build-essential,ccache,clang,clang-tidy,vim" + } + }, + + // Runs commands after the container is created. + "postCreateCommand": "pip install -r .devcontainer/requirements.txt" +} diff --git a/.devcontainer/requirements.txt b/.devcontainer/requirements.txt new file mode 100644 index 00000000..c8d7813d --- /dev/null +++ b/.devcontainer/requirements.txt @@ -0,0 +1,2 @@ +cmake +ninja diff --git a/.gitignore b/.gitignore index ba652fbd..4ca66036 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ build +out venv .pytest_cache .eggs @@ -9,3 +10,6 @@ Cargo.lock docs/api_reference/html/ docs/api_reference/projects/ target/ + +# User-defined cmake presets +CMakeUserPresets.json diff --git a/.gitmodules b/.gitmodules index 96283b9e..b14575ea 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,9 @@ [submodule "third_party/eigen"] path = cpp/thirdparty/eigen url = https://github.com/eigen-mirror/eigen.git +[submodule "vcpkg"] + path = vcpkg + url = https://github.com/Microsoft/vcpkg.git +[submodule "examples/cpp-vcpkg/vcpkg"] + path = examples/cpp-vcpkg/vcpkg + url = https://github.com/Microsoft/vcpkg.git diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 554de7a8..2a59bb40 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -22,7 +22,7 @@ repos: rev: v1.1.13 hooks: - id: insert-license - files: cpp/sophus/.*\.(h|hpp|cpp|proto|ipp)$ + files: cpp/sophus2/.*\.(h|hpp|cpp|proto|ipp)$ exclude: (thirdparty|sympy/cpp_gencode)/.*$ args: - --comment-style @@ -34,7 +34,7 @@ repos: hooks: - id: insert-license files: \.(h|cpp|proto|ipp)$ - exclude: (thirdparty|cpp/sophus)/.*$ + exclude: (thirdparty|cpp/sophus2)/.*$ args: - --comment-style - // @@ -45,7 +45,7 @@ repos: hooks: - id: clang-format files: \.(h|cpp|ipp) - exclude: (cpp/sophus/sympy|thirdparty)/.*$ + exclude: (cpp/sophus2/sympy|thirdparty)/.*$ - repo: local hooks: - id: pragma-once @@ -86,7 +86,7 @@ repos: rev: v0.0.284 hooks: - id: ruff - exclude: (thirdparty|cpp/sophus)/.*$ + exclude: (thirdparty|cpp/sophus2)/.*$ args: [--fix, --exit-non-zero-on-fix] - repo: https://github.com/PyCQA/docformatter rev: v1.7.5 diff --git a/CMakeLists.txt b/CMakeLists.txt index 3dbbcda0..086a2bec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,18 +1,44 @@ -cmake_minimum_required(VERSION 3.16) -# https://semver.org/ +cmake_minimum_required(VERSION 3.24) + project(farm_ng_core VERSION 0.2.0) -set(farm_ng_cmake_DIR ${CMAKE_CURRENT_SOURCE_DIR}/cmake) -include(${farm_ng_cmake_DIR}/farm_ng_cmakeConfig.cmake) -farm_ng_module(farm_ng_core) +# Custom farm-ng macros +include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/farm_ng_cmakeConfig.cmake) -message("CMAKE_CXX_FLAGS" ${CMAKE_CXX_FLAGS}) +# Support local development package override +include(farm_ng_dev_packages) +# Support ccache +include(farm_ng_ccache) -farm_ng_enable_testing() +find_package(fmt REQUIRED) +find_package(GTest REQUIRED) +find_package(Eigen3 3.4.0 REQUIRED) +find_package(Protobuf REQUIRED) +find_package(gRPC REQUIRED) +find_package(CLI11 REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) -set(CMAKE_CXX_CLANG_TIDY "") +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED True) -find_package(fmt REQUIRED) +enable_testing() -add_subdirectory(protos) +include(GNUInstallDirs) +include(CMakePackageConfigHelpers) + +add_subdirectory(cmake) add_subdirectory(cpp) +add_subdirectory(protos) + +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake + COMPATIBILITY AnyNewerVersion +) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}-config.cmake + DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME} +) + +install(EXPORT ${PROJECT_NAME}Targets NAMESPACE farm_ng_core:: DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME}) diff --git a/CMakePresets.json b/CMakePresets.json new file mode 100644 index 00000000..2d05b337 --- /dev/null +++ b/CMakePresets.json @@ -0,0 +1,187 @@ +{ + "version": 3, + "cmakeMinimumRequired": { + "major": 3, + "minor": 16, + "patch": 0 + }, + "configurePresets": [ + { + "name": "common", + "hidden": true, + "description": "Common farm-ng configuration", + "generator": "Ninja", + "binaryDir": "${sourceDir}/out/build/${presetName}", + "installDir": "${sourceDir}/out/install/${presetName}", + "cacheVariables": { + "BUILD_SOPHUS_TESTS": "On", + "CMAKE_TOOLCHAIN_FILE": "${sourceDir}/vcpkg/scripts/buildsystems/vcpkg.cmake", + "CMAKE_CXX_FLAGS": "-Werror -Wall -Wextra -Wpedantic -Wno-unused-local-typedef -Wno-unused-local-typedef -Wno-gnu-zero-variadic-macro-arguments -Wno-unused-parameter -Wno-unused-variable -Wno-vla-extension", + "CMAKE_CXX_EXTENSIONS": "OFF", + "CMAKE_CXX_STANDARD": "20", + "CMAKE_CXX_STANDARD_REQUIRED": "ON", + "CMAKE_CXX_FLAGS_RELWITHDEBINFO": "-O2 -g", + "FARM_NG_PROVIDER_DEV_PACKAGES": "" + }, + "condition": { + "type": "inList", + "string": "${hostSystemName}", + "list": ["Linux", "Darwin"] + }, + "warnings": { + "dev": false, + "deprecated": true, + "uninitialized": true, + "unusedCli": true, + "systemVars": false + }, + "errors": { + "dev": false, + "deprecated": true + } + }, + { + "name": "gcc-debug", + "displayName": "GCC Debug", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "gcc", + "CMAKE_CXX_COMPILER": "g++", + "CMAKE_BUILD_TYPE": "Debug", + "CMAKE_CXX_FLAGS": "-fconcepts -Werror -Wall -Wextra -Wno-unused-parameter -Wno-unused-but-set-variable -Wno-unused-variable -Wno-unused-function -Wno-maybe-uninitialized", + "CMAKE_CXX_STANDARD": "17", + "CMAKE_CXX_EXTENSIONS": "ON" + } + }, + { + "name": "gcc-relwithdebinfo", + "displayName": "GCC RelWithDebInfo", + "inherits": "common", + "cacheVariables": { + "BUILD_SOPHUS_TESTS": "On", + "CMAKE_C_COMPILER": "gcc", + "CMAKE_CXX_COMPILER": "g++", + "CMAKE_BUILD_TYPE": "RelWithDebInfo", + "CMAKE_CXX_FLAGS": "-fconcepts -Werror -Wall -Wextra -Wno-unused-parameter -Wno-unused-but-set-variable -Wno-unused-variable -Wno-unused-function -Wno-maybe-uninitialized", + "CMAKE_CXX_STANDARD": "17", + "CMAKE_CXX_EXTENSIONS": "ON" + } + }, + { + "name": "clang-debug", + "displayName": "Clang Debug", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "clang", + "CMAKE_CXX_COMPILER": "clang++", + "CMAKE_BUILD_TYPE": "Debug" + } + }, + { + "name": "clang-relwithdebinfo", + "displayName": "Clang RelWithDebInfo", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "clang", + "CMAKE_CXX_COMPILER": "clang++", + "CMAKE_BUILD_TYPE": "RelWithDebInfo" + } + }, + { + "name": "clang-tidy", + "displayName": "Clang Tidy", + "hidden": true, + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "clang", + "CMAKE_CXX_COMPILER": "clang++", + "CMAKE_BUILD_TYPE": "Debug", + "CMAKE_CXX_CLANG_TIDY": "clang-tidy" + } + }, + { + "name": "asan", + "displayName": "AddressSanitizer+", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "clang", + "CMAKE_CXX_COMPILER": "clang++", + "CMAKE_BUILD_TYPE": "Asan", + "CMAKE_CXX_FLAGS_ASAN": "-O2 -g -fsanitize=address,bool,builtin,bounds,enum,float-cast-overflow,float-divide-by-zero,function,implicit-unsigned-integer-truncation,implicit-signed-integer-truncation,implicit-integer-sign-change,integer-divide-by-zero,leak,nonnull-attribute,null,nullability-arg,nullability-assign,nullability-return,object-size,pointer-overflow,return,returns-nonnull-attribute,shift,unsigned-shift-base,signed-integer-overflow,undefined,unreachable,unsigned-integer-overflow,vla-bound,vptr -fno-omit-frame-pointer" + } + }, + { + "name": "tsan", + "displayName": "ThreadSanitizer", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "clang", + "CMAKE_CXX_COMPILER": "clang++", + "CMAKE_BUILD_TYPE": "Tsan", + "CMAKE_CXX_FLAGS_TSAN": "-O2 -g -fsanitize=thread -fno-omit-frame-pointer" + } + }, + { + "name": "msan", + "displayName": "MemorySanitizer", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "clang", + "CMAKE_CXX_COMPILER": "clang++", + "CMAKE_BUILD_TYPE": "Msan", + "CMAKE_CXX_FLAGS_MSAN": "-O2 -g -fsanitize=memory -fno-omit-frame-pointer" + } + } + ], + "buildPresets": [ + { + "name": "gcc-debug", + "configurePreset": "gcc-debug" + }, + { + "name": "gcc-relwithdebinfo", + "configurePreset": "gcc-relwithdebinfo" + }, + { + "name": "clang-debug", + "configurePreset": "clang-debug" + }, + { + "name": "clang-relwithdebinfo", + "configurePreset": "clang-relwithdebinfo" + } + ], + "testPresets": [ + { + "name": "test-common", + "description": "Test CMake settings that apply to all configurations", + "hidden": true, + "output": { + "outputOnFailure": true + }, + "execution": { + "noTestsAction": "error" + } + }, + { + "name": "gcc-debug", + "inherits": "test-common", + "configurePreset": "gcc-debug" + }, + { + "name": "gcc-relwithdebinfo", + "inherits": "test-common", + "configurePreset": "gcc-relwithdebinfo" + }, + { + "name": "clang-debug", + "inherits": "test-common", + "configurePreset": "clang-debug" + }, + { + "name": "clang-relwithdebinfo", + "inherits": "test-common", + "configurePreset": "clang-relwithdebinfo" + } + ] +} diff --git a/README.md b/README.md index 6a6c879f..dd15d040 100644 --- a/README.md +++ b/README.md @@ -97,10 +97,10 @@ points North again. It wrapped around: `90 "+" 90 "+" 90 "+" 90 = 0`. // The following demonstrates the group multiplication of rotation matrices // Create rotation matrices from rotations around the x and y and z axes: - double const kPi = sophus::kPi; - sophus::Rotation3F64 R1 = sophus::Rotation3F64::fromRx(kPi / 4); - sophus::Rotation3F64 R2 = sophus::Rotation3F64::fromRy(kPi / 6); - sophus::Rotation3F64 R3 = sophus::Rotation3F64::fromRz(-kPi / 3); + double const kPi = sophus2::kPi; + sophus2::Rotation3F64 R1 = sophus2::Rotation3F64::fromRx(kPi / 4); + sophus2::Rotation3F64 R2 = sophus2::Rotation3F64::fromRy(kPi / 6); + sophus2::Rotation3F64 R3 = sophus2::Rotation3F64::fromRz(-kPi / 3); std::cout << "The rotation matrices are" << std::endl; std::cout << "R1:\n" << R1.matrix() << std::endl; @@ -137,15 +137,15 @@ points North again. It wrapped around: `90 "+" 90 "+" 90 "+" 90 = 0`. // Let use assume there is a camera in the world. First we describe its // orientation in the world reference frame. - sophus::Rotation3F64 world_from_camera_rotation = - sophus::Rotation3F64::fromRx(sophus::kPi / 4); + sophus2::Rotation3F64 world_from_camera_rotation = + sophus2::Rotation3F64::fromRx(sophus2::kPi / 4); // Then the position of the camera in the world. Eigen::Vector3d camera_in_world(0.0, 0.0, 1.0); // The pose (position and orientation) of the camera in the world is // constructed by its orientation ``world_from_camera_rotation`` as well as // its position ``camera_in_world``. - sophus::Isometry3F64 world_anchored_camera_pose( + sophus2::Isometry3F64 world_anchored_camera_pose( world_from_camera_rotation, camera_in_world); // SE(3) naturally representation is a 4x4 matrix which can be accessed using diff --git a/READMEv2.md b/READMEv2.md new file mode 100644 index 00000000..385d9d00 --- /dev/null +++ b/READMEv2.md @@ -0,0 +1,110 @@ +# Overview + +- Uses CMakePresets.json to define common build configurations as presets + + - Includes presets for clang/gcc, debug/release, clang-tidy asan, tsan, msan + - Persistent build directories per preset + - Great Vscode integration + - Developer can add CMakeUserPresets.json for additional configurations + +- Separate package definition from development environment + + - Modern cmake best practices encourage us to separate build/packaging requirements (used by downstream consumers) + from the development environment (used by developers) + - For example: + + - Our CMakeLists.txt should not specify where to find dependencies, only that they are found by `find_package` + - Our CMakeLists.txt should not specify build flags, only target requirements (e.g. `target_compile_features`) + +- Leverage vcpkg as a package manager + + - This setup does not require use of a package manager, but recommends it and uses it in CI + - Vcpkg has been easy to setup and the semantics feel minimal and complete + - To me, use of a package manager feels like the only way to address potential ODR violations. + You need a centralized place to reason about package versioning compatibility across the entire dependency tree. + - Eliminates the need for our custom C++ "virtualenv" setup + - Open-source contributors contribute patches that improve packaging of popular libraries (e.g. support for static linking) + +- Easily switch between development / released / system versions of packages + + - Switching to a development version relies on a few lines of code in `cmake/farm_ng_dev_packages.cmake` and one-line edits to `CMakePresets.json` and `vcpkg.json` + This seems to be working so far -- it is demonstrated here with sophus, Eigen, and CLI11. I also played with CMake's "Dependency Providers" feature which allows more customization if necessary. + - Switching to a system version requires a one-line edit to `vcpkg.json`, and then find_package() should resolve to a system installed version. + +- Move towards Sophus as separate project + + - For simplicity it is still vendored in here, but can easily be made a submodule or pulled in with vcpkg + +- Optional devcontainer.json + + - Dockerized development environment is optional but not required + - Better reproducibility for CI + - One-click Github Codespaces integration + +- Better cross-platform support + + - OSX should work, clear path to Windows + - Clear path to support cross-compilation + +- Begins to explore refactoring/cleanup of farm_ng module macros in `cmake/` + - There are some new Cmake APIs and recommended patterns we can take advantage of + +# Instructions + +```bash +# Open a terminal **without** the farm-ng venv activated +# Should print 'OK': +printenv CMAKE_PREFIX_PATH || echo "OK" + +# Ensure you have a reasonably up-to-date cmake (tested with 3.25) and ninja (tested with 1.10) +cmake --version +ninja --version + +# Update submodules +git submodule update --init --recursive + +# Bootstrap vcpkg +./vcpkg/bootstrap-vcpkg.sh + +# Configure +# Note: Builds dependencies from source on first invocation, may take 5-10mins +cmake --preset clang-debug + +# Build +cmake --build --preset clang-debug + +# Test (one test fails on my machine) +ctest --preset clang-debug + +# Install +mkdir /tmp/farm-ng-prefix +cmake --build --preset clang-debug --target install -DCMAKE_PREFIX_PATH=/tmp/farm-ng-prefix + +# Try the same workflows in Vscode +# Select a configure preset such as "Clang RelWithDebInfo", then build and run as usual +code farm_ng_core.code-workspace + +# Optionally, you can try the devcontainer workflow in Vscode by clicking "Reopen in Container" when prompted +# - Note, this will not re-use the same vcpkg cache, so the first build will take more time +# - Note, if you select a configure preset in the devcontainer that you've already built on your host machine you will need to "CMake: Delete Cache and Reconfigure" +``` + +# FAQ + +**Q: How does this work for repositories downstream of farm-ng-core? What about consuming closed-source packages?** + +A: Largely the same. It will help if we define a vcpkg "port" for each farm-ng C++ package ([an example](https://github.com/northwindtraders/vcpkg-registry/blob/main/ports/beicode/portfile.cmake)). We can then host these ports in a farm-ng vcpkg registry, which is just a (optionally private) Github repository ([an example](https://github.com/northwindtraders/vcpkg-registry/tree/main)). I have not tried this yet. + +**Q: Is building dependencies from source too slow?** + +A: So far, in this repository, no. But it does not rely on Ceres and OpenCV. And we won't want to rebuild on every run in CI. Vcpkg's support for [binary caching](https://learn.microsoft.com/en-us/vcpkg/users/binarycaching) is the next obvious optimization. + +# TODO + +- Define and (re-)implement package options to enable/disable testing, protos, etc. +- Finish refactor of farm_ng macros in `cmake/` +- Update and validate updated package `install` logic +- Specify desired third-party dependency versions in vcpkg.json +- Validate advanced packaging features: shared and static library support, etc. +- Add CI, including an integration test of a simple "consuming" project +- Add CMake linting if valuable diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt new file mode 100644 index 00000000..c1389dd9 --- /dev/null +++ b/cmake/CMakeLists.txt @@ -0,0 +1,17 @@ +# Install CMake modules so they can reused downstream +# Reference: +# "Modern CMake Modules - Bret Brown - CppCon 2021" +# https://www.youtube.com/watch?v=IZXNsim9TWI + +install( + FILES + farm_ng_add_library.cmake + farm_ng_add_protobufs.cmake + farm_ng_add_test.cmake + farm_ng_ccache.cmake + farm_ng_dev_packages.cmake + farm_ng_macos_ssl_fix.cmake + farm_ng_module.cmake + DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME} + COMPONENT cmake-farm-ng-core +) diff --git a/cmake/farm_ng_add_library.cmake b/cmake/farm_ng_add_library.cmake index 6aeb3aaa..135fbe1d 100644 --- a/cmake/farm_ng_add_library.cmake +++ b/cmake/farm_ng_add_library.cmake @@ -47,21 +47,21 @@ macro(farm_ng_add_library target) "$" "$") - if(DEFINED FARM_NG_ARGS_SOURCES) - foreach ( file ${FARM_NG_ARGS_HEADERS} ) + foreach ( file ${FARM_NG_ARGS_HEADERS} ) + if(IS_ABSOLUTE ${file}) + file(RELATIVE_PATH rel ${CMAKE_SOURCE_DIR}/${abs_include} ${file}) + else() + file(RELATIVE_PATH rel ${CMAKE_SOURCE_DIR}/${abs_include} ${CMAKE_CURRENT_SOURCE_DIR}/${file}) + endif() - if(IS_ABSOLUTE ${file}) - file(RELATIVE_PATH rel ${CMAKE_SOURCE_DIR}/${abs_include} ${file}) - else() - file(RELATIVE_PATH rel ${CMAKE_SOURCE_DIR}/${abs_include} ${CMAKE_CURRENT_SOURCE_DIR}/${file}) - endif() + get_filename_component( dir ${rel} DIRECTORY ) + install( FILES ${file} + DESTINATION include/${dir} + COMPONENT Devel) + endforeach() - get_filename_component( dir ${rel} DIRECTORY ) - install( FILES ${file} - DESTINATION include/${dir} - COMPONENT Devel) - endforeach() + if(DEFINED FARM_NG_ARGS_SOURCES) string(REPLACE "." ";" VERSION_LIST ${PROJECT_VERSION}) list(GET VERSION_LIST 0 VERSION_MAJOR) list(GET VERSION_LIST 1 VERSION_MINOR) @@ -82,4 +82,6 @@ macro(farm_ng_add_library target) COMPONENT Libs ) + message(VERBOSE "Added library ${target}") + endmacro() diff --git a/cmake/farm_ng_add_protobufs.cmake b/cmake/farm_ng_add_protobufs.cmake index 0e337d33..10b2f64e 100644 --- a/cmake/farm_ng_add_protobufs.cmake +++ b/cmake/farm_ng_add_protobufs.cmake @@ -63,6 +63,7 @@ macro(farm_ng_add_protobufs target) SOURCES ${FARM_NG_ADD_PROTOBUFS_PROTO_FILES} ) + message(VERBOSE "Added proto library ${target}.\n - INCLUDE_DIR: ${_proto_output_dir_cpp}") target_link_libraries(${target} PUBLIC protobuf::libprotobuf gRPC::grpc gRPC::grpc++ diff --git a/cmake/farm_ng_ccache.cmake b/cmake/farm_ng_ccache.cmake new file mode 100644 index 00000000..622d0063 --- /dev/null +++ b/cmake/farm_ng_ccache.cmake @@ -0,0 +1,8 @@ +find_program(CCACHE_FOUND ccache) +if(CCACHE_FOUND) + message("Enabling ccache") + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) +else() + message("Ccache not found.") +endif(CCACHE_FOUND) diff --git a/cmake/farm_ng_core-config.cmake b/cmake/farm_ng_core-config.cmake new file mode 100644 index 00000000..0128f67a --- /dev/null +++ b/cmake/farm_ng_core-config.cmake @@ -0,0 +1,9 @@ +include(CMakeFindDependencyMacro) + +find_dependency(fmt REQUIRED) +find_dependency(Eigen3 3.4.0 REQUIRED) +find_dependency(Protobuf REQUIRED) +find_dependency(gRPC REQUIRED) +find_dependency(Boost REQUIRED COMPONENTS system) + +include(${CMAKE_CURRENT_LIST_DIR}/farm_ng_coreTargets.cmake) diff --git a/cmake/farm_ng_dev_packages.cmake b/cmake/farm_ng_dev_packages.cmake new file mode 100644 index 00000000..4ee631e2 --- /dev/null +++ b/cmake/farm_ng_dev_packages.cmake @@ -0,0 +1,17 @@ +set(FARM_NG_PROVIDER_DEV_PACKAGES "" CACHE STRING "Packages to use local development versions of") + +include(FetchContent) + +if(FARM_NG_PROVIDER_DEV_PACKAGES) + message("Local development versions enabled") +elseif() + message("No local development versions enabled") +endif() +foreach(package_name IN LISTS FARM_NG_PROVIDER_DEV_PACKAGES) + message(" - Using local development version of ${package_name} at ${CMAKE_SOURCE_DIR}/external/${package_name}") + FetchContent_Declare( + ${package_name} + SOURCE_DIR "${CMAKE_SOURCE_DIR}/external/${package_name}" + OVERRIDE_FIND_PACKAGE + ) +endforeach() diff --git a/cmake/farm_ng_module.cmake b/cmake/farm_ng_module.cmake index a9361f4b..cda1e0c0 100644 --- a/cmake/farm_ng_module.cmake +++ b/cmake/farm_ng_module.cmake @@ -5,57 +5,57 @@ include(CMakePackageConfigHelpers) macro(farm_ng_module) - message(STATUS "farm_ng_module ${PROJECT_NAME}") - # Configure CCache if available - find_program(CCACHE_FOUND ccache) - if(CCACHE_FOUND) - set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) - set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) - endif(CCACHE_FOUND) + # message(STATUS "farm_ng_module ${PROJECT_NAME}") + # # Configure CCache if available + # find_program(CCACHE_FOUND ccache) + # if(CCACHE_FOUND) + # set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + # set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) + # endif(CCACHE_FOUND) - include(CMakeToolsHelpers OPTIONAL) + # include(CMakeToolsHelpers OPTIONAL) # No NDEBUG for RelWithDebInfo. Among others, this makes sure that essential Eigen # runtime asserts are enabled. - set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O2 -g") + # set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O2 -g") - set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH ON) - set(BUILD_SHARED_LIBS ON) - set(CMAKE_CXX_STANDARD_REQUIRED ON) + # set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH ON) + # set(BUILD_SHARED_LIBS ON) + # set(CMAKE_CXX_STANDARD_REQUIRED ON) # Coarsely, the c++ standard used is c++17 plus concepts from c++20. - if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang") - # For clang, c++20 is enabled, but only concepts shall be used from the features newly added. - set(CMAKE_CXX_STANDARD 20) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Wall -Wextra -Wsign-compare -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-local-typedef -Wreorder-ctor -Wreorder-init-list") - elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fconcepts") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Wall -Wextra -Wno-unused-parameter -Wno-unused-but-set-variable -Wno-unused-variable -Wno-unused-function -Wno-maybe-uninitialized") - endif() - - if(BUILD_ASAN) - if (NOT "${CMAKE_BUILD_TYPE}" STREQUAL "Debug") - message(FATAL_ERROR "need to build with Debug when using asan: " ${CMAKE_BUILD_TYPE}) - endif() - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} \ - -fsanitize=address \ - -fsanitize=bool \ - -fsanitize=bounds \ - -fsanitize=enum \ - -fsanitize=float-cast-overflow \ - -fsanitize=float-divide-by-zero \ - -fsanitize=nonnull-attribute \ - -fsanitize=returns-nonnull-attribute \ - -fsanitize=signed-integer-overflow \ - -fsanitize=undefined \ - -fsanitize=vla-bound \ - -fno-sanitize=alignment \ - -fsanitize=leak \ - ") - endif() - - set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + # if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang") + # # For clang, c++20 is enabled, but only concepts shall be used from the features newly added. + # set(CMAKE_CXX_STANDARD 20) + # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Wall -Wextra -Wsign-compare -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-local-typedef -Wreorder-ctor -Wreorder-init-list") + # elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") + # set(CMAKE_CXX_STANDARD 17) + # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fconcepts") + # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Wall -Wextra -Wno-unused-parameter -Wno-unused-but-set-variable -Wno-unused-variable -Wno-unused-function -Wno-maybe-uninitialized") + # endif() + + # if(BUILD_ASAN) + # if (NOT "${CMAKE_BUILD_TYPE}" STREQUAL "Debug") + # message(FATAL_ERROR "need to build with Debug when using asan: " ${CMAKE_BUILD_TYPE}) + # endif() + # set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} \ + # -fsanitize=address \ + # -fsanitize=bool \ + # -fsanitize=bounds \ + # -fsanitize=enum \ + # -fsanitize=float-cast-overflow \ + # -fsanitize=float-divide-by-zero \ + # -fsanitize=nonnull-attribute \ + # -fsanitize=returns-nonnull-attribute \ + # -fsanitize=signed-integer-overflow \ + # -fsanitize=undefined \ + # -fsanitize=vla-bound \ + # -fno-sanitize=alignment \ + # -fsanitize=leak \ + # ") + # endif() + + # set(CMAKE_EXPORT_COMPILE_COMMANDS ON) endmacro() diff --git a/cmake/vcpkg/ports/default/farm-ng-core/portfile.cmake b/cmake/vcpkg/ports/default/farm-ng-core/portfile.cmake new file mode 100644 index 00000000..55ee4251 --- /dev/null +++ b/cmake/vcpkg/ports/default/farm-ng-core/portfile.cmake @@ -0,0 +1,31 @@ +vcpkg_from_github( + OUT_SOURCE_PATH SOURCE_PATH + REPO farm-ng/farm-ng-core + REF "${VERSION}" + SHA512 0 + HEAD_REF main +) + +# TODO: Should be able to clean this up +set(VCPKG_C_FLAGS "${VCPKG_C_FLAGS}") +set(VCPKG_CXX_FLAGS "-fconcepts ${VCPKG_CXX_FLAGS}") + +vcpkg_cmake_configure( + SOURCE_PATH "${SOURCE_PATH}" + OPTIONS + "-DBUILD_SOPHUS_TESTS=Off" + "-DFARM_NG_PROVIDER_DEV_PACKAGES=Sophus" + "-DCMAKE_CXX_STANDARD=17" # TODO: Should be able to remove + "-DCMAKE_CXX_EXTENSIONS=On" # TODO: Should be able to remove +) + +vcpkg_cmake_install() + +vcpkg_cmake_config_fixup(PACKAGE_NAME "farm_ng_core") + +file(REMOVE_RECURSE "${CURRENT_PACKAGES_DIR}/debug/include") + +file(INSTALL "${SOURCE_PATH}/LICENSE" DESTINATION "${CURRENT_PACKAGES_DIR}/share/${PORT}" RENAME copyright) + +# TODO: Re-enable +# configure_file("${CMAKE_CURRENT_LIST_DIR}/usage" "${CURRENT_PACKAGES_DIR}/share/${PORT}/usage" COPYONLY) diff --git a/cmake/vcpkg/ports/default/farm-ng-core/vcpkg.json b/cmake/vcpkg/ports/default/farm-ng-core/vcpkg.json new file mode 120000 index 00000000..fd63aa64 --- /dev/null +++ b/cmake/vcpkg/ports/default/farm-ng-core/vcpkg.json @@ -0,0 +1 @@ +../../../../../vcpkg.json \ No newline at end of file diff --git a/cmake/vcpkg/ports/deps-only/farm-ng-core/portfile.cmake b/cmake/vcpkg/ports/deps-only/farm-ng-core/portfile.cmake new file mode 100644 index 00000000..065116c2 --- /dev/null +++ b/cmake/vcpkg/ports/deps-only/farm-ng-core/portfile.cmake @@ -0,0 +1 @@ +set(VCPKG_POLICY_EMPTY_PACKAGE enabled) diff --git a/cmake/vcpkg/ports/deps-only/farm-ng-core/vcpkg.json b/cmake/vcpkg/ports/deps-only/farm-ng-core/vcpkg.json new file mode 120000 index 00000000..fd63aa64 --- /dev/null +++ b/cmake/vcpkg/ports/deps-only/farm-ng-core/vcpkg.json @@ -0,0 +1 @@ +../../../../../vcpkg.json \ No newline at end of file diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index 7524e407..5930c67e 100644 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -1,4 +1,4 @@ add_subdirectory(thirdparty/farm_pp) add_subdirectory(farm_ng/core) -add_subdirectory(sophus) +add_subdirectory(sophus2) diff --git a/cpp/farm_ng/core/CMakeLists.txt b/cpp/farm_ng/core/CMakeLists.txt index 33eefa55..544450d5 100644 --- a/cpp/farm_ng/core/CMakeLists.txt +++ b/cpp/farm_ng/core/CMakeLists.txt @@ -1,14 +1,8 @@ -#[[ -core -]] - add_subdirectory(logging) add_subdirectory(enum) add_subdirectory(struct) add_subdirectory(misc) -if(BUILD_FARM_NG_PROTOS) - add_subdirectory(prototools) - add_subdirectory(proto_conv) - add_subdirectory(pipeline) - add_subdirectory(plotting) -endif() +add_subdirectory(prototools) +add_subdirectory(proto_conv) +add_subdirectory(pipeline) +add_subdirectory(plotting) diff --git a/cpp/farm_ng/core/enum/enum.h b/cpp/farm_ng/core/enum/enum.h index dcf6ce84..96ce5cb5 100644 --- a/cpp/farm_ng/core/enum/enum.h +++ b/cpp/farm_ng/core/enum/enum.h @@ -25,26 +25,32 @@ #include "farm_ng/core/enum/enum_without_iostream.h" +#include + #include #include /// Adds ostream overloads for the enum. -#define FARM_ENUM_IOSTREAM_OVERLOAD(EnumName) \ - namespace enum_wrapper_ { \ - inline auto operator<<(std::ostream &os, EnumName##Impl value) \ - -> std::ostream & { \ - os << toPretty(value); \ - return os; \ - } \ - inline auto operator>>(std::istream &is, EnumName##Impl &value) \ - -> std::istream & { \ - std::string str; \ - is >> str; \ - if (!trySetFromString(value, str)) { \ - throw std::runtime_error(std::string("Bad Value: ") + str); \ - } \ - return is; \ - } \ +#define FARM_ENUM_IOSTREAM_OVERLOAD(EnumName) \ + namespace enum_wrapper_ { \ + [[maybe_unused]] inline auto operator<<( \ + std::ostream &os, EnumName##Impl value) -> std::ostream & { \ + os << toPretty(value); \ + return os; \ + } \ + [[maybe_unused]] inline auto operator>>( \ + std::istream &is, EnumName##Impl &value) -> std::istream & { \ + std::string str; \ + is >> str; \ + if (!trySetFromString(value, str)) { \ + throw std::runtime_error(std::string("Bad Value: ") + str); \ + } \ + return is; \ + } \ + \ + [[maybe_unused]] inline auto format_as(EnumName##Impl value) { \ + return toPretty(value); \ + } \ } // namespace enum_wrapper_ /// Convenience marco which defines the enum plus alias and adds the ostream diff --git a/cpp/farm_ng/core/logging/CMakeLists.txt b/cpp/farm_ng/core/logging/CMakeLists.txt index bcd87ea9..e9afc25d 100644 --- a/cpp/farm_ng/core/logging/CMakeLists.txt +++ b/cpp/farm_ng/core/logging/CMakeLists.txt @@ -13,6 +13,10 @@ farm_ng_add_library(farm_ng_core_logging HEADERS assert_within.h expected.h + fmt_ceres.h + fmt_eigen.h + fmt_grpc.h + fmt_ptr.h format.h trace_debug_log.h logger.h diff --git a/cpp/farm_ng/core/logging/assert_within.h b/cpp/farm_ng/core/logging/assert_within.h index 8bcae6a6..41476e48 100644 --- a/cpp/farm_ng/core/logging/assert_within.h +++ b/cpp/farm_ng/core/logging/assert_within.h @@ -15,6 +15,7 @@ #pragma once #include "farm_ng/core/logging/expected.h" +#include "sophus2/linalg/batch.h" #include @@ -144,6 +145,74 @@ struct CheckNear< } }; +template +struct CheckNear< + Eigen::Matrix, + Eigen::Matrix, + std::enable_if_t::kIsBatch>> { + static Expected check( + bool check_relative, + Eigen::Matrix const& lhs, + char const* lhs_cstr, + Eigen::Matrix const& rhs, + char const* rhs_cstr, + double const& thr) { + double max_error = 0.0; + int max_row_idx = -1; + int max_col_idx = -1; + int max_batch_idx = -1; + + auto nearness_fn = + check_relative + ? [](double x, double y) { return relativeCloseness(x, y); } + : [](double x, double y) { return std::abs(x - y); }; + + for (int c = 0; c < lhs.cols(); ++c) { + for (int r = 0; r < lhs.rows(); ++r) { + for (int batch = 0; r < sophus2::BatchTrait::kNumSingeScalars; + ++r) { + double err = nearness_fn(lhs(r, c).b[batch], rhs(r, c).b[batch]); + + if (!(err <= max_error)) { + // inverted comparison, so we also end up here if err or max_error + // are NAN + max_error = err; + max_row_idx = r; + max_col_idx = c; + max_batch_idx = batch; + } + } + } + } + if (max_error < thr) { + // all errors below threshold + return Success{}; + } + return FARM_UNEXPECTED( + "Not true: {}[{}, {}; {}] near {}[{}, {}; {}]; has error of {} (thr: {})\n" + "LHS `{}` is:\n" + "{}\n\n" + "RHS `{}` is:\n" + "{}\n\n", + lhs_cstr, + max_row_idx, + max_col_idx, + max_batch_idx, + rhs_cstr, + max_row_idx, + max_col_idx, + max_batch_idx, + max_error, + thr, + lhs_cstr, + lhs, + rhs_cstr, + rhs); + + return Success{}; + } +}; + template inline Expected checkAbsNear( TScalar lhs, @@ -164,8 +233,8 @@ inline Expected checkRelativeNear( double thr) { if (!((thr >= 0.0) && (thr <= 1.0))) { FARM_WARN( - "The threshold of the WITHIN_REL macro shall be in [0.0, 1.0], but we " - "got `{}`.", + "The threshold of the WITHIN_REL macro shall be in [0.0, 1.0], but " + "we got `{}`.", thr); } return CheckNear::check( diff --git a/cpp/farm_ng/core/logging/expected.h b/cpp/farm_ng/core/logging/expected.h index 6ac7c772..79ac7417 100644 --- a/cpp/farm_ng/core/logging/expected.h +++ b/cpp/farm_ng/core/logging/expected.h @@ -16,6 +16,7 @@ #include "farm_ng/core/logging/logger.h" +#include #include #include @@ -121,3 +122,12 @@ Expected fromOptional(std::optional optional) { } } // namespace farm_ng + +namespace fmt { + +template +requires std::is_base_of_v +struct formatter : ostream_formatter { +}; + +} // namespace fmt diff --git a/cpp/farm_ng/core/logging/fmt_boost.h b/cpp/farm_ng/core/logging/fmt_boost.h new file mode 100644 index 00000000..66ab3e50 --- /dev/null +++ b/cpp/farm_ng/core/logging/fmt_boost.h @@ -0,0 +1,25 @@ +// Copyright 2022, farm-ng inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +template <> +struct fmt::formatter : ostream_formatter {}; + +template <> +struct fmt::formatter : ostream_formatter {}; diff --git a/cpp/farm_ng/core/logging/fmt_ceres.h b/cpp/farm_ng/core/logging/fmt_ceres.h new file mode 100644 index 00000000..6371f755 --- /dev/null +++ b/cpp/farm_ng/core/logging/fmt_ceres.h @@ -0,0 +1,21 @@ +// Copyright 2022, farm-ng inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +template +struct fmt::formatter> : ostream_formatter {}; diff --git a/cpp/farm_ng/core/logging/fmt_eigen.h b/cpp/farm_ng/core/logging/fmt_eigen.h new file mode 100644 index 00000000..d3d72914 --- /dev/null +++ b/cpp/farm_ng/core/logging/fmt_eigen.h @@ -0,0 +1,32 @@ +// Copyright 2022, farm-ng inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +// Defined in fmt namespace due to gcc bug +// https://stackoverflow.com/a/69144223 +namespace fmt { + +template +requires std::is_base_of_v, T> +struct formatter : ostream_formatter { +}; + +template +struct formatter> : ostream_formatter {}; + +} // namespace fmt diff --git a/cpp/farm_ng/core/logging/fmt_grpc.h b/cpp/farm_ng/core/logging/fmt_grpc.h new file mode 100644 index 00000000..006cad42 --- /dev/null +++ b/cpp/farm_ng/core/logging/fmt_grpc.h @@ -0,0 +1,21 @@ +// Copyright 2022, farm-ng inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +template <> +struct fmt::formatter : ostream_formatter {}; diff --git a/cpp/farm_ng/core/logging/fmt_ptr.h b/cpp/farm_ng/core/logging/fmt_ptr.h new file mode 100644 index 00000000..f4613868 --- /dev/null +++ b/cpp/farm_ng/core/logging/fmt_ptr.h @@ -0,0 +1,27 @@ +// Copyright 2022, farm-ng inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include + +template +struct fmt::formatter> { + auto constexpr parse(format_parse_context& ctx) { return ctx.begin(); } + auto format(std::shared_ptr const& p, fmt::format_context& ctx) const { + return fmt::format_to(ctx.out(), "{}", fmt::ptr(p)); + } +}; diff --git a/cpp/farm_ng/core/logging/format.h b/cpp/farm_ng/core/logging/format.h index 1f819bb7..20aebf0a 100644 --- a/cpp/farm_ng/core/logging/format.h +++ b/cpp/farm_ng/core/logging/format.h @@ -15,12 +15,14 @@ #pragma once #include +#include #include #include #include #include #include #include +#include #include #include diff --git a/cpp/farm_ng/core/logging/logger.cpp b/cpp/farm_ng/core/logging/logger.cpp index 16f455ab..9f0ba72d 100644 --- a/cpp/farm_ng/core/logging/logger.cpp +++ b/cpp/farm_ng/core/logging/logger.cpp @@ -42,6 +42,9 @@ void StreamLogger::writeHeader( std::string const& file, int line, std::string const& function) { + auto filename_idx = file.rfind("/", std::string::npos); + auto parent_idx = file.rfind("/", filename_idx - 1); + write( disk_logging_, FARM_FORMAT( @@ -51,6 +54,8 @@ void StreamLogger::writeHeader( fmt::arg("text", header_text), fmt::arg("file", file), fmt::arg("line", line), + fmt::arg("fileparent", file.substr(parent_idx + 1)), + fmt::arg("filename", file.substr(filename_idx + 1)), fmt::arg("function", function))); } @@ -115,7 +120,7 @@ void StreamLogger::flush(DiskLogging& disk_logging) { } std::string const StreamLogger::kDefaultHeaderFormat = - "[FARM {text} in {file}:{line}]\n"; + "[FARM {text} in {fileparent}:{line}]\n"; StreamLogger::LogClock const StreamLogger::kDefaultLogClock = StreamLogger::LogClock{.now = []() { diff --git a/cpp/farm_ng/core/logging/logger.h b/cpp/farm_ng/core/logging/logger.h index 0d7d0761..82ad3bf8 100644 --- a/cpp/farm_ng/core/logging/logger.h +++ b/cpp/farm_ng/core/logging/logger.h @@ -296,6 +296,24 @@ inline StreamLogger& defaultLogger() { } \ } while (false) +/// LOG WARN: Only log every nth encounter. +#define FARM_WARN_EVERY_N(N, ...) \ + do { \ + static std::atomic counter(0); \ + ++counter; \ + if (counter > (N)) { \ + counter -= (N); \ + } \ + if (counter == 1) { \ + farm_ng::defaultLogger().log( \ + farm_ng::LogLevel::warning, \ + FARM_FORMAT("LOG WARN EVERY N( = {} )", #N), \ + __FILE__, \ + __LINE__, \ + __func__ FARM_MAYBE_VARGS(__VA_ARGS__)(__VA_ARGS__)); \ + } \ + } while (false) + /// If condition is false, Print formatted error message and then panic. #define FARM_ASSERT(condition, ...) \ do { \ diff --git a/cpp/farm_ng/core/misc/CMakeLists.txt b/cpp/farm_ng/core/misc/CMakeLists.txt index 8602fd7a..a50d0aa1 100644 --- a/cpp/farm_ng/core/misc/CMakeLists.txt +++ b/cpp/farm_ng/core/misc/CMakeLists.txt @@ -32,7 +32,7 @@ farm_ng_add_library(farm_ng_core_misc target_link_libraries(farm_ng_core_misc PUBLIC farm_ng_core_enum farm_ng_core_logging - Sophus::sophus_linalg + farm_ng_core::sophus2_linalg ) foreach(test_basename diff --git a/cpp/farm_ng/core/misc/conversions.h b/cpp/farm_ng/core/misc/conversions.h index 14dbb8ce..89b0e848 100644 --- a/cpp/farm_ng/core/misc/conversions.h +++ b/cpp/farm_ng/core/misc/conversions.h @@ -16,60 +16,65 @@ #include #include -#include -#include +#include +#include namespace farm_ng { /// Converts meters [m] to inches [in] -template +template T inchesFromMeters(T m) { return m / 0.0254f; } /// Converts inches [in] to meters [m] -template +template T metersFromInches(T inches) { return 0.0254 * inches; } /// Converts feet [ft] to meters [m] -template +template T metersFromFeet(T feet) { return metersFromInches(12.0 * feet); } /// Converts meters [m] to feet [ft] -template +template T feetFromMeters(T meters) { return inchesFromMeters(meters) / 12.0; } /// Converts radians to degrees -template +template T degFromRad(T rad) { return T(180.0) / M_PI * rad; } +template +auto degFromRad(TPoint const& v_rad) { + return degFromRad(typename TPoint::Scalar(1.0)) * v_rad; +} + /// Converts degrees to radians -template +template T radFromDeg(T deg) { return M_PI / T(180.0) * deg; } -template +template auto radFromDeg(TPoint const& v_deg) { return radFromDeg(typename TPoint::Scalar(1.0)) * v_deg; } /// Converts radians to arc minutes (60th of a degree) -template +template T arcMinuteFromRad(T rad) { return T(60.) * degFromRad(rad); } /// Converts arc minutes (60th of a degree) to radians -template +template T radFromArcMinute(T arc_minute) { return radFromDeg(arc_minute / T(60.)); } diff --git a/cpp/farm_ng/core/misc/conversions_test.cpp b/cpp/farm_ng/core/misc/conversions_test.cpp index af6971cf..7abbb48d 100644 --- a/cpp/farm_ng/core/misc/conversions_test.cpp +++ b/cpp/farm_ng/core/misc/conversions_test.cpp @@ -35,6 +35,10 @@ TEST(conversions, unit) { FARM_ASSERT_WITHIN_REL(degFromRad(M_PI), 180.0, 1e-3); FARM_ASSERT_WITHIN_REL(radFromDeg(180.0), M_PI, 1e-3); + using Vec2d = Eigen::Vector2d; + FARM_ASSERT(degFromRad(Vec2d{M_PI, M_PI}).isApprox(Vec2d{180.0, 180.0})); + FARM_ASSERT(radFromDeg(Vec2d{180.0, 180.0}).isApprox(Vec2d{M_PI, M_PI})); + FARM_ASSERT_WITHIN_REL(radFromArcMinute(1.0), 0.000290888, 1e-3); FARM_ASSERT_WITHIN_REL(arcMinuteFromRad(1.0), 3437.75, 1e-3); } diff --git a/cpp/farm_ng/core/misc/shared.h b/cpp/farm_ng/core/misc/shared.h index a1c6718f..302dac87 100644 --- a/cpp/farm_ng/core/misc/shared.h +++ b/cpp/farm_ng/core/misc/shared.h @@ -15,6 +15,7 @@ #pragma once #include "farm_ng/core/logging/expected.h" +#include "farm_ng/core/logging/fmt_ptr.h" #include "farm_ng/core/misc/concept_utils.h" #include diff --git a/cpp/farm_ng/core/misc/time_series.h b/cpp/farm_ng/core/misc/time_series.h index 05c557c8..cea4da9c 100644 --- a/cpp/farm_ng/core/misc/time_series.h +++ b/cpp/farm_ng/core/misc/time_series.h @@ -15,7 +15,7 @@ #pragma once #include -#include +#include #include @@ -36,7 +36,7 @@ auto getStamp(TType const& value) -> double { /// StampedValue is a type that has a timestamp. template concept StampedValue = requires(TType m) { - { getStamp(m) } -> sophus::concepts::ConvertibleTo; + { getStamp(m) } -> sophus2::concepts::ConvertibleTo; }; /// Interpolate between two values. @@ -49,7 +49,7 @@ auto interpolate( template concept InterpolativeValue = StampedValue && requires(TType m, double p) { - { interpolate(m, m, p) } -> sophus::concepts::ConvertibleTo; + { interpolate(m, m, p) } -> sophus2::concepts::ConvertibleTo; }; /// New type to specify maximal allowed time gap for interpolation. diff --git a/cpp/farm_ng/core/misc/time_series_test.cpp b/cpp/farm_ng/core/misc/time_series_test.cpp index 12d782e2..ab860732 100644 --- a/cpp/farm_ng/core/misc/time_series_test.cpp +++ b/cpp/farm_ng/core/misc/time_series_test.cpp @@ -14,11 +14,12 @@ #include "farm_ng/core/misc/time_series.h" +#include "farm_ng/core/logging/fmt_eigen.h" #include "farm_ng/core/misc/conversions.h" -#include "sophus/interp/interpolate.h" +#include "sophus2/interp/interpolate.h" #include -#include +#include #include @@ -180,7 +181,7 @@ TEST(time_series, index) { struct StampedInterpolative { double t; - sophus::Rotation3F64 world_from_foo; + sophus2::Rotation3F64 world_from_foo; Eigen::Vector3d angular_velocity; }; @@ -198,7 +199,7 @@ auto interpolate( StampedInterpolative result; result.t = (1.0 - p) * lhs.t + p * rhs.t; result.world_from_foo = - sophus::interpolate(lhs.world_from_foo, rhs.world_from_foo, p); + sophus2::interpolate(lhs.world_from_foo, rhs.world_from_foo, p); result.angular_velocity = (1.0 - p) * lhs.angular_velocity + p * rhs.angular_velocity; return result; @@ -244,7 +245,7 @@ TEST(interpolative_time_series, regression) { double acceleration_y = 0.033; Eigen::Vector3d angular_velocity(0.0, 0.0, 0.0); - sophus::Rotation3F64 world_from_foo; + sophus2::Rotation3F64 world_from_foo; TimeSeries series; @@ -252,7 +253,7 @@ TEST(interpolative_time_series, regression) { StampedInterpolative value; value.t = t; value.world_from_foo = - world_from_foo * sophus::Rotation3F64::exp(delta_t * angular_velocity); + world_from_foo * sophus2::Rotation3F64::exp(delta_t * angular_velocity); value.angular_velocity = angular_velocity; angular_velocity.x() += delta_t * acceleration_x; angular_velocity.y() += delta_t * acceleration_y; @@ -286,7 +287,7 @@ TEST(interpolative_time_series, regression) { "interp.world_from_foo:\n {}", interpolated.world_from_foo.compactMatrix()); - static double const kEps = sophus::kEpsilonF32; + static double const kEps = sophus2::kEpsilonF32; FARM_ASSERT_WITHIN_REL(interpolated.t, value.t, kEps); FARM_ASSERT_WITHIN_REL( interpolated.angular_velocity, value.angular_velocity, 0.01); diff --git a/cpp/farm_ng/core/misc/tokenize.h b/cpp/farm_ng/core/misc/tokenize.h index 5ff5fd58..ce0c2183 100644 --- a/cpp/farm_ng/core/misc/tokenize.h +++ b/cpp/farm_ng/core/misc/tokenize.h @@ -25,14 +25,22 @@ namespace farm_ng { inline std::vector tokenize( std::string const& str, char delimiter = ' ') { std::vector tokens; - std::stringstream strstream(str); std::string token; - while (getline(strstream, token, delimiter)) { + // Added check to handle the case where the last character is the delimiter + bool lastCharDelimiter = !str.empty() && str.back() == delimiter; + + while (std::getline(strstream, token, delimiter)) { tokens.push_back(token); } + // If the last character in the input string is a delimiter, + // add an empty string to tokens to account for the final empty token. + if (lastCharDelimiter) { + tokens.push_back(""); + } + return tokens; } diff --git a/cpp/farm_ng/core/misc/tokenize_test.cpp b/cpp/farm_ng/core/misc/tokenize_test.cpp index d123ed2a..59c8d30b 100644 --- a/cpp/farm_ng/core/misc/tokenize_test.cpp +++ b/cpp/farm_ng/core/misc/tokenize_test.cpp @@ -14,6 +14,23 @@ #include "farm_ng/core/misc/tokenize.h" +#include #include -TEST(tokenize, smoke) {} // NOLINT +TEST(tokenize, smoke) { + using namespace farm_ng; + + std::vector tokens = tokenize("foo bar"); + FARM_ASSERT_EQ(tokens.size(), 2); + FARM_ASSERT_EQ(tokens[0], "foo"); + FARM_ASSERT_EQ(tokens[1], "bar"); + + tokens = tokenize(",foo,,bar,", ','); + FARM_ASSERT_EQ(tokens.size(), 5); + FARM_ASSERT_EQ(tokens[0], ""); + FARM_ASSERT_EQ(tokens[1], "foo"); + FARM_ASSERT_EQ(tokens[2], ""); + FARM_ASSERT_EQ(tokens[3], "bar"); + FARM_ASSERT_EQ(tokens[4], ""); + +} // NOLINT diff --git a/cpp/farm_ng/core/plotting/CMakeLists.txt b/cpp/farm_ng/core/plotting/CMakeLists.txt index 25195416..684f3491 100644 --- a/cpp/farm_ng/core/plotting/CMakeLists.txt +++ b/cpp/farm_ng/core/plotting/CMakeLists.txt @@ -21,7 +21,7 @@ endforeach() farm_ng_add_library(farm_ng_core_plotting NAMESPACE farm_ng_core - INCLUDE_DIR ../.. + INCLUDE_DIR ../../.. HEADERS ${farm_ng_component_headers} SOURCES @@ -43,8 +43,6 @@ foreach(test_basename ${farm_ng_component_src_prefixes}) LABELS small) endforeach() -find_package(CLI11 REQUIRED) - add_executable(remote_plot_example remote_plot_example.cpp) target_link_libraries(remote_plot_example farm_ng_core_plotting diff --git a/cpp/farm_ng/core/plotting/remote_plot_example.cpp b/cpp/farm_ng/core/plotting/remote_plot_example.cpp index 8ff8712c..1a7c974e 100644 --- a/cpp/farm_ng/core/plotting/remote_plot_example.cpp +++ b/cpp/farm_ng/core/plotting/remote_plot_example.cpp @@ -34,16 +34,16 @@ int main(int argc, char* argv[]) { .y_bounds = {.height = 2.0, .offset = -0.8}}; trig_graph.path = "trig/ {sin,cos,tan}"; trig_graph.color = { - sophus::Color::neonRed(), - sophus::Color::neonGreen(), - sophus::Color::neonBlue()}; + sophus2::Color::neonRed(), + sophus2::Color::neonGreen(), + sophus2::Color::neonBlue()}; plotting::Curve sawtooth; sawtooth.bounds = { .x_bounds = {.len = 2.5 * M_PI}, .y_bounds = {.height = 2.0, .offset = 0.0}}; sawtooth.path = "trig1/sawtooth"; - sawtooth.color = sophus::Color::blue(); + sawtooth.color = sophus2::Color::blue(); plotting::Vec3CurveWithConfInterval conf_trig_graph; conf_trig_graph.bounds = { @@ -51,13 +51,13 @@ int main(int argc, char* argv[]) { .y_bounds = {.height = 2.5, .offset = 0.0}}; conf_trig_graph.path = "trig3/ {sin,cos,tan}_conf"; conf_trig_graph.color = { - sophus::Color::neonRed(), - sophus::Color::neonGreen(), - sophus::Color::neonBlue()}; + sophus2::Color::neonRed(), + sophus2::Color::neonGreen(), + sophus2::Color::neonBlue()}; conf_trig_graph.conf_color = { - sophus::Color::darkRed(), - sophus::Color::darkGreen(), - sophus::Color::darkBlue()}; + sophus2::Color::darkRed(), + sophus2::Color::darkGreen(), + sophus2::Color::darkBlue()}; plotting::RectPlot timestamps; timestamps.path = "t/timestamps"; diff --git a/cpp/farm_ng/core/plotting/remote_plotting_client.cpp b/cpp/farm_ng/core/plotting/remote_plotting_client.cpp index 01df1b86..f1d79326 100644 --- a/cpp/farm_ng/core/plotting/remote_plotting_client.cpp +++ b/cpp/farm_ng/core/plotting/remote_plotting_client.cpp @@ -14,6 +14,7 @@ #include "farm_ng/core/plotting/remote_plotting_client.h" +#include "farm_ng/core/logging/fmt_grpc.h" #include "farm_ng/core/plotting/plotting.grpc.pb.h" #include "farm_ng/core/proto_conv/linalg/conv.h" #include "farm_ng/core/proto_conv/plotting/conv.h" diff --git a/cpp/farm_ng/core/plotting/remote_plotting_client.h b/cpp/farm_ng/core/plotting/remote_plotting_client.h index 812c956f..10f3195f 100644 --- a/cpp/farm_ng/core/plotting/remote_plotting_client.h +++ b/cpp/farm_ng/core/plotting/remote_plotting_client.h @@ -15,7 +15,7 @@ #include "farm_ng/core/misc/shared.h" #include "farm_ng/core/pipeline/component.h" #include "farm_ng/core/plotting/types.h" -#include "sophus/calculus/region.h" +#include "sophus2/calculus/region.h" #pragma once diff --git a/cpp/farm_ng/core/plotting/types.cpp b/cpp/farm_ng/core/plotting/types.cpp index ad858468..a3f4e733 100644 --- a/cpp/farm_ng/core/plotting/types.cpp +++ b/cpp/farm_ng/core/plotting/types.cpp @@ -19,25 +19,25 @@ namespace farm_ng { namespace plotting { ColoredRect ColoredRect::xLine( - sophus::Color const& color, + sophus2::Color const& color, double x, - sophus::RegionF64 const& y_range, + sophus2::RegionF64 const& y_range, double thickness) { return ColoredRect{ .color = color, - .region = sophus::Region2F64::fromMinMax( + .region = sophus2::Region2F64::fromMinMax( {x - 0.5 * thickness, y_range.min()}, {x + 0.5 * thickness, y_range.max()}), }; } ColoredRect ColoredRect::yLine( - sophus::Color const& color, + sophus2::Color const& color, double y, - sophus::RegionF64 const& x_range, + sophus2::RegionF64 const& x_range, double thickness) { return ColoredRect{ .color = color, - .region = sophus::Region2F64::fromMinMax( + .region = sophus2::Region2F64::fromMinMax( {x_range.min(), y - 0.5 * thickness}, {x_range.max(), y + 0.5 * thickness}), }; diff --git a/cpp/farm_ng/core/plotting/types.h b/cpp/farm_ng/core/plotting/types.h index 0137601d..f74005d8 100644 --- a/cpp/farm_ng/core/plotting/types.h +++ b/cpp/farm_ng/core/plotting/types.h @@ -13,8 +13,8 @@ // limitations under the License. #include "farm_ng/core/struct/struct.h" -#include "sophus/calculus/region.h" -#include "sophus/color/color.h" +#include "sophus2/calculus/region.h" +#include "sophus2/color/color.h" #include @@ -61,19 +61,19 @@ FARM_STRUCT( Curve, ((Bounds, bounds, {}), (ClearCondition, clear_cond, {}), - (sophus::Color, color, {}), + (sophus2::Color, color, {}), (LineType, line_type, {LineType::line_strip}), (std::filesystem::path, path, {}), (std::deque, x_y_pairs, {}))); // No commas allowed in macro arguments, so we need to define this here. -using ColorArray3 = std::array; +using ColorArray3 = std::array; ColorArray3 constexpr kDefaultColorArray3 = { - sophus::Color::red(), sophus::Color::green(), sophus::Color::blue()}; + sophus2::Color::red(), sophus2::Color::green(), sophus2::Color::blue()}; ColorArray3 constexpr kDefaultConfColorArray3 = { - sophus::Color::orange(), - sophus::Color::turquoise(), - sophus::Color::violet()}; + sophus2::Color::orange(), + sophus2::Color::turquoise(), + sophus2::Color::violet()}; // Add a "3-vec curve" to the plot or append to exiting one with path being // "plot_name/curve_name". If a curve with that name already exists, it will be @@ -115,7 +115,7 @@ struct ColoredRect { "color", "region"}; /// Tuple of field types (for FARM_PROTO_CONV_IMPL to work) - using FieldTypes = std::tuple; + using FieldTypes = std::tuple; static_assert( std::tuple_size_v == kNumFields, "Tuple size mismatch. Make sure to update kNumFields and kFieldNames when" @@ -123,23 +123,23 @@ struct ColoredRect { /// Vertical Line static ColoredRect xLine( - sophus::Color const& color, + sophus2::Color const& color, double x, - sophus::RegionF64 const& y_range, + sophus2::RegionF64 const& y_range, double thickness = 0.01); /// Horizontal line static ColoredRect yLine( - sophus::Color const& color, + sophus2::Color const& color, double y, - sophus::RegionF64 const& x_range, + sophus2::RegionF64 const& x_range, double thickness = 0.01); /// Color of the rectangle. - sophus::Color color{}; + sophus2::Color color{}; /// Region the rectangle describes. - sophus::Region2F64 region = sophus::Region2F64::empty(); + sophus2::Region2F64 region = sophus2::Region2F64::empty(); }; FARM_STRUCT( diff --git a/cpp/farm_ng/core/proto_conv/calculus/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/calculus/CMakeLists.txt index 12d5a247..e8866391 100644 --- a/cpp/farm_ng/core/proto_conv/calculus/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/calculus/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_calculus NAMESPACE farm_ng_core - INCLUDE_DIR ../../.. + INCLUDE_DIR ../../../.. HEADERS conv.h SOURCES @@ -8,7 +8,7 @@ farm_ng_add_library(farm_ng_core_proto_conv_calculus ) target_link_libraries(farm_ng_core_proto_conv_calculus PUBLIC - Sophus::sophus_calculus + farm_ng_core::sophus2_calculus protobuf::libprotobuf farm_ng_core::farm_ng_core_prototools farm_ng_core_proto_defs diff --git a/cpp/farm_ng/core/proto_conv/calculus/conv.cpp b/cpp/farm_ng/core/proto_conv/calculus/conv.cpp index 043bbb25..894d89a8 100644 --- a/cpp/farm_ng/core/proto_conv/calculus/conv.cpp +++ b/cpp/farm_ng/core/proto_conv/calculus/conv.cpp @@ -14,27 +14,28 @@ #include "farm_ng/core/proto_conv/calculus/conv.h" +#include "farm_ng/core/logging/fmt_eigen.h" #include "farm_ng/core/proto_conv/linalg/conv.h" -#include "farm_ng/core/proto_conv/std/conv_impl_macro.ipp" +#include "farm_ng/core/proto_conv/std/repeated_message_impl_macro.ipp" namespace farm_ng { template <> auto fromProt(core::proto::RegionI32 const& proto) - -> Expected { + -> Expected { if (proto.is_empty()) { - return sophus::RegionI::empty(); + return sophus2::RegionI::empty(); } if (!(proto.min() <= proto.max())) { return FARM_UNEXPECTED( "min {} must be <= max {}", proto.min(), proto.max()); } - return sophus::RegionI::fromMinMax(proto.min(), proto.max()); + return sophus2::RegionI::fromMinMax(proto.min(), proto.max()); } template <> -auto toProt(sophus::RegionI const& region) +auto toProt(sophus2::RegionI const& region) -> core::proto::RegionI32 { core::proto::RegionI32 proto; if (region.isEmpty()) { @@ -49,19 +50,19 @@ auto toProt(sophus::RegionI const& region) template <> auto fromProt(core::proto::RegionF32 const& proto) - -> Expected { + -> Expected { if (proto.is_empty()) { - return sophus::RegionF32::empty(); + return sophus2::RegionF32::empty(); } if (!(proto.min() <= proto.max())) { return FARM_UNEXPECTED( "min {} must be <= max {}", proto.min(), proto.max()); } - return sophus::RegionF32::fromMinMax(proto.min(), proto.max()); + return sophus2::RegionF32::fromMinMax(proto.min(), proto.max()); } template <> -auto toProt(sophus::RegionF32 const& region) +auto toProt(sophus2::RegionF32 const& region) -> core::proto::RegionF32 { core::proto::RegionF32 proto; if (region.isEmpty()) { @@ -76,20 +77,20 @@ auto toProt(sophus::RegionF32 const& region) template <> auto fromProt(core::proto::RegionF64 const& proto) - -> Expected { + -> Expected { if (proto.is_empty()) { - return sophus::RegionF64::empty(); + return sophus2::RegionF64::empty(); } if (!(proto.min() <= proto.max())) { return FARM_UNEXPECTED( "min {} must be <= max {}", proto.min(), proto.max()); } - return sophus::RegionF64::fromMinMax(proto.min(), proto.max()); + return sophus2::RegionF64::fromMinMax(proto.min(), proto.max()); } template <> -auto toProt(sophus::RegionF64 const& region) +auto toProt(sophus2::RegionF64 const& region) -> core::proto::RegionF64 { core::proto::RegionF64 proto; if (region.isEmpty()) { @@ -104,9 +105,9 @@ auto toProt(sophus::RegionF64 const& region) template <> auto fromProt(core::proto::Region2F64 const& proto) - -> Expected { + -> Expected { if (proto.is_empty()) { - return sophus::Region2F64::empty(); + return sophus2::Region2F64::empty(); } FARM_TRY(auto, mi, fromProt(proto.min())); FARM_TRY(auto, ma, fromProt(proto.max())); @@ -114,11 +115,11 @@ auto fromProt(core::proto::Region2F64 const& proto) return FARM_UNEXPECTED( "min ({}) must be <= max ({})", mi.transpose(), ma.transpose()); } - return sophus::Region2F64::fromMinMax(mi, ma); + return sophus2::Region2F64::fromMinMax(mi, ma); } template <> -auto toProt(sophus::Region2F64 const& region) +auto toProt(sophus2::Region2F64 const& region) -> core::proto::Region2F64 { core::proto::Region2F64 proto; if (region.isEmpty()) { @@ -131,6 +132,7 @@ auto toProt(sophus::Region2F64 const& region) return proto; } -FARM_CONV_IMPL_REPEATED(core::proto::RepeatedG0Region2F64, sophus::Region2F64) +FARM_CONV_IMPL_REPEATED_MESSAGE( + core::proto::RepeatedG0Region2F64, std::deque) } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/calculus/conv.h b/cpp/farm_ng/core/proto_conv/calculus/conv.h index b40e0b30..c761aee2 100644 --- a/cpp/farm_ng/core/proto_conv/calculus/conv.h +++ b/cpp/farm_ng/core/proto_conv/calculus/conv.h @@ -16,19 +16,19 @@ #include "farm_ng/core/calculus.pb.h" #include "farm_ng/core/proto_conv/traits.h" -#include "sophus/calculus/region.h" +#include "sophus2/calculus/region.h" #include namespace farm_ng { -FARM_PROTO_CONV_TRAIT(sophus::RegionI, core::proto::RegionI32); -FARM_PROTO_CONV_TRAIT(sophus::RegionF32, core::proto::RegionF32); -FARM_PROTO_CONV_TRAIT(sophus::RegionF64, core::proto::RegionF64); +FARM_PROTO_CONV_TRAIT(sophus2::RegionI, core::proto::RegionI32); +FARM_PROTO_CONV_TRAIT(sophus2::RegionF32, core::proto::RegionF32); +FARM_PROTO_CONV_TRAIT(sophus2::RegionF64, core::proto::RegionF64); -FARM_PROTO_CONV_TRAIT(sophus::Region2F64, core::proto::Region2F64); +FARM_PROTO_CONV_TRAIT(sophus2::Region2F64, core::proto::Region2F64); FARM_PROTO_CONV_TRAIT( - std::deque, core::proto::RepeatedG0Region2F64); + std::deque, core::proto::RepeatedG0Region2F64); } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/calculus/conv_test.cpp b/cpp/farm_ng/core/proto_conv/calculus/conv_test.cpp index 0042d605..b4e593ea 100644 --- a/cpp/farm_ng/core/proto_conv/calculus/conv_test.cpp +++ b/cpp/farm_ng/core/proto_conv/calculus/conv_test.cpp @@ -16,21 +16,21 @@ #include -using namespace sophus; +using namespace sophus2; using namespace farm_ng; using namespace farm_ng::core; TEST(conv_calculus, unit) { { - sophus::RegionI region = sophus::RegionI::empty(); + sophus2::RegionI region = sophus2::RegionI::empty(); auto proto = toProt(region); EXPECT_TRUE(proto.is_empty()); auto maybe_region = fromProt(proto); auto region_copy = FARM_UNWRAP(maybe_region); EXPECT_TRUE(region_copy.isEmpty()); - region = sophus::RegionI::fromMinMax(1, 2); + region = sophus2::RegionI::fromMinMax(1, 2); proto = toProt(region); EXPECT_FALSE(proto.is_empty()); maybe_region = fromProt(proto); @@ -40,14 +40,14 @@ TEST(conv_calculus, unit) { EXPECT_EQ(region_copy.max(), 2); } { - sophus::RegionF32 region = sophus::RegionF32::empty(); + sophus2::RegionF32 region = sophus2::RegionF32::empty(); auto proto = toProt(region); EXPECT_TRUE(proto.is_empty()); auto maybe_region = fromProt(proto); auto region_copy = FARM_UNWRAP(maybe_region); EXPECT_TRUE(region_copy.isEmpty()); - region = sophus::RegionF32::fromMinMax(1, 2); + region = sophus2::RegionF32::fromMinMax(1, 2); proto = toProt(region); EXPECT_FALSE(proto.is_empty()); maybe_region = fromProt(proto); @@ -58,14 +58,14 @@ TEST(conv_calculus, unit) { } { - sophus::RegionF64 region = sophus::RegionF64::empty(); + sophus2::RegionF64 region = sophus2::RegionF64::empty(); auto proto = toProt(region); EXPECT_TRUE(proto.is_empty()); auto maybe_region = fromProt(proto); auto region_copy = FARM_UNWRAP(maybe_region); EXPECT_TRUE(region_copy.isEmpty()); - region = sophus::RegionF64::fromMinMax(1, 2); + region = sophus2::RegionF64::fromMinMax(1, 2); proto = toProt(region); EXPECT_FALSE(proto.is_empty()); maybe_region = fromProt(proto); diff --git a/cpp/farm_ng/core/proto_conv/color/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/color/CMakeLists.txt index 3b5f1f85..be9247fd 100644 --- a/cpp/farm_ng/core/proto_conv/color/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/color/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_color NAMESPACE farm_ng_core - INCLUDE_DIR ../../.. + INCLUDE_DIR ../../../.. HEADERS conv.h SOURCES @@ -8,7 +8,7 @@ farm_ng_add_library(farm_ng_core_proto_conv_color ) target_link_libraries(farm_ng_core_proto_conv_color PUBLIC - Sophus::sophus_color + farm_ng_core::sophus2_color protobuf::libprotobuf farm_ng_core::farm_ng_core_prototools farm_ng_core_proto_defs diff --git a/cpp/farm_ng/core/proto_conv/color/conv.cpp b/cpp/farm_ng/core/proto_conv/color/conv.cpp index ebe61cbb..06ff45f7 100644 --- a/cpp/farm_ng/core/proto_conv/color/conv.cpp +++ b/cpp/farm_ng/core/proto_conv/color/conv.cpp @@ -17,12 +17,12 @@ namespace farm_ng { template <> auto fromProt(core::proto::Color const& proto) - -> Expected { - return sophus::Color(proto.r(), proto.g(), proto.b(), proto.a()); + -> Expected { + return sophus2::Color(proto.r(), proto.g(), proto.b(), proto.a()); } template <> -auto toProt(sophus::Color const& c) -> core::proto::Color { +auto toProt(sophus2::Color const& c) -> core::proto::Color { core::proto::Color proto; proto.set_r(c.r); proto.set_g(c.g); diff --git a/cpp/farm_ng/core/proto_conv/color/conv.h b/cpp/farm_ng/core/proto_conv/color/conv.h index 596e2b30..e0427fb3 100644 --- a/cpp/farm_ng/core/proto_conv/color/conv.h +++ b/cpp/farm_ng/core/proto_conv/color/conv.h @@ -16,10 +16,10 @@ #include "farm_ng/core/color.pb.h" #include "farm_ng/core/proto_conv/traits.h" -#include "sophus/color/color.h" +#include "sophus2/color/color.h" namespace farm_ng { -FARM_PROTO_CONV_TRAIT(sophus::Color, core::proto::Color); +FARM_PROTO_CONV_TRAIT(sophus2::Color, core::proto::Color); } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/color/conv_test.cpp b/cpp/farm_ng/core/proto_conv/color/conv_test.cpp index 92daf3b3..9e06c1e7 100644 --- a/cpp/farm_ng/core/proto_conv/color/conv_test.cpp +++ b/cpp/farm_ng/core/proto_conv/color/conv_test.cpp @@ -16,7 +16,7 @@ #include -using namespace sophus; +using namespace sophus2; using namespace farm_ng; using namespace farm_ng::core; diff --git a/cpp/farm_ng/core/proto_conv/geometry/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/geometry/CMakeLists.txt index 75f34c25..21603bb7 100644 --- a/cpp/farm_ng/core/proto_conv/geometry/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/geometry/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_geometry NAMESPACE farm_ng_core - INCLUDE_DIR ../../.. + INCLUDE_DIR ../../../.. HEADERS conv.h SOURCES @@ -8,7 +8,7 @@ farm_ng_add_library(farm_ng_core_proto_conv_geometry ) target_link_libraries(farm_ng_core_proto_conv_geometry PUBLIC - Sophus::sophus_geometry + farm_ng_core::sophus2_geometry protobuf::libprotobuf farm_ng_core::farm_ng_core_prototools farm_ng_core_proto_defs diff --git a/cpp/farm_ng/core/proto_conv/geometry/conv.cpp b/cpp/farm_ng/core/proto_conv/geometry/conv.cpp index 997b8119..ef572842 100644 --- a/cpp/farm_ng/core/proto_conv/geometry/conv.cpp +++ b/cpp/farm_ng/core/proto_conv/geometry/conv.cpp @@ -20,13 +20,13 @@ namespace farm_ng { template <> auto fromProt(core::proto::UnitVec3F64 const& proto) - -> Expected { + -> Expected { FARM_TRY(auto, vec3, fromProt(proto.vec3())); - return sophus::UnitVector3F64::tryFromUnitVector(vec3); + return sophus2::UnitVector3F64::tryFromUnitVector(vec3); } template <> -auto toProt(sophus::UnitVector3F64 const& uvec) +auto toProt(sophus2::UnitVector3F64 const& uvec) -> core::proto::UnitVec3F64 { core::proto::UnitVec3F64 proto; *proto.mutable_vec3() = toProt(uvec.params()); @@ -37,7 +37,7 @@ template <> auto fromProt( core::proto::Hyperplane3F64 const& proto) -> Expected> { - SOPHUS_TRY(sophus::UnitVector3F64, normal, fromProt(proto.normal())); + SOPHUS_TRY(sophus2::UnitVector3F64, normal, fromProt(proto.normal())); return Eigen::Hyperplane{normal.params(), proto.offset()}; } @@ -46,7 +46,7 @@ auto toProt>( Eigen::Hyperplane const& plane) -> core::proto::Hyperplane3F64 { core::proto::Hyperplane3F64 proto; *proto.mutable_normal() = - toProt(sophus::UnitVector3F64::fromVectorAndNormalize(plane.normal())); + toProt(sophus2::UnitVector3F64::fromVectorAndNormalize(plane.normal())); proto.set_offset(plane.offset()); return proto; } diff --git a/cpp/farm_ng/core/proto_conv/geometry/conv.h b/cpp/farm_ng/core/proto_conv/geometry/conv.h index 66a14945..91b6549f 100644 --- a/cpp/farm_ng/core/proto_conv/geometry/conv.h +++ b/cpp/farm_ng/core/proto_conv/geometry/conv.h @@ -16,13 +16,13 @@ #include "farm_ng/core/geometry.pb.h" #include "farm_ng/core/proto_conv/traits.h" -#include "sophus/geometry/ray.h" +#include "sophus2/geometry/ray.h" namespace farm_ng { using Hyperplane3d = Eigen::Hyperplane; -FARM_PROTO_CONV_TRAIT(sophus::UnitVector3F64, core::proto::UnitVec3F64); +FARM_PROTO_CONV_TRAIT(sophus2::UnitVector3F64, core::proto::UnitVec3F64); FARM_PROTO_CONV_TRAIT(Hyperplane3d, core::proto::Hyperplane3F64); } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/geometry/conv_test.cpp b/cpp/farm_ng/core/proto_conv/geometry/conv_test.cpp index 63258750..41ee71a7 100644 --- a/cpp/farm_ng/core/proto_conv/geometry/conv_test.cpp +++ b/cpp/farm_ng/core/proto_conv/geometry/conv_test.cpp @@ -16,21 +16,22 @@ #include -using namespace sophus; +using namespace sophus2; using namespace farm_ng; using namespace farm_ng::core; TEST(conv_geometry, unit) { { - auto uvec = sophus::UnitVector3F64::fromVectorAndNormalize({1.0, 2.0, 3.0}); + auto uvec = + sophus2::UnitVector3F64::fromVectorAndNormalize({1.0, 2.0, 3.0}); proto::UnitVec3F64 proto = toProt(uvec); EXPECT_EQ(proto.vec3().x(), uvec.vector().x()); EXPECT_EQ(proto.vec3().y(), uvec.vector().y()); EXPECT_EQ(proto.vec3().z(), uvec.vector().z()); auto maybe_uvec2 = fromProt(proto); - sophus::UnitVector3F64 uvec2 = FARM_UNWRAP(maybe_uvec2); + sophus2::UnitVector3F64 uvec2 = FARM_UNWRAP(maybe_uvec2); EXPECT_EQ(uvec2.vector(), uvec.vector()); auto vec = uvec2.vector(); EXPECT_NEAR(vec.norm(), 1.0, 1e-6); diff --git a/cpp/farm_ng/core/proto_conv/image/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/image/CMakeLists.txt index b1b40135..f4bc8df5 100644 --- a/cpp/farm_ng/core/proto_conv/image/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/image/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_image NAMESPACE farm_ng_core - INCLUDE_DIR ../../.. + INCLUDE_DIR ../../../.. HEADERS conv.h SOURCES @@ -12,7 +12,7 @@ target_link_libraries(farm_ng_core_proto_conv_image PUBLIC farm_ng_core::farm_ng_core_proto_defs farm_ng_core::farm_ng_core_prototools farm_ng_core::farm_ng_core_proto_conv_linalg - Sophus::sophus_image + farm_ng_core::sophus2_image ) farm_ng_add_test(conv diff --git a/cpp/farm_ng/core/proto_conv/image/conv.cpp b/cpp/farm_ng/core/proto_conv/image/conv.cpp index 8fc31478..35b78546 100644 --- a/cpp/farm_ng/core/proto_conv/image/conv.cpp +++ b/cpp/farm_ng/core/proto_conv/image/conv.cpp @@ -18,12 +18,12 @@ namespace farm_ng { template <> auto fromProt(core::proto::ImageSize const& proto) - -> Expected { - return sophus::ImageSize(proto.width(), proto.height()); + -> Expected { + return sophus2::ImageSize(proto.width(), proto.height()); } template <> -auto toProt(sophus::ImageSize const& image_size) +auto toProt(sophus2::ImageSize const& image_size) -> core::proto::ImageSize { core::proto::ImageSize proto; proto.set_width(image_size.width); @@ -33,13 +33,13 @@ auto toProt(sophus::ImageSize const& image_size) template <> auto fromProt(core::proto::ImageLayout const& proto) - -> Expected { + -> Expected { FARM_TRY(auto, size, fromProt(proto.size())); - return sophus::ImageLayout(size, proto.pitch_bytes()); + return sophus2::ImageLayout(size, proto.pitch_bytes()); } template <> -auto toProt(sophus::ImageLayout const& layout) +auto toProt(sophus2::ImageLayout const& layout) -> core::proto::ImageLayout { core::proto::ImageLayout proto; *proto.mutable_size() = toProt(layout.imageSize()); @@ -49,8 +49,8 @@ auto toProt(sophus::ImageLayout const& layout) template <> auto fromProt(core::proto::PixelFormat const& proto) - -> Expected { - sophus::PixelFormat format; + -> Expected { + sophus2::PixelFormat format; FARM_TRY_ASSERT(trySetFromString(format.number_type, proto.number_type())); format.num_components = proto.num_components(); format.num_bytes_per_component = proto.num_bytes_per_component(); @@ -58,7 +58,7 @@ auto fromProt(core::proto::PixelFormat const& proto) } template <> -auto toProt(sophus::PixelFormat const& layout) +auto toProt(sophus2::PixelFormat const& layout) -> core::proto::PixelFormat { core::proto::PixelFormat proto; proto.set_number_type(toString(layout.number_type)); @@ -69,19 +69,19 @@ auto toProt(sophus::PixelFormat const& layout) template <> auto fromProt(core::proto::DynImage const& proto) - -> Expected> { - SOPHUS_TRY(sophus::PixelFormat, format, fromProt(proto.pixel_format())); + -> Expected> { + SOPHUS_TRY(sophus2::PixelFormat, format, fromProt(proto.pixel_format())); SOPHUS_TRY(auto, layout, fromProt(proto.layout())); SOPHUS_ASSERT_EQ(size_t(layout.sizeBytes()), proto.data().size()); SOPHUS_TRY( - auto, mut_image, sophus::MutAnyImage<>::tryFromFormat(layout, format)); + auto, mut_image, sophus2::MutAnyImage<>::tryFromFormat(layout, format)); std::memcpy(mut_image.rawMutPtr(), proto.data().data(), proto.data().size()); return mut_image; } template <> -auto toProt>(sophus::AnyImage<> const& image) +auto toProt>(sophus2::AnyImage<> const& image) -> core::proto::DynImage { core::proto::DynImage proto; *proto.mutable_pixel_format() = toProt(image.pixelFormat()); @@ -90,14 +90,14 @@ auto toProt>(sophus::AnyImage<> const& image) return proto; } -Expected> intensityImageFromProto( +Expected> intensityImageFromProto( core::proto::DynImage const& proto) { - SOPHUS_TRY(sophus::AnyImage<>, any_image, fromProt(proto)); - return sophus::IntensityImage<>::tryFrom(any_image); + SOPHUS_TRY(sophus2::AnyImage<>, any_image, fromProt(proto)); + return sophus2::IntensityImage<>::tryFrom(any_image); } template <> -auto toProt>(sophus::IntensityImage<> const& image) +auto toProt>(sophus2::IntensityImage<> const& image) -> core::proto::DynImage { core::proto::DynImage proto; *proto.mutable_pixel_format() = toProt(image.pixelFormat()); diff --git a/cpp/farm_ng/core/proto_conv/image/conv.h b/cpp/farm_ng/core/proto_conv/image/conv.h index a47f5ba0..22cc9fac 100644 --- a/cpp/farm_ng/core/proto_conv/image/conv.h +++ b/cpp/farm_ng/core/proto_conv/image/conv.h @@ -16,29 +16,29 @@ #include "farm_ng/core/image.pb.h" #include "farm_ng/core/proto_conv/traits.h" -#include "sophus/image/dyn_image_types.h" +#include "sophus2/image/dyn_image_types.h" namespace farm_ng { -FARM_PROTO_CONV_TRAIT(sophus::ImageSize, core::proto::ImageSize); -FARM_PROTO_CONV_TRAIT(sophus::ImageLayout, core::proto::ImageLayout); -FARM_PROTO_CONV_TRAIT(sophus::PixelFormat, core::proto::PixelFormat); -FARM_PROTO_CONV_TRAIT(sophus::AnyImage<>, core::proto::DynImage); +FARM_PROTO_CONV_TRAIT(sophus2::ImageSize, core::proto::ImageSize); +FARM_PROTO_CONV_TRAIT(sophus2::ImageLayout, core::proto::ImageLayout); +FARM_PROTO_CONV_TRAIT(sophus2::PixelFormat, core::proto::PixelFormat); +FARM_PROTO_CONV_TRAIT(sophus2::AnyImage<>, core::proto::DynImage); -/// Converts core::proto::DynImage to a sophus::IntensityImage. -Expected> intensityImageFromProto( +/// Converts core::proto::DynImage to a sophus2::IntensityImage. +Expected> intensityImageFromProto( core::proto::DynImage const& proto); -/// Associates a proto type to a sophus::IntensityImage<>. +/// Associates a proto type to a sophus2::IntensityImage<>. template <> -struct ToProtoTrait> { +struct ToProtoTrait> { /// proto type using ProtoType = core::proto::DynImage; }; -/// Converts a sophus::IntensityImage<> to a proto. +/// Converts a sophus2::IntensityImage<> to a proto. template <> -auto toProt>(sophus::IntensityImage<> const& cpp) +auto toProt>(sophus2::IntensityImage<> const& cpp) -> core::proto::DynImage; } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/image/conv_test.cpp b/cpp/farm_ng/core/proto_conv/image/conv_test.cpp index ef6ba313..20cf9f51 100644 --- a/cpp/farm_ng/core/proto_conv/image/conv_test.cpp +++ b/cpp/farm_ng/core/proto_conv/image/conv_test.cpp @@ -14,11 +14,13 @@ #include "farm_ng/core/proto_conv/image/conv.h" +#include "farm_ng/core/logging/fmt_eigen.h" + #include namespace farm_ng::test { -using namespace sophus; +using namespace sophus2; template void testPixelFormat() { diff --git a/cpp/farm_ng/core/proto_conv/lie/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/lie/CMakeLists.txt index 5b5b4e9f..1d27712d 100644 --- a/cpp/farm_ng/core/proto_conv/lie/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/lie/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_lie NAMESPACE farm_ng_core - INCLUDE_DIR ../../.. + INCLUDE_DIR ../../../.. HEADERS conv.h SOURCES @@ -8,8 +8,9 @@ farm_ng_add_library(farm_ng_core_proto_conv_lie ) target_link_libraries(farm_ng_core_proto_conv_lie PUBLIC - Sophus::sophus_lie + farm_ng_core::sophus2_lie protobuf::libprotobuf + farm_ng_core::farm_ng_core_proto_conv_std farm_ng_core::farm_ng_core_proto_conv_linalg farm_ng_core::farm_ng_core_prototools ) diff --git a/cpp/farm_ng/core/proto_conv/lie/conv.cpp b/cpp/farm_ng/core/proto_conv/lie/conv.cpp index 11f6127a..92a66004 100644 --- a/cpp/farm_ng/core/proto_conv/lie/conv.cpp +++ b/cpp/farm_ng/core/proto_conv/lie/conv.cpp @@ -15,14 +15,16 @@ #include "farm_ng/core/proto_conv/lie/conv.h" #include "farm_ng/core/proto_conv/linalg/conv.h" +#include "farm_ng/core/proto_conv/std/optional_message_impl_macro.ipp" +#include "farm_ng/core/proto_conv/std/repeated_message_impl_macro.ipp" namespace farm_ng { template <> auto fromProt( core::proto::QuaternionF64 const& proto) - -> Expected { - sophus::QuaternionF64 quat; + -> Expected { + sophus2::QuaternionF64 quat; FARM_TRY(auto, vec3, fromProt(proto.imag())); quat.imag() = vec3; quat.real() = proto.real(); @@ -30,7 +32,7 @@ auto fromProt( } template <> -auto toProt(sophus::QuaternionF64 const& quat) +auto toProt(sophus2::QuaternionF64 const& quat) -> core::proto::QuaternionF64 { core::proto::QuaternionF64 proto; proto.set_real(quat.real()); @@ -40,12 +42,12 @@ auto toProt(sophus::QuaternionF64 const& quat) template <> auto fromProt(core::proto::Rotation2F64 const& proto) - -> Expected { - return sophus::Rotation2F64(proto.theta()); + -> Expected { + return sophus2::Rotation2F64(proto.theta()); } template <> -auto toProt(sophus::Rotation2F64 const& rotation) +auto toProt(sophus2::Rotation2F64 const& rotation) -> core::proto::Rotation2F64 { core::proto::Rotation2F64 proto; proto.set_theta(rotation.log()[0]); @@ -54,14 +56,14 @@ auto toProt(sophus::Rotation2F64 const& rotation) template <> auto fromProt(core::proto::Isometry2F64 const& proto) - -> Expected { + -> Expected { FARM_TRY(auto, translation, fromProt(proto.translation())); FARM_TRY(auto, rotation, fromProt(proto.rotation())); - return sophus::Isometry2F64(translation, rotation); + return sophus2::Isometry2F64(translation, rotation); } template <> -auto toProt(sophus::Isometry2F64 const& pose) +auto toProt(sophus2::Isometry2F64 const& pose) -> core::proto::Isometry2F64 { core::proto::Isometry2F64 proto; *proto.mutable_rotation() = toProt(pose.rotation()); @@ -71,18 +73,18 @@ auto toProt(sophus::Isometry2F64 const& pose) template <> auto fromProt(core::proto::Rotation3F64 const& proto) - -> Expected { + -> Expected { FARM_TRY(auto, quat, fromProt(proto.unit_quaternion())); auto valid = - sophus::lie::Rotation3Impl::areParamsValid(quat.params()); + sophus2::lie::Rotation3Impl::areParamsValid(quat.params()); if (!valid) { return FARM_UNEXPECTED("{}", valid.error()); } - return sophus::Rotation3F64::fromUnitQuaternion(quat); + return sophus2::Rotation3F64::fromUnitQuaternion(quat); } template <> -auto toProt(sophus::Rotation3F64 const& rotation) +auto toProt(sophus2::Rotation3F64 const& rotation) -> core::proto::Rotation3F64 { core::proto::Rotation3F64 proto; *proto.mutable_unit_quaternion() = toProt(rotation.unitQuaternion()); @@ -91,14 +93,14 @@ auto toProt(sophus::Rotation3F64 const& rotation) template <> auto fromProt(core::proto::Isometry3F64 const& proto) - -> Expected { + -> Expected { FARM_TRY(auto, rotation, fromProt(proto.rotation())); FARM_TRY(auto, translation, fromProt(proto.translation())); - return sophus::Isometry3F64(translation, rotation); + return sophus2::Isometry3F64(translation, rotation); } template <> -auto toProt(sophus::Isometry3F64 const& pose) +auto toProt(sophus2::Isometry3F64 const& pose) -> core::proto::Isometry3F64 { core::proto::Isometry3F64 proto; *proto.mutable_rotation() = toProt(pose.rotation()); @@ -106,4 +108,10 @@ auto toProt(sophus::Isometry3F64 const& pose) return proto; } +FARM_CONV_IMPL_MESSAGE_OPTIONAL( + core::proto::OptionalG0Isometry3F64, sophus2::Isometry3F64); + +FARM_CONV_IMPL_REPEATED_MESSAGE( + core::proto::RepeatedG0Isometry3F64, std::vector); + } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/lie/conv.h b/cpp/farm_ng/core/proto_conv/lie/conv.h index c5aa5936..75af940d 100644 --- a/cpp/farm_ng/core/proto_conv/lie/conv.h +++ b/cpp/farm_ng/core/proto_conv/lie/conv.h @@ -15,18 +15,26 @@ #pragma once #include "farm_ng/core/lie.pb.h" +#include "farm_ng/core/proto_conv/std/conv.h" #include "farm_ng/core/proto_conv/traits.h" -#include "sophus/lie/isometry2.h" -#include "sophus/lie/isometry3.h" -#include "sophus/lie/rotation2.h" -#include "sophus/lie/rotation3.h" +#include "sophus2/lie/isometry2.h" +#include "sophus2/lie/isometry3.h" +#include "sophus2/lie/rotation2.h" +#include "sophus2/lie/rotation3.h" + +#include namespace farm_ng { -FARM_PROTO_CONV_TRAIT(sophus::QuaternionF64, core::proto::QuaternionF64); -FARM_PROTO_CONV_TRAIT(sophus::Rotation2F64, core::proto::Rotation2F64); -FARM_PROTO_CONV_TRAIT(sophus::Isometry2F64, core::proto::Isometry2F64); -FARM_PROTO_CONV_TRAIT(sophus::Rotation3F64, core::proto::Rotation3F64); -FARM_PROTO_CONV_TRAIT(sophus::Isometry3F64, core::proto::Isometry3F64); +FARM_PROTO_CONV_TRAIT(sophus2::QuaternionF64, core::proto::QuaternionF64); +FARM_PROTO_CONV_TRAIT(sophus2::Rotation2F64, core::proto::Rotation2F64); +FARM_PROTO_CONV_TRAIT(sophus2::Isometry2F64, core::proto::Isometry2F64); +FARM_PROTO_CONV_TRAIT(sophus2::Rotation3F64, core::proto::Rotation3F64); +FARM_PROTO_CONV_TRAIT(sophus2::Isometry3F64, core::proto::Isometry3F64); + +FARM_PROTO_CONV_TRAIT( + std::optional, core::proto::OptionalG0Isometry3F64); +FARM_PROTO_CONV_TRAIT( + std::vector, core::proto::RepeatedG0Isometry3F64); } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/lie/conv_test.cpp b/cpp/farm_ng/core/proto_conv/lie/conv_test.cpp index 08dbae26..691f05a7 100644 --- a/cpp/farm_ng/core/proto_conv/lie/conv_test.cpp +++ b/cpp/farm_ng/core/proto_conv/lie/conv_test.cpp @@ -16,7 +16,7 @@ #include -using namespace sophus; +using namespace sophus2; using namespace farm_ng; using namespace farm_ng::core; @@ -31,7 +31,7 @@ TEST(conv_quat, unit) { EXPECT_EQ(proto.imag().y(), 3); EXPECT_EQ(proto.imag().z(), 4); auto maybe_q2 = fromProt(proto); - sophus::QuaternionF64 q2 = FARM_UNWRAP(maybe_q2); + sophus2::QuaternionF64 q2 = FARM_UNWRAP(maybe_q2); EXPECT_EQ(q2.real(), q.real()); EXPECT_EQ(q2.imag(), q.imag()); } @@ -44,7 +44,7 @@ TEST(conv_quat, unit) { EXPECT_EQ(proto.imag().y(), 0); EXPECT_EQ(proto.imag().z(), 0); auto maybe_q2 = fromProt(proto); - sophus::QuaternionF64 q2 = FARM_UNWRAP(maybe_q2); + sophus2::QuaternionF64 q2 = FARM_UNWRAP(maybe_q2); EXPECT_EQ(q2.real(), q.real()); EXPECT_EQ(q2.imag(), q.imag()); } @@ -56,12 +56,12 @@ TEST(conv_rot, unit) { proto::Rotation2F64 proto = toProt(q); EXPECT_EQ(proto.theta(), 1); auto maybe_q2 = fromProt(proto); - sophus::Rotation2F64 q2 = FARM_UNWRAP(maybe_q2); + sophus2::Rotation2F64 q2 = FARM_UNWRAP(maybe_q2); EXPECT_EQ(q2.angle(), q.angle()); } { - auto q = sophus::Rotation3F64::elementExamples().at(1); + auto q = sophus2::Rotation3F64::elementExamples().at(1); proto::Rotation3F64 proto = toProt(q); EXPECT_EQ(proto.unit_quaternion().real(), q.unitQuaternion().real()); EXPECT_EQ( @@ -72,7 +72,7 @@ TEST(conv_rot, unit) { proto.unit_quaternion().imag().z(), q.unitQuaternion().imag().z()); auto maybe_q2 = fromProt(proto); - sophus::Rotation3F64 q2 = FARM_UNWRAP(maybe_q2); + sophus2::Rotation3F64 q2 = FARM_UNWRAP(maybe_q2); EXPECT_EQ(q2.unitQuaternion().real(), q.unitQuaternion().real()); EXPECT_EQ(q2.unitQuaternion().imag(), q.unitQuaternion().imag()); EXPECT_EQ(q2.params(), q.params()); @@ -87,20 +87,20 @@ TEST(conv_rot, unit) { TEST(conv_isometry, unit) { { - auto iso = sophus::Isometry2F64::elementExamples().at(1); + auto iso = sophus2::Isometry2F64::elementExamples().at(1); proto::Isometry2F64 proto = toProt(iso); EXPECT_EQ(proto.translation().x(), iso.translation().x()); EXPECT_EQ(proto.translation().y(), iso.translation().y()); EXPECT_EQ(proto.rotation().theta(), iso.rotation().angle()); auto maybe_iso2 = fromProt(proto); - sophus::Isometry2F64 iso2 = FARM_UNWRAP(maybe_iso2); + sophus2::Isometry2F64 iso2 = FARM_UNWRAP(maybe_iso2); EXPECT_EQ(iso2.translation(), iso.translation()); EXPECT_EQ(iso2.rotation().angle(), iso.rotation().angle()); } { - auto iso = sophus::Isometry3F64::elementExamples().at(1); + auto iso = sophus2::Isometry3F64::elementExamples().at(1); proto::Isometry3F64 proto = toProt(iso); EXPECT_EQ(proto.translation().x(), iso.translation().x()); EXPECT_EQ(proto.translation().y(), iso.translation().y()); @@ -119,7 +119,7 @@ TEST(conv_isometry, unit) { iso.rotation().unitQuaternion().imag().z()); auto maybe_iso2 = fromProt(proto); - sophus::Isometry3F64 iso2 = FARM_UNWRAP(maybe_iso2); + sophus2::Isometry3F64 iso2 = FARM_UNWRAP(maybe_iso2); EXPECT_EQ(iso2.compactMatrix(), iso.compactMatrix()); } } diff --git a/cpp/farm_ng/core/proto_conv/linalg/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/linalg/CMakeLists.txt index b23e7b41..a3b0b54f 100644 --- a/cpp/farm_ng/core/proto_conv/linalg/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/linalg/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_linalg NAMESPACE farm_ng_core - INCLUDE_DIR ../../.. + INCLUDE_DIR ../../../.. HEADERS conv.h SOURCES @@ -8,7 +8,7 @@ farm_ng_add_library(farm_ng_core_proto_conv_linalg ) target_link_libraries(farm_ng_core_proto_conv_linalg PUBLIC - Sophus::sophus_linalg + farm_ng_core::sophus2_linalg protobuf::libprotobuf farm_ng_core::farm_ng_core_proto_defs farm_ng_core::farm_ng_core_prototools diff --git a/cpp/farm_ng/core/proto_conv/plotting/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/plotting/CMakeLists.txt index 531152ec..3942336a 100644 --- a/cpp/farm_ng/core/proto_conv/plotting/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/plotting/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_plotting NAMESPACE farm_ng_core - INCLUDE_DIR ../../.. + INCLUDE_DIR ../../../.. HEADERS conv.h SOURCES @@ -8,7 +8,7 @@ farm_ng_add_library(farm_ng_core_proto_conv_plotting ) target_link_libraries(farm_ng_core_proto_conv_plotting PUBLIC - Sophus::sophus_color + farm_ng_core::sophus2_color protobuf::libprotobuf farm_ng_core::farm_ng_core_proto_defs farm_ng_core::farm_ng_core_proto_conv_color diff --git a/cpp/farm_ng/core/proto_conv/plotting/conv.cpp b/cpp/farm_ng/core/proto_conv/plotting/conv.cpp index 00282670..6fb717f6 100644 --- a/cpp/farm_ng/core/proto_conv/plotting/conv.cpp +++ b/cpp/farm_ng/core/proto_conv/plotting/conv.cpp @@ -19,7 +19,7 @@ #include "farm_ng/core/proto_conv/color/conv.h" #include "farm_ng/core/proto_conv/linalg/conv.h" #include "farm_ng/core/proto_conv/std/conv.h" -#include "farm_ng/core/proto_conv/std/conv_impl_macro.ipp" +#include "farm_ng/core/proto_conv/std/repeated_message_impl_macro.ipp" #include "farm_ng/core/proto_conv/struct/conv_impl_macro.ipp" namespace farm_ng { @@ -41,22 +41,7 @@ auto toProt(plotting::ClearCondition const& v) return proto; } -template <> -auto fromProt( - core::plotting::proto::LineType const& proto) - -> Expected { - plotting::LineType line_type; - FARM_TRY_ASSERT(trySetFromString(line_type, proto.variant_name())); - return line_type; -} - -template <> -auto toProt(plotting::LineType const& v) - -> core::plotting::proto::LineType { - core::plotting::proto::LineType proto; - proto.set_variant_name(toString(v)); - return proto; -} +FARM_PROTO_CONV_ENUM_IMPL(plotting::LineType, core::plotting::proto::LineType); template <> auto fromProt( @@ -139,8 +124,9 @@ auto toProt>(std::deque const& v) FARM_PROTO_CONV_IMPL( plotting::ColoredRect, core::plotting::proto::ColoredRect, (color, region)); -FARM_CONV_IMPL_REPEATED( - core::plotting::proto::RepeatedG0ColoredRect, plotting::ColoredRect); +FARM_CONV_IMPL_REPEATED_MESSAGE( + core::plotting::proto::RepeatedG0ColoredRect, + std::deque); FARM_PROTO_CONV_IMPL( plotting::RectPlot, diff --git a/cpp/farm_ng/core/proto_conv/sensor/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/sensor/CMakeLists.txt index bdcf010a..369a0f81 100644 --- a/cpp/farm_ng/core/proto_conv/sensor/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/sensor/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_sensor NAMESPACE farm_ng_core -INCLUDE_DIR ../../.. +INCLUDE_DIR ../../../.. HEADERS conv.h SOURCES @@ -8,7 +8,7 @@ SOURCES ) target_link_libraries(farm_ng_core_proto_conv_sensor PUBLIC - Sophus::sophus_sensor + farm_ng_core::sophus2_sensor protobuf::libprotobuf farm_ng_core::farm_ng_core_prototools farm_ng_core::farm_ng_core_proto_conv_image diff --git a/cpp/farm_ng/core/proto_conv/sensor/conv.cpp b/cpp/farm_ng/core/proto_conv/sensor/conv.cpp index d722bc4d..8bedb291 100644 --- a/cpp/farm_ng/core/proto_conv/sensor/conv.cpp +++ b/cpp/farm_ng/core/proto_conv/sensor/conv.cpp @@ -21,7 +21,7 @@ namespace farm_ng { template <> auto fromProt(core::proto::CameraModel const& proto) - -> Expected { + -> Expected { auto get_params = [&proto]() -> Eigen::VectorXd { Eigen::VectorXd params(proto.params_size()); for (int i = 0; i < params.rows(); ++i) { @@ -29,18 +29,18 @@ auto fromProt(core::proto::CameraModel const& proto) } return params; }; - sophus::CameraDistortionType model = sophus::CameraDistortionType::pinhole; + sophus2::CameraDistortionType model = sophus2::CameraDistortionType::pinhole; FARM_TRY_ASSERT( trySetFromString(model, proto.distortion_type()), "distortion type not supported: {}", proto.distortion_type()); FARM_TRY(auto, size, fromProt(proto.image_size())); - return sophus::CameraModel(size, model, get_params()); + return sophus2::CameraModel(size, model, get_params()); } template <> -auto toProt(sophus::CameraModel const& camera_model) +auto toProt(sophus2::CameraModel const& camera_model) -> core::proto::CameraModel { core::proto::CameraModel proto; *proto.mutable_image_size() = toProt(camera_model.imageSize()); @@ -54,18 +54,18 @@ auto toProt(sophus::CameraModel const& camera_model) template <> auto fromProt(core::proto::CameraModels const& proto) - -> Expected> { - std::vector models; + -> Expected> { + std::vector models; for (int i = 0; i < proto.camera_models_size(); ++i) { - SOPHUS_TRY(sophus::CameraModel, cam, fromProt(proto.camera_models(i))); + SOPHUS_TRY(sophus2::CameraModel, cam, fromProt(proto.camera_models(i))); models.push_back(cam); } return models; } template <> -auto toProt>( - std::vector const& camera_models) +auto toProt>( + std::vector const& camera_models) -> core::proto::CameraModels { core::proto::CameraModels proto; for (auto const& model : camera_models) { @@ -76,8 +76,8 @@ auto toProt>( template <> auto fromProt(core::proto::RigidCamera const& proto) - -> Expected { - sophus::RigidCamera s; + -> Expected { + sophus2::RigidCamera s; SOPHUS_TRY(auto, intrinsics, fromProt(proto.intrinsics())); SOPHUS_TRY(auto, extrinsics, fromProt(proto.rig_from_camera())); s.intrinsics = intrinsics; @@ -86,7 +86,7 @@ auto fromProt(core::proto::RigidCamera const& proto) } template <> -auto toProt(sophus::RigidCamera const& s) +auto toProt(sophus2::RigidCamera const& s) -> core::proto::RigidCamera { core::proto::RigidCamera proto; *proto.mutable_intrinsics() = toProt(s.intrinsics); @@ -94,4 +94,26 @@ auto toProt(sophus::RigidCamera const& s) return proto; } +template <> +auto fromProt( + core::proto::MultiCameraRig const& proto) + -> Expected { + sophus2::MultiCameraRig cameras; + for (int i = 0; i < proto.cameras_size(); ++i) { + FARM_TRY(auto, cam, fromProt(proto.cameras(i))); + cameras.push_back(cam); + } + return cameras; +} + +template <> +auto toProt(sophus2::MultiCameraRig const& cameras) + -> core::proto::MultiCameraRig { + core::proto::MultiCameraRig proto; + for (auto const& cam : cameras) { + *proto.add_cameras() = toProt(cam); + } + return proto; +} + } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/sensor/conv.h b/cpp/farm_ng/core/proto_conv/sensor/conv.h index ca85cfd6..7d3f50e9 100644 --- a/cpp/farm_ng/core/proto_conv/sensor/conv.h +++ b/cpp/farm_ng/core/proto_conv/sensor/conv.h @@ -16,14 +16,15 @@ #include "farm_ng/core/proto_conv/traits.h" #include "farm_ng/core/sensor.pb.h" -#include "sophus/sensor/camera_model.h" -#include "sophus/sensor/camera_rig.h" +#include "sophus2/sensor/camera_model.h" +#include "sophus2/sensor/camera_rig.h" namespace farm_ng { -FARM_PROTO_CONV_TRAIT(sophus::CameraModel, core::proto::CameraModel); +FARM_PROTO_CONV_TRAIT(sophus2::CameraModel, core::proto::CameraModel); FARM_PROTO_CONV_TRAIT( - std::vector, core::proto::CameraModels); -FARM_PROTO_CONV_TRAIT(sophus::RigidCamera, core::proto::RigidCamera); + std::vector, core::proto::CameraModels); +FARM_PROTO_CONV_TRAIT(sophus2::RigidCamera, core::proto::RigidCamera); +FARM_PROTO_CONV_TRAIT(sophus2::MultiCameraRig, core::proto::MultiCameraRig); } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/sensor/conv_test.cpp b/cpp/farm_ng/core/proto_conv/sensor/conv_test.cpp index bc9f1cfc..d8531b97 100644 --- a/cpp/farm_ng/core/proto_conv/sensor/conv_test.cpp +++ b/cpp/farm_ng/core/proto_conv/sensor/conv_test.cpp @@ -16,14 +16,14 @@ #include -using namespace sophus; +using namespace sophus2; using namespace farm_ng; using namespace farm_ng::core; TEST(conv_sensor, unit) { { - sophus::CameraModel pinhole = createDefaultPinholeModel({64, 24}); + sophus2::CameraModel pinhole = createDefaultPinholeModel({64, 24}); proto::CameraModel proto = toProt(pinhole); EXPECT_EQ(proto.image_size().width(), 64); EXPECT_EQ(proto.image_size().height(), 24); @@ -34,21 +34,21 @@ TEST(conv_sensor, unit) { } auto maybe_pinhole2 = fromProt(proto); - sophus::CameraModel pinhole2 = FARM_UNWRAP(maybe_pinhole2); + sophus2::CameraModel pinhole2 = FARM_UNWRAP(maybe_pinhole2); EXPECT_EQ(pinhole2.imageSize(), pinhole.imageSize()); ASSERT_EQ(pinhole2.params(), pinhole.params()); EXPECT_EQ(pinhole2.distortionType(), pinhole.distortionType()); } { - std::vector models; + std::vector models; models.push_back(createDefaultPinholeModel({64, 24})); models.push_back(createDefaultOrthoModel({128, 48})); proto::CameraModels proto = toProt(models); ASSERT_EQ(proto.camera_models_size(), models.size()); auto maybe_models2 = fromProt(proto); - std::vector models2 = FARM_UNWRAP(maybe_models2); + std::vector models2 = FARM_UNWRAP(maybe_models2); ASSERT_EQ(models2.size(), models.size()); for (size_t i = 0; i < models.size(); ++i) { EXPECT_EQ(models2[i].imageSize(), models[i].imageSize()); diff --git a/cpp/farm_ng/core/proto_conv/std/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/std/CMakeLists.txt index 64151bd6..33ef873d 100644 --- a/cpp/farm_ng/core/proto_conv/std/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/std/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_std NAMESPACE farm_ng_core - INCLUDE_DIR ../../.. + INCLUDE_DIR ../../../.. HEADERS conv.h SOURCES diff --git a/cpp/farm_ng/core/proto_conv/std/conv.cpp b/cpp/farm_ng/core/proto_conv/std/conv.cpp index be43f1c1..f4f1388d 100644 --- a/cpp/farm_ng/core/proto_conv/std/conv.cpp +++ b/cpp/farm_ng/core/proto_conv/std/conv.cpp @@ -13,19 +13,23 @@ // limitations under the License. #include "farm_ng/core/proto_conv/std/conv.h" -#include "farm_ng/core/proto_conv/std/conv_impl_macro.ipp" +#include "farm_ng/core/proto_conv/lie/conv.h" +#include "farm_ng/core/proto_conv/std/optional_primitive_impl_macro.ipp" +#include "farm_ng/core/proto_conv/std/repeated_primitive_impl_macro.ipp" namespace farm_ng { -FARM_PP_SEQ_FOR_EACH( - FARM_CONV_IMPL_OPTIONAL_ONE_ITER, - _, - ((core::proto::OptionalG0Float, float)) // - ((core::proto::OptionalG0Double, double)) // - ((core::proto::OptionalG0Int32, int32_t)) // - ((core::proto::OptionalG0Int64, int64_t)) // - ((core::proto::OptionalG0String, std::string)) // - ((core::proto::OptionalG0Bool, bool))); +FARM_CONV_IMPL_PRIMITIVE_OPTIONAL(core::proto::OptionalG0Float, float); +FARM_CONV_IMPL_PRIMITIVE_OPTIONAL(core::proto::OptionalG0Double, double); +FARM_CONV_IMPL_PRIMITIVE_OPTIONAL(core::proto::OptionalG0Int32, int32_t); +FARM_CONV_IMPL_PRIMITIVE_OPTIONAL(core::proto::OptionalG0Int64, int64_t); +FARM_CONV_IMPL_PRIMITIVE_OPTIONAL(core::proto::OptionalG0String, std::string); +FARM_CONV_IMPL_PRIMITIVE_OPTIONAL(core::proto::OptionalG0Bool, bool); + +FARM_CONV_IMPL_REPEATED_PRIMITIVE( + core::proto::RepeatedG0Float, std::vector); +FARM_CONV_IMPL_REPEATED_PRIMITIVE( + core::proto::RepeatedG0UInt32, std::vector); template <> auto fromProt( diff --git a/cpp/farm_ng/core/proto_conv/std/conv.h b/cpp/farm_ng/core/proto_conv/std/conv.h index de3a6eb0..29346208 100644 --- a/cpp/farm_ng/core/proto_conv/std/conv.h +++ b/cpp/farm_ng/core/proto_conv/std/conv.h @@ -22,6 +22,7 @@ namespace farm_ng { +// optional of primitive types FARM_PROTO_CONV_TRAIT(std::optional, core::proto::OptionalG0Float); FARM_PROTO_CONV_TRAIT(std::optional, core::proto::OptionalG0Double); FARM_PROTO_CONV_TRAIT(std::optional, core::proto::OptionalG0Int32); @@ -30,6 +31,10 @@ FARM_PROTO_CONV_TRAIT( std::optional, core::proto::OptionalG0String); FARM_PROTO_CONV_TRAIT(std::optional, core::proto::OptionalG0Bool); +// std::vector of primitive types +FARM_PROTO_CONV_TRAIT(std::vector, core::proto::RepeatedG0Float); +FARM_PROTO_CONV_TRAIT(std::vector, core::proto::RepeatedG0UInt32); + FARM_PROTO_CONV_TRAIT(std::filesystem::path, core::proto::FileSystemPath); } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/std/optional_message_impl_macro.ipp b/cpp/farm_ng/core/proto_conv/std/optional_message_impl_macro.ipp new file mode 100644 index 00000000..73fedb65 --- /dev/null +++ b/cpp/farm_ng/core/proto_conv/std/optional_message_impl_macro.ipp @@ -0,0 +1,55 @@ +// Copyright 2022, farm-ng inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +// NOTE: This file should typically only be included in *.cpp files - to keep +// compile times low. + +#include "farm_ng/core/proto_conv/std/conv.h" + +#include + +#include + +namespace farm_ng { + +#define FARM_CONV_IMPL_MESSAGE_OPTIONAL(Proto_Type_, Cpp_Type_) \ + template <> \ + auto fromProt(Proto_Type_ const& proto) \ + -> Expected> { \ + std::optional v; \ + if (proto.has_message()) { \ + FARM_TRY(auto, vv, fromProt(proto.value())); \ + v = vv; \ + } else { \ + v = std::nullopt; \ + } \ + return v; \ + } \ + \ + template <> \ + auto toProt>(std::optional const& v) \ + -> Proto_Type_ { \ + Proto_Type_ proto; \ + if (v.has_value()) { \ + *proto.mutable_value() = toProt(v.value()); \ + proto.set_has_message(true); \ + } else { \ + proto.set_has_message(false); \ + } \ + return proto; \ + } + +} // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/std/conv_impl_macro.ipp b/cpp/farm_ng/core/proto_conv/std/optional_primitive_impl_macro.ipp similarity index 59% rename from cpp/farm_ng/core/proto_conv/std/conv_impl_macro.ipp rename to cpp/farm_ng/core/proto_conv/std/optional_primitive_impl_macro.ipp index 9dbd2793..b41570e9 100644 --- a/cpp/farm_ng/core/proto_conv/std/conv_impl_macro.ipp +++ b/cpp/farm_ng/core/proto_conv/std/optional_primitive_impl_macro.ipp @@ -25,7 +25,7 @@ namespace farm_ng { -#define FARM_CONV_IMPL_OPTIONAL(Proto_Type_, Cpp_Type_) \ +#define FARM_CONV_IMPL_PRIMITIVE_OPTIONAL(Proto_Type_, Cpp_Type_) \ template <> \ auto fromProt(Proto_Type_ const& proto) \ -> Expected> { \ @@ -51,33 +51,4 @@ namespace farm_ng { return proto; \ } -// Proxy for FARM_CONV_IMPL_OPTIONAL to make it callable from -// FARM_PP_SEQ_FOR_EACH. -#define FARM_CONV_IMPL_OPTIONAL_ONE_ITER(D0_, D1_, Proto_Cpp_Pair_) \ - FARM_CONV_IMPL_OPTIONAL( \ - FARM_PP_TUPLE_ELEM(0, Proto_Cpp_Pair_), \ - FARM_PP_TUPLE_ELEM(1, Proto_Cpp_Pair_)) - -#define FARM_CONV_IMPL_REPEATED(Proto_Type_, Cpp_Type_) \ - template <> \ - auto fromProt(Proto_Type_ const& proto) \ - -> Expected> { \ - std::deque v; \ - for (int i = 0; i < proto.value_size(); ++i) { \ - FARM_TRY(auto, value, fromProt(proto.value(i))); \ - v.push_back(value); \ - } \ - return v; \ - } \ - \ - template <> \ - auto toProt>(std::deque const& v) \ - -> Proto_Type_ { \ - Proto_Type_ proto; \ - for (auto const& value : v) { \ - *proto.add_value() = toProt(value); \ - } \ - return proto; \ - } - } // namespace farm_ng diff --git a/cpp/farm_ng/core/proto_conv/std/repeated_message_impl_macro.ipp b/cpp/farm_ng/core/proto_conv/std/repeated_message_impl_macro.ipp new file mode 100644 index 00000000..14686bc8 --- /dev/null +++ b/cpp/farm_ng/core/proto_conv/std/repeated_message_impl_macro.ipp @@ -0,0 +1,35 @@ +// Copyright 2022, farm-ng inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#define FARM_CONV_IMPL_REPEATED_MESSAGE(Proto_Type_, Cpp_Type_Container_) \ + template <> \ + auto fromProt(Proto_Type_ const& proto) \ + -> Expected { \ + Cpp_Type_Container_ v; \ + for (int i = 0; i < proto.value_size(); ++i) { \ + FARM_TRY(auto, value, fromProt(proto.value(i))); \ + v.push_back(value); \ + } \ + return v; \ + } \ + \ + template <> \ + auto toProt(Cpp_Type_Container_ const& v) \ + -> Proto_Type_ { \ + Proto_Type_ proto; \ + for (auto const& value : v) { \ + *proto.add_value() = toProt(value); \ + } \ + return proto; \ + } diff --git a/cpp/farm_ng/core/proto_conv/std/repeated_primitive_impl_macro.ipp b/cpp/farm_ng/core/proto_conv/std/repeated_primitive_impl_macro.ipp new file mode 100644 index 00000000..dd4eab07 --- /dev/null +++ b/cpp/farm_ng/core/proto_conv/std/repeated_primitive_impl_macro.ipp @@ -0,0 +1,34 @@ +// Copyright 2022, farm-ng inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#define FARM_CONV_IMPL_REPEATED_PRIMITIVE(Proto_Type_, Cpp_Type_Container_) \ + template <> \ + auto fromProt(Proto_Type_ const& proto) \ + -> Expected { \ + Cpp_Type_Container_ v; \ + for (int i = 0; i < proto.value_size(); ++i) { \ + v.push_back(proto.value(i)); \ + } \ + return v; \ + } \ + \ + template <> \ + auto toProt(Cpp_Type_Container_ const& v) \ + -> Proto_Type_ { \ + Proto_Type_ proto; \ + for (auto const& value : v) { \ + proto.add_value(value); \ + } \ + return proto; \ + } diff --git a/cpp/farm_ng/core/proto_conv/struct/CMakeLists.txt b/cpp/farm_ng/core/proto_conv/struct/CMakeLists.txt index ab3ccf57..92fad076 100644 --- a/cpp/farm_ng/core/proto_conv/struct/CMakeLists.txt +++ b/cpp/farm_ng/core/proto_conv/struct/CMakeLists.txt @@ -1,6 +1,6 @@ farm_ng_add_library(farm_ng_core_proto_conv_struct NAMESPACE farm_ng_core -INCLUDE_DIR ../../.. +INCLUDE_DIR ../../../.. HEADERS conv_impl_macro.ipp SOURCES diff --git a/cpp/farm_ng/core/proto_conv/struct/conv_impl_macro.ipp b/cpp/farm_ng/core/proto_conv/struct/conv_impl_macro.ipp index db73db03..3dbcc65b 100644 --- a/cpp/farm_ng/core/proto_conv/struct/conv_impl_macro.ipp +++ b/cpp/farm_ng/core/proto_conv/struct/conv_impl_macro.ipp @@ -40,7 +40,7 @@ // tuple: (Type0, name0, {}) // // Output: static_assert(TCpp::kFieldNames[i] == "name0"); -#define FARM_STRUCT_DETAILS_ASSERT_FIELD_NAME( \ +#define FARM_STRUCT_DETAILS_ASSERT_FIELD_NAME( \ Dummy_, Field_Names_, Index_, Type_Name_Init_) \ static_assert( \ Field_Names_[Index_] == \ @@ -193,13 +193,12 @@ struct ToProtImpl; }; \ \ template <> \ - auto fromProt(Proto_Type_ const& proto) \ - -> Expected { \ + auto fromProt(Proto_Type_ const& proto)->Expected { \ return FromProtImpl::impl(proto); \ } \ \ template <> \ - auto toProt(Cpp_Type_ const& s) -> Proto_Type_ { \ + auto toProt(Cpp_Type_ const& s)->Proto_Type_ { \ return ToProtImpl::impl(s); \ } @@ -275,3 +274,23 @@ struct ToProtImpl; }, \ FARM_PP_TUPLE_SIZE(Tuple_Of_Fields_), \ FARM_MACROLIB_SEQ_OF_TUPLES_FROM_TUPLE(Tuple_Of_Fields_)) + +// Given a FARM_ENUM-defined C++ enum and a Proto type, create from-proto and +// to-proto conversion code. +// +// Assumes the proto-representation of the enum is: +// message FooEnum { string variant_name = 1; } +#define FARM_PROTO_CONV_ENUM_IMPL(Cpp_Type_, Proto_Type_) \ + template <> \ + auto fromProt(Proto_Type_ const& proto)->Expected { \ + Cpp_Type_ v; \ + FARM_TRY_ASSERT(trySetFromString(v, proto.variant_name())); \ + return v; \ + } \ + \ + template <> \ + auto toProt(Cpp_Type_ const& v)->Proto_Type_ { \ + Proto_Type_ proto; \ + proto.set_variant_name(toString(v)); \ + return proto; \ + } diff --git a/cpp/farm_ng/core/proto_conv/struct/conv_impl_macro_test.cpp b/cpp/farm_ng/core/proto_conv/struct/conv_impl_macro_test.cpp index 3094fb61..245a5170 100644 --- a/cpp/farm_ng/core/proto_conv/struct/conv_impl_macro_test.cpp +++ b/cpp/farm_ng/core/proto_conv/struct/conv_impl_macro_test.cpp @@ -26,17 +26,16 @@ namespace farm_ng { FARM_STRUCT(StructExample1, ((int, integer, {1}), (float, f, {0.5}))); FARM_STRUCT( StructExample2, ((std::string, name, {}), (StructExample1, ex1, {}))); +FARM_ENUM(EnumExample1, (a, b, c)); -// this typically goes in proto_conv.h file +// these typically go in proto_conv.h file FARM_PROTO_CONV_TRAIT(StructExample1, core::proto::StructExample1); +FARM_PROTO_CONV_TRAIT(StructExample2, core::proto::StructExample2); +FARM_PROTO_CONV_TRAIT(EnumExample1, core::proto::EnumExample1); -// this typically goes in proto_conv.cc file +// these typically go in proto_conv.cc file FARM_PROTO_CONV_IMPL(StructExample1, core::proto::StructExample1, (integer, f)); -// this typically goes in proto_conv.h file -FARM_PROTO_CONV_TRAIT(StructExample2, core::proto::StructExample2); - -// this typically goes in proto_conv.cc file FARM_PROTO_CONV_IMPL_FN( StructExample2, core::proto::StructExample2, @@ -47,8 +46,27 @@ FARM_PROTO_CONV_IMPL_FN( return std::move(proto); }); +FARM_PROTO_CONV_ENUM_IMPL(EnumExample1, core::proto::EnumExample1); + } // namespace farm_ng +TEST(FARM_PROTO_CONV_ENUM_IMPL, roundtrip) { + using namespace farm_ng; + auto value = EnumExample1::b; + auto proto = toProt(value); + auto maybe_value = fromProt(proto); + ASSERT_TRUE(maybe_value.has_value()); + EXPECT_EQ(FARM_UNWRAP(maybe_value), value); +} + +TEST(FARM_PROTO_CONV_ENUM_IMPL, invalid_proto) { + using namespace farm_ng; + core::proto::EnumExample1 invalid_proto; + invalid_proto.set_variant_name("invalid"); + auto maybe_value = fromProt(invalid_proto); + EXPECT_FALSE(maybe_value.has_value()); +} + TEST(FARM_STRUCT_DETAILS_FROM_PROTO, macro) { // clang-format off std::string expected_string = diff --git a/cpp/farm_ng/core/prototools/proto_reader_writer.h b/cpp/farm_ng/core/prototools/proto_reader_writer.h index e0ec6df8..2a8890d4 100644 --- a/cpp/farm_ng/core/prototools/proto_reader_writer.h +++ b/cpp/farm_ng/core/prototools/proto_reader_writer.h @@ -45,7 +45,8 @@ Expected readProtobufFromJsonFile( auto status = google::protobuf::util::JsonStringToMessage( json_string, &message, parse_options); if (!status.ok()) { - return FARM_UNEXPECTED("Failed to parse json string: {}", status); + return FARM_UNEXPECTED( + "Failed to parse json string: {}", status.ToString()); } return message; } diff --git a/cpp/farm_ng/core/struct/CMakeLists.txt b/cpp/farm_ng/core/struct/CMakeLists.txt index 63f523bf..a1c84359 100644 --- a/cpp/farm_ng/core/struct/CMakeLists.txt +++ b/cpp/farm_ng/core/struct/CMakeLists.txt @@ -24,9 +24,9 @@ farm_ng_add_library(farm_ng_core_struct NAMESPACE farm_ng_core INCLUDE_DIR ../../.. HEADERS - base - macro_lib - struct + base.h + macro_lib.h + struct.h ) target_link_libraries(farm_ng_core_struct diff --git a/cpp/farm_ng/core/struct/base.h b/cpp/farm_ng/core/struct/base.h index dd685d3a..99a9727c 100644 --- a/cpp/farm_ng/core/struct/base.h +++ b/cpp/farm_ng/core/struct/base.h @@ -87,6 +87,43 @@ #define FARM_STRUCT_DETAILS_FIELD_TYPE(Dummy1_, Dummy2_, Type_Name_Init_) \ , FARM_PP_TUPLE_ELEM(0, Type_Name_Init_) +/// Given a triplet such as (Type, name, init), return assignment as shown +/// below. +/// +/// ${triplet} +/// +/// => s.${triplet}[1] = std::move(std::get(tuple)); +/// +/// Example: +/// +/// Input: (Type0, name0, {init0}) +/// Output: s.name0 = std::move(std::get(tuple)); +#define FARM_STRUCT_DETAILS_SET_FIELD_FROM_TUPLE( \ + Dummy1_, Dummy2_, index, Type_Name_Init_) \ + s.FARM_PP_TUPLE_ELEM(1, Type_Name_Init_) = std::move(std::get(tuple)); + +/// Given a sequence of triplets, create a sequence of assignments +/// +/// Example: +/// +/// Input: ((Type0, name0, init0)) ((Type1, name1, init1)) ... Output: , +/// +/// Output: +/// s.name0 = std::move(std::get(tuple)); +/// s.name1 = std::move(std::get(tuple)); +/// ... +#define FARM_STRUCT_DETAILS_SET_FIELD_FROM_TUPLE_LOOP(Field_Seq_) \ + FARM_PP_SEQ_FOR_EACH_I( \ + FARM_STRUCT_DETAILS_SET_FIELD_FROM_TUPLE, _, Field_Seq_) + +#define FARM_STRUCT_DETAILS_TO_TUPLE(Dummy1_, Dummy2_, index, Type_Name_Init_) \ + , this->FARM_PP_TUPLE_ELEM(1, Type_Name_Init_) + +#define FARM_STRUCT_DETAILS_TO_TUPLE_LOOP(Field_Seq_) \ + this->FARM_PP_TUPLE_ELEM(1, FARM_PP_SEQ_ELEM(0, Field_Seq_)) \ + FARM_PP_SEQ_FOR_EACH_I( \ + FARM_STRUCT_DETAILS_TO_TUPLE, _, FARM_PP_SEQ_POP_FRONT(Field_Seq_)) + /// Given a sequence of triplets, create a comma separated list of field types. /// /// Example: @@ -99,10 +136,20 @@ FARM_STRUCT_DETAILS_FIELD_TYPE, _, FARM_PP_SEQ_POP_FRONT(Field_Seq_)) /// Implementation details for FARM_STRUCT. -#define FARM_STRUCT_DETAILS_BASE(Num_Fields_, Field_Seq_) \ +#define FARM_STRUCT_DETAILS_BASE(Struct_Name_, Num_Fields_, Field_Seq_) \ static int constexpr kNumFields = Num_Fields_; \ static std::array constexpr kFieldNames = { \ FARM_STRUCT_DETAILS_FIELD_NAME_LOOP(Field_Seq_)}; \ using FieldTypes = \ std::tuple; \ + \ + static Struct_Name_ fromTuple(FieldTypes&& tuple) noexcept { \ + Struct_Name_ s; \ + FARM_STRUCT_DETAILS_SET_FIELD_FROM_TUPLE_LOOP(Field_Seq_) \ + return s; \ + } \ + \ + auto toTuple() const noexcept->FieldTypes { \ + return std::make_tuple(FARM_STRUCT_DETAILS_TO_TUPLE_LOOP(Field_Seq_)); \ + } \ FARM_STRUCT_DETAILS_FIELD_DECLARATION_LOOP(Field_Seq_) diff --git a/cpp/farm_ng/core/struct/base_test.cpp b/cpp/farm_ng/core/struct/base_test.cpp index e66287e5..f7cfbf2b 100644 --- a/cpp/farm_ng/core/struct/base_test.cpp +++ b/cpp/farm_ng/core/struct/base_test.cpp @@ -23,11 +23,20 @@ TEST(struct_test, unit) { "static int constexpr kNumFields = 2; " "static std::array constexpr kFieldNames = { \"i\" , \"d\" }; " "using FieldTypes = std::tuple; " + "static Foo fromTuple(FieldTypes&& tuple) noexcept { " + "Foo s; " + "s.i = std::move(std::get<0>(tuple)); " + "s.d = std::move(std::get<1>(tuple)); " + "return s; " + "} " + "auto toTuple() const noexcept -> FieldTypes { " + "return std::make_tuple(this->i , this->d ); " + "} " "int i {1}; " "double d {0.5};"); // clang-format on EXPECT_EQ( - FARM_PP_STRINGIZE( - FARM_STRUCT_DETAILS_BASE(2, ((int, i, {1}))((double, d, {0.5})))), + FARM_PP_STRINGIZE(FARM_STRUCT_DETAILS_BASE( + Foo, 2, ((int, i, {1}))((double, d, {0.5})))), expected_string); } diff --git a/cpp/farm_ng/core/struct/struct.h b/cpp/farm_ng/core/struct/struct.h index 9c7d2544..ebc21e23 100644 --- a/cpp/farm_ng/core/struct/struct.h +++ b/cpp/farm_ng/core/struct/struct.h @@ -19,7 +19,7 @@ /// Takes in a struct name and a sequence of fields and generates a struct. /// -/// This is an implementation details. User shall call FARM_STRUCT(StructName, +/// This is an implementation details. User shall call FARM_STRUCT(Struct_Name_, /// ((Type0, name0, init0), ...))); instead; /// /// This intermediate macro exists since the conversion from a list to a @@ -27,7 +27,7 @@ /// it. #define FARM_STRUCT_DETAIL_FROM_SEQ(Struct_Name_, Num_Fields_, Field_Seq_) \ struct Struct_Name_ { \ - FARM_STRUCT_DETAILS_BASE(Num_Fields_, Field_Seq_) \ + FARM_STRUCT_DETAILS_BASE(Struct_Name_, Num_Fields_, Field_Seq_) \ } /// Takes in a struct name and a tuple of fields and generates a struct. diff --git a/cpp/farm_ng/core/struct/struct_test.cpp b/cpp/farm_ng/core/struct/struct_test.cpp index 66df23bb..5d282d91 100644 --- a/cpp/farm_ng/core/struct/struct_test.cpp +++ b/cpp/farm_ng/core/struct/struct_test.cpp @@ -33,6 +33,15 @@ TEST(struct_test, unit) { "static std::array constexpr kFieldNames " "= { \"i\" , \"d\" }; " "using FieldTypes = std::tuple; " + "static Foo fromTuple(FieldTypes&& tuple) noexcept { " + "Foo s; " + "s.i = std::move(std::get<0>(tuple)); " + "s.d = std::move(std::get<1>(tuple)); " + "return s; " + "} " + "auto toTuple() const noexcept -> FieldTypes { " + "return std::make_tuple(this->i , this->d ); " + "} " "int i {1}; " "double d {0.5}; " "}"); @@ -40,4 +49,14 @@ TEST(struct_test, unit) { EXPECT_EQ( FARM_PP_STRINGIZE(FARM_STRUCT(Foo, ((int, i, {1}), (double, d, {0.5})))), expected_string); + + StructExample1 instance; + std::tuple tuple = instance.toTuple(); + FARM_ASSERT_EQ(std::get<0>(tuple), instance.integer); + FARM_ASSERT_EQ(std::get<1>(tuple), instance.f); + tuple = std::make_tuple(-1, 0.25); + StructExample1 instance2 = StructExample1::fromTuple(std::move(tuple)); + + FARM_ASSERT_EQ(std::get<0>(tuple), instance2.integer); + FARM_ASSERT_EQ(std::get<1>(tuple), instance2.f); } diff --git a/cpp/sophus/.clang-tidy b/cpp/sophus/.clang-tidy deleted file mode 100644 index 13da5e02..00000000 --- a/cpp/sophus/.clang-tidy +++ /dev/null @@ -1,84 +0,0 @@ -Checks: "-*, - readability-*, - -readability-magic-numbers, - -readability-function-cognitive-complexity, - cppcoreguidelines-*, - -cppcoreguidelines-macro-usage, - -cppcoreguidelines-avoid-*, - -cppcoreguidelines-narrowing-conversions, - -cppcoreguidelines-pro-*, - -cppcoreguidelines-slicing, - -cppcoreguidelines-special-member-functions, - -cppcoreguidelines-owning-memory, - -cppcoreguidelines-non-private-member-variables-in-classes, - bugprone-dynamic-static-initializers, - bugprone-easily-swappable-parameters, - bugprone-integer-division, - bugprone-use-after-move, - performance-noexcept-move-constructor, - performance-inefficient-algorithm, - modernize-make-unique, - modernize-make-shared, - modernize-use-bool-literals, - modernize-use-nodiscard, - modernize-use-using" -HeaderFilterRegex: .*/sophus/.* -CheckOptions: - - { key: readability-uppercase-literal-suffix.NewSuffixes, value: L } - - - { key: readability-identifier-naming.ClassCase, value: CamelCase } - - { key: readability-identifier-naming.StructCase, value: CamelCase } - - { - key: readability-identifier-naming.TemplateParameterCase, - value: CamelCase, - } - - { key: readability-identifier-naming.TemplateParameterPrefix, value: T } - - { - key: readability-identifier-naming.TemplateParameterIgnoredRegexp, - value: "expr-type", - } - - { key: readability-identifier-naming.TypedefCase, value: CamelCase } - - - { key: readability-identifier-naming.FunctionCase, value: camelBack } - - { key: readability-identifier-naming.MethodCase, value: camelBack } - - - { key: readability-identifier-naming.EnumConstantCase, value: lower_case } - - { key: readability-identifier-naming.MemberCase, value: lower_case } - - { key: readability-identifier-naming.NamespaceCase, value: lower_case } - - { key: readability-identifier-naming.ParameterCase, value: lower_case } - - { key: readability-identifier-naming.ParameterPackCase, value: lower_case } - - { - key: readability-identifier-naming.ScopedEnumConstantCase, - value: lower_case, - } - - { key: readability-identifier-naming.VariableCase, value: lower_case } - - { key: readability-identifier-naming.ConstantCase, value: lower_case } - - - { key: readability-identifier-naming.PrivateMemberSuffix, value: _ } - - { key: readability-identifier-naming.ProtectedMemberSuffix, value: _ } - - - { key: readability-identifier-naming.GlobalConstantCase, value: CamelCase } - - { key: readability-identifier-naming.GlobalConstantPrefix, value: k } - - { key: readability-identifier-naming.StaticConstantCase, value: CamelCase } - - { key: readability-identifier-naming.StaticConstantPrefix, value: k } - - { - key: readability-identifier-naming.ValueTemplateParameterCase, - value: CamelCase, - } - - { - key: readability-identifier-naming.ValueTemplateParameterPrefix, - value: k, - } - - - { key: readability-identifier-naming.GlobalVariablePrefix, value: Static_ } - - { - key: readability-identifier-naming.GlobalVariableCase, - value: Camel_Snake_Case, - } - - { key: readability-identifier-naming.StaticVariablePrefix, value: Static_ } - - { - key: readability-identifier-naming.StaticVariableCase, - value: Camel_Snake_Case, - } - - - { key: cppcoreguidelines-init-variables.MathHeader, value: } diff --git a/cpp/sophus/calculus/CMakeLists.txt b/cpp/sophus/calculus/CMakeLists.txt deleted file mode 100644 index 3923b6c6..00000000 --- a/cpp/sophus/calculus/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -#[[ -sophus_calculus - - - -]] - -farm_ng_add_library(sophus_calculus - NAMESPACE Sophus - INCLUDE_DIR ../../ - HEADERS - num_diff.h - region.h -) -target_link_libraries(sophus_calculus INTERFACE sophus_common) - -if(${BUILD_SOPHUS_TESTS}) - farm_ng_add_test(region - PARENT_LIBRARY sophus_calculus - LABELS small) -endif() diff --git a/cpp/sophus/color/CMakeLists.txt b/cpp/sophus/color/CMakeLists.txt deleted file mode 100644 index 3e4df80b..00000000 --- a/cpp/sophus/color/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -#[[ -sophus_color - - - -]] - -farm_ng_add_library(sophus_color - NAMESPACE Sophus - INCLUDE_DIR ../../ - HEADERS - color.h - SOURCES - color.cpp -) -target_link_libraries(sophus_color PUBLIC sophus_calculus) - -if(${BUILD_SOPHUS_TESTS}) - foreach(test_basename - color) - farm_ng_add_test(${test_basename} - PARENT_LIBRARY sophus_color - LINK_LIBRARIES sophus_color - LABELS small) - endforeach() -endif() diff --git a/cpp/sophus/concepts/division_ring.h b/cpp/sophus/concepts/division_ring.h deleted file mode 100644 index 058dc431..00000000 --- a/cpp/sophus/concepts/division_ring.h +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright (c) 2011, Hauke Strasdat -// Copyright (c) 2012, Steven Lovegrove -// Copyright (c) 2021, farm-ng, inc. -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -#pragma once -#include "sophus/concepts/params.h" -#include "sophus/concepts/utils.h" - -namespace sophus { -namespace concepts { - -template -class UnitVector; - -template -concept DivisionRingImpl = - ::sophus::concepts::ParamsImpl // or homogeneous point representation - && requires( - typename TT::Params params, - Eigen::Vector, TT::kNumParams> - compatible_params) { - // constructors and factories - { TT::one() } -> ::sophus::concepts::ConvertibleTo; - - { TT::zero() } -> ::sophus::concepts::ConvertibleTo; - - // operations - - { - TT::addition(params, params) - } -> ::sophus::concepts::ConvertibleTo; - - { - TT::multiplication(params, params) - } -> ::sophus::concepts::ConvertibleTo; - -#if __cplusplus >= 202002L - { - TT::template addition>( - params, compatible_params) - } -> ::sophus::concepts::ConvertibleTo>>; - - { - TT::template multiplication>( - params, compatible_params) - } -> ::sophus::concepts::ConvertibleTo>>; -#endif - - { - TT::conjugate(params) - } -> ::sophus::concepts::ConvertibleTo; - - { - TT::inverse(params) - } -> ::sophus::concepts::ConvertibleTo; - - // reduction - { - TT::norm(params) - } -> ::sophus::concepts::ConvertibleTo; - - { - TT::squaredNorm(params) - } -> ::sophus::concepts::ConvertibleTo; -}; - -template -concept DivisionRingConcept = DivisionRingImpl && requires( - TT r, - typename TT::Scalar real, - typename TT::Imag imag, - typename TT::Params params) { - // operations - { r.operator+(r) } -> ::sophus::concepts::ConvertibleTo; - - { r.operator*(r) } -> ::sophus::concepts::ConvertibleTo; - - { r.conjugate() } -> ::sophus::concepts::ConvertibleTo; - - { r.inverse() } -> ::sophus::concepts::ConvertibleTo; - - // reduction - { r.norm() } -> ::sophus::concepts::ConvertibleTo; - - { r.squaredNorm() } -> ::sophus::concepts::ConvertibleTo; - - { r.real() } -> ::sophus::concepts::ConvertibleTo; - - { r.imag() } -> ::sophus::concepts::ConvertibleTo; -}; -} // namespace concepts -} // namespace sophus diff --git a/cpp/sophus/image/CMakeLists.txt b/cpp/sophus/image/CMakeLists.txt deleted file mode 100644 index 559deb8a..00000000 --- a/cpp/sophus/image/CMakeLists.txt +++ /dev/null @@ -1,51 +0,0 @@ -#[[ -sophus_image - - - -]] - -set(sophus_image_src_prefixes - interpolation - image - image_size - layout - image_types - image_view - mut_image - mut_image_view - mut_dyn_image - mut_dyn_image_view - dyn_image - dyn_image_view - dyn_image_types - pixel_format -) - -set(sophus_image_cpp) -set(sophus_image_h) -foreach(x ${sophus_image_src_prefixes} ) - list(APPEND sophus_image_cpp ${x}.cpp) - list(APPEND sophus_image_h ${x}.h) -endforeach() - -farm_ng_add_library(sophus_image - NAMESPACE Sophus - INCLUDE_DIR ../../ - HEADERS - ${sophus_image_h} - SOURCES - ${sophus_image_cpp} -) -target_link_libraries(sophus_image sophus_geometry) - - - -if(${BUILD_SOPHUS_TESTS}) - foreach(test_basename ${sophus_image_src_prefixes}) - farm_ng_add_test(${test_basename} - PARENT_LIBRARY sophus_image - LINK_LIBRARIES sophus_image - LABELS small) - endforeach() -endif() diff --git a/cpp/sophus/interp/CMakeLists.txt b/cpp/sophus/interp/CMakeLists.txt deleted file mode 100644 index 31bbe6d9..00000000 --- a/cpp/sophus/interp/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -#[[ -sophus_interp - -]] - -farm_ng_add_library(sophus_interp - NAMESPACE Sophus - INCLUDE_DIR ../../../ - HEADERS - average.h - interpolate.h -) -target_link_libraries(sophus_interp INTERFACE sophus_lie) - -farm_ng_add_test(interpolate - PARENT_LIBRARY sophus_interp - LINK_LIBRARIES sophus_interp - LABELS small) - -add_subdirectory(spline) diff --git a/cpp/sophus/interp/spline/CMakeLists.txt b/cpp/sophus/interp/spline/CMakeLists.txt deleted file mode 100644 index 1f2e1da7..00000000 --- a/cpp/sophus/interp/spline/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -#[[ -sophus_interp_spline - -]] - -farm_ng_add_library(sophus_interp_spline - NAMESPACE Sophus - INCLUDE_DIR ../../../../ - HEADERS - bspline.h - group_bspline.h - details/bspline_segment.h - details/cubic_basis.h - details/group_bspline_segment.h -) -target_link_libraries(sophus_interp_spline INTERFACE sophus_lie) - - -farm_ng_add_test(spline - PARENT_LIBRARY sophus_interp_spline - LINK_LIBRARIES sophus_interp_spline - LABELS small) diff --git a/cpp/sophus/lie/CMakeLists.txt b/cpp/sophus/lie/CMakeLists.txt deleted file mode 100644 index 2c83fe8a..00000000 --- a/cpp/sophus/lie/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -#[[ -sophus_lie - - - -]] - -set(sophus_lie_src_prefixes - group_manifold - identity - isometry2 - isometry3 - lie_group - pose3 - rotation2 - rotation3 - scaling_translation - scaling - similarity2 - similarity3 - spiral_similarity2 - spiral_similarity3 - translation -) - -set(sophus_lie_h) - -foreach(x ${sophus_lie_src_prefixes} ) - list(APPEND sophus_lie_h ${x}.h) -endforeach() - - - -farm_ng_add_library(sophus_lie - NAMESPACE Sophus - INCLUDE_DIR ../../ - HEADERS - ${sophus_lie_h} -) -target_link_libraries(sophus_lie INTERFACE sophus_linalg) - - -foreach(test_basename ${sophus_lie_src_prefixes}) - farm_ng_add_test(${test_basename} - PARENT_LIBRARY sophus_lie - LINK_LIBRARIES sophus_lie - LABELS small) -endforeach() diff --git a/cpp/sophus/linalg/CMakeLists.txt b/cpp/sophus/linalg/CMakeLists.txt index 44119d44..b2f0bdf2 100644 --- a/cpp/sophus/linalg/CMakeLists.txt +++ b/cpp/sophus/linalg/CMakeLists.txt @@ -6,6 +6,7 @@ sophus_linalg ]] set(sophus_linalg_src_prefixes + batch cast homogeneous orthogonal @@ -29,11 +30,9 @@ farm_ng_add_library(sophus_linalg target_link_libraries(sophus_linalg INTERFACE sophus_common sophus_concept) -if(${BUILD_SOPHUS_TESTS}) - foreach(test_basename ${sophus_linalg_src_prefixes}) - farm_ng_add_test(${test_basename} - PARENT_LIBRARY sophus_linalg - LINK_LIBRARIES sophus_linalg - LABELS small) - endforeach() -endif() +foreach(test_basename ${sophus_linalg_src_prefixes}) + farm_ng_add_test(${test_basename} + PARENT_LIBRARY sophus_linalg + LINK_LIBRARIES sophus_linalg + LABELS small) +endforeach() diff --git a/cpp/sophus/sensor/CMakeLists.txt b/cpp/sophus/sensor/CMakeLists.txt deleted file mode 100644 index 57839a35..00000000 --- a/cpp/sophus/sensor/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -#[[ -sophus_sensor - - - -]] - -set(sophus_sensor_src_prefixes - camera_model - clipping_planes - imu_model - camera_rig - orthographic - ) - -set(sophus_sensor_cpp) -set(sophus_sensor_h) -foreach(x ${sophus_sensor_src_prefixes} ) - list(APPEND sophus_sensor_cpp ${x}.cpp) - list(APPEND sophus_sensor_h ${x}.h) -endforeach() - -farm_ng_add_library(sophus_sensor SHARED -NAMESPACE Sophus -INCLUDE_DIR ../../ -HEADERS -${sophus_sensor_h} -camera_distortion/affine.h -camera_distortion/brown_conrady.h -camera_distortion/kannala_brandt.h -camera_projection/projection_z1.h -camera_projection/projection_ortho.h -SOURCES - ${sophus_sensor_cpp}) - -target_link_libraries(sophus_sensor sophus_lie sophus_image) - - -if(${BUILD_SOPHUS_TESTS}) - foreach(test_basename ${sophus_sensor_src_prefixes}) - farm_ng_add_test(${test_basename} - PARENT_LIBRARY sophus_sensor - LINK_LIBRARIES sophus_sensor - LABELS small) - endforeach() -endif() diff --git a/cpp/sophus/.gitignore b/cpp/sophus2/.gitignore similarity index 100% rename from cpp/sophus/.gitignore rename to cpp/sophus2/.gitignore diff --git a/cpp/sophus/CMakeLists.txt b/cpp/sophus2/CMakeLists.txt similarity index 58% rename from cpp/sophus/CMakeLists.txt rename to cpp/sophus2/CMakeLists.txt index 427e6625..5d9b2ce5 100644 --- a/cpp/sophus/CMakeLists.txt +++ b/cpp/sophus2/CMakeLists.txt @@ -1,17 +1,16 @@ -project(Sophus VERSION 2.0.0) +# project(Sophus VERSION 2.0.0) include(CMakePackageConfigHelpers) include(GNUInstallDirs) set(farm_ng_cmake_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../cmake) -message("farm_ng_cmake_DIR" ${farm_ng_cmake_DIR}) include(${farm_ng_cmake_DIR}/farm_ng_cmakeConfig.cmake) find_package(Eigen3 3.4.0 REQUIRED) -farm_ng_module(sophus) +farm_ng_module(sophus2) message("COMPILE_OPTIONS" ${COMPILE_OPTIONS}) @@ -31,14 +30,19 @@ add_subdirectory(manifold) add_subdirectory(ceres) -add_library(sophus INTERFACE) -target_link_libraries(sophus INTERFACE - sophus_common - sophus_calculus - sophus_linalg - sophus_lie - sophus_interp - sophus_interp_spline - sophus_geometry +add_library(sophus2 INTERFACE) +target_link_libraries(sophus2 INTERFACE + sophus2_common + sophus2_calculus + sophus2_linalg + sophus2_lie + sophus2_interp + sophus2_interp_spline + sophus2_geometry +) +add_library(farm_ng_core::sophus2 ALIAS sophus2) +install(TARGETS sophus2 + EXPORT ${PROJECT_NAME}Targets + LIBRARY DESTINATION lib + COMPONENT Libs ) -add_library (Sophus::sophus ALIAS sophus) diff --git a/cpp/sophus/README.md b/cpp/sophus2/README.md similarity index 100% rename from cpp/sophus/README.md rename to cpp/sophus2/README.md diff --git a/cpp/sophus2/calculus/CMakeLists.txt b/cpp/sophus2/calculus/CMakeLists.txt new file mode 100644 index 00000000..1b615617 --- /dev/null +++ b/cpp/sophus2/calculus/CMakeLists.txt @@ -0,0 +1,21 @@ +#[[ +sophus2_calculus + + + +]] + +farm_ng_add_library(sophus2_calculus + NAMESPACE ${PROJECT_NAME} + INCLUDE_DIR ../../ + HEADERS + num_diff.h + region.h +) +target_link_libraries(sophus2_calculus INTERFACE sophus2_common) + +if(${BUILD_SOPHUS_TESTS}) + farm_ng_add_test(region + PARENT_LIBRARY sophus2_calculus + LABELS small) +endif() diff --git a/cpp/sophus/calculus/num_diff.h b/cpp/sophus2/calculus/num_diff.h similarity index 97% rename from cpp/sophus/calculus/num_diff.h rename to cpp/sophus2/calculus/num_diff.h index d8a4ceba..66c4eea8 100644 --- a/cpp/sophus/calculus/num_diff.h +++ b/cpp/sophus2/calculus/num_diff.h @@ -11,13 +11,13 @@ #pragma once -#include "sophus/common/common.h" +#include "sophus2/common/common.h" #include #include #include -namespace sophus { +namespace sophus2 { namespace details { template @@ -93,4 +93,4 @@ auto vectorFieldNumDiff( vector_field, a, eps); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/calculus/region.h b/cpp/sophus2/calculus/region.h similarity index 93% rename from cpp/sophus/calculus/region.h rename to cpp/sophus2/calculus/region.h index 75d1167e..9685de17 100644 --- a/cpp/sophus/calculus/region.h +++ b/cpp/sophus2/calculus/region.h @@ -9,11 +9,11 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/linalg/vector_space.h" -#include "sophus/linalg/vector_space_traits.h" +#include "sophus2/common/common.h" +#include "sophus2/linalg/vector_space.h" +#include "sophus2/linalg/vector_space_traits.h" -namespace sophus { +namespace sophus2 { template class Region; @@ -233,7 +233,7 @@ class Region { /// /// Precondition: !this->isEmptpy() [[nodiscard]] auto clamp(TPoint const& point) const noexcept -> TPoint { - return sophus::clamp(point, min(), max()); + return sophus2::clamp(point, min(), max()); } /// Returns true if the region contains the given point. @@ -278,8 +278,8 @@ class Region { *this = other; return *this; } - min_max_[0] = sophus::min(min(), other.min()); - min_max_[1] = sophus::max(max(), other.max()); + min_max_[0] = sophus2::min(min(), other.min()); + min_max_[1] = sophus2::max(max(), other.max()); return *this; } @@ -288,8 +288,8 @@ class Region { if (this->isEmpty()) { *this = from(point); } else { - min_max_[0] = sophus::min(min(), point); - min_max_[1] = sophus::max(max(), point); + min_max_[0] = sophus2::min(min(), point); + min_max_[1] = sophus2::max(max(), point); } return *this; } @@ -319,15 +319,15 @@ class Region { if constexpr (kIsInteger == Region::kIsInteger) { // case 1: floating => floating and integer => integer is trivial return Region( - sophus::cast(min()), sophus::cast(max())); + sophus2::cast(min()), sophus2::cast(max())); } if constexpr (kIsInteger && !Region::kIsInteger) { // case 2: integer to floating. // // example: [2, 5] -> [1.5, 5.5] return Region( - plus(sophus::cast(min()), -0.5), - plus(sophus::cast(max()), 0.5)); + plus(sophus2::cast(min()), -0.5), + plus(sophus2::cast(max()), 0.5)); } // case 3: floating to integer. static_assert( @@ -346,8 +346,8 @@ class Region { return Region::empty(); } return Region( - sophus::cast(sophus::floor(min())), - sophus::cast(sophus::ceil(max()))); + sophus2::cast(sophus2::floor(min())), + sophus2::cast(sophus2::ceil(max()))); } /// Rounds given region bounds and returns resulting integer region. @@ -361,8 +361,8 @@ class Region { } static_assert(!kIsInteger && Region::kIsInteger); return Region( - sophus::cast(sophus::round(min())), - sophus::cast(sophus::round(max()))); + sophus2::cast(sophus2::round(min())), + sophus2::cast(sophus2::round(max()))); } /// Returns true if region is empty. @@ -400,4 +400,4 @@ auto operator==(Region const& lhs, Region const& rhs) -> bool { return lhs.min() == rhs.min() && lhs.max() == rhs.max(); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/calculus/region_test.cpp b/cpp/sophus2/calculus/region_test.cpp similarity index 97% rename from cpp/sophus/calculus/region_test.cpp rename to cpp/sophus2/calculus/region_test.cpp index 197e9429..9dd848e9 100644 --- a/cpp/sophus/calculus/region_test.cpp +++ b/cpp/sophus2/calculus/region_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/calculus/region.h" +#include "sophus2/calculus/region.h" #include -using namespace sophus; +using namespace sophus2; template void regionTests(std::vector const& points) { diff --git a/cpp/sophus/ceres/CMakeLists.txt b/cpp/sophus2/ceres/CMakeLists.txt similarity index 81% rename from cpp/sophus/ceres/CMakeLists.txt rename to cpp/sophus2/ceres/CMakeLists.txt index 7a2480e8..2f09128c 100644 --- a/cpp/sophus/ceres/CMakeLists.txt +++ b/cpp/sophus2/ceres/CMakeLists.txt @@ -5,14 +5,14 @@ sophus_ceres ]] farm_ng_add_library(sophus_ceres - NAMESPACE Sophus + NAMESPACE ${PROJECT_NAME} INCLUDE_DIR ../../ HEADERS jet_helpers.h manifold.h typetraits.h ) -target_link_libraries(sophus_ceres INTERFACE sophus_lie) +target_link_libraries(sophus_ceres INTERFACE sophus2_lie) if(${BUILD_SOPHUS_TESTS}) find_package(Ceres 2.1.0 REQUIRED) @@ -25,7 +25,7 @@ if(${BUILD_SOPHUS_TESTS}) LINK_LIBRARIES Ceres::ceres sophus_ceres farm_ng_core::farm_ng_core_pipeline - sophus_sensor + sophus2_sensor LABELS large) endforeach() endif() diff --git a/cpp/sophus/ceres/ceres_test.cpp b/cpp/sophus2/ceres/ceres_test.cpp similarity index 93% rename from cpp/sophus/ceres/ceres_test.cpp rename to cpp/sophus2/ceres/ceres_test.cpp index 8d7c369c..b1472aab 100644 --- a/cpp/sophus/ceres/ceres_test.cpp +++ b/cpp/sophus2/ceres/ceres_test.cpp @@ -6,21 +6,21 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/ceres/manifold.h" -#include "sophus/common/common.h" -#include "sophus/lie/identity.h" -#include "sophus/lie/isometry2.h" -#include "sophus/lie/isometry3.h" -#include "sophus/lie/scaling.h" -#include "sophus/lie/scaling_translation.h" -#include "sophus/lie/similarity2.h" -#include "sophus/lie/similarity3.h" -#include "sophus/lie/spiral_similarity2.h" -#include "sophus/lie/spiral_similarity3.h" -#include "sophus/lie/translation.h" -#include "sophus/manifold/complex.h" -#include "sophus/manifold/quaternion.h" -#include "sophus/sensor/camera_rig.h" +#include "sophus2/ceres/manifold.h" +#include "sophus2/common/common.h" +#include "sophus2/lie/identity.h" +#include "sophus2/lie/isometry2.h" +#include "sophus2/lie/isometry3.h" +#include "sophus2/lie/scaling.h" +#include "sophus2/lie/scaling_translation.h" +#include "sophus2/lie/similarity2.h" +#include "sophus2/lie/similarity3.h" +#include "sophus2/lie/spiral_similarity2.h" +#include "sophus2/lie/spiral_similarity3.h" +#include "sophus2/lie/translation.h" +#include "sophus2/manifold/complex.h" +#include "sophus2/manifold/quaternion.h" +#include "sophus2/sensor/camera_rig.h" #include #include @@ -28,7 +28,7 @@ #include -namespace sophus::test { +namespace sophus2::test { struct Cost { double median() const { @@ -83,7 +83,7 @@ struct SimplePriorProblem { void solve(Variables& estimate) { ::ceres::Problem problem; - auto parametrization = new sophus::ceres::Manifold; + auto parametrization = new sophus2::ceres::Manifold; ::ceres::CostFunction* cost_function = new ::ceres::AutoDiffCostFunction< CostFunctor, @@ -213,7 +213,7 @@ struct TransformGraphProblem { void solve(Variables& estimate) { ::ceres::Problem problem; - auto parametrization = new sophus::ceres::Manifold; + auto parametrization = new sophus2::ceres::Manifold; for (size_t i = 0; i < estimate.world_from_robot_transforms.size(); ++i) { problem.AddParameterBlock( @@ -328,12 +328,12 @@ struct SensorMeasurementProblem { PinholeModel intrinsics; /// Camera extrinsics - sophus::Isometry3F64 robot_from_camera; + sophus2::Isometry3F64 robot_from_camera; }; struct CostFunctor { CostFunctor( - sophus::PinholeModel const& camera_model, + sophus2::PinholeModel const& camera_model, Isometry3F64 const& robot_from_camera, Eigen::Vector2d const& observation) : camera_model(camera_model), @@ -362,7 +362,7 @@ struct SensorMeasurementProblem { return true; } - sophus::PinholeModel camera_model; + sophus2::PinholeModel camera_model; Isometry3F64 robot_from_camera; Eigen::Vector2d observation; }; @@ -373,16 +373,16 @@ struct SensorMeasurementProblem { std::function>(int)> const& create_path) { int width = 640; int height = 480; - sophus::PinholeModel pinhole_intrinsics = + sophus2::PinholeModel pinhole_intrinsics = createDefaultPinholeModel({width, height}); Sensor cam_right; cam_right.intrinsics = pinhole_intrinsics; - cam_right.robot_from_camera = sophus::Isometry3F64::fromTy(0.25); + cam_right.robot_from_camera = sophus2::Isometry3F64::fromTy(0.25); Sensor cam_left; cam_left.intrinsics = pinhole_intrinsics; - cam_left.robot_from_camera = sophus::Isometry3F64::fromTy(-0.25); + cam_left.robot_from_camera = sophus2::Isometry3F64::fromTy(-0.25); this->camera_rig.push_back(cam_right); this->camera_rig.push_back(cam_left); @@ -453,7 +453,7 @@ struct SensorMeasurementProblem { void solve(Variables& estimate) { ::ceres::Problem problem; - auto parametrization = new sophus::ceres::Manifold; + auto parametrization = new sophus2::ceres::Manifold; for (size_t pose_idx = 0; pose_idx < estimate.world_from_robot_path.size(); ++pose_idx) { @@ -578,7 +578,7 @@ TEST(ceres_solve, regression_test) { { auto perturb = [](Isometry2F64& est) { est.translation() += Eigen::Vector2d(0.2, -0.1); - est.setRotation(est.rotation() * sophus::Rotation2F64(0.1)); + est.setRotation(est.rotation() * sophus2::Rotation2F64(0.1)); }; auto is_near = [](Isometry2F64 const& truth, Isometry2F64 const& est) -> Expected { @@ -610,7 +610,7 @@ TEST(ceres_solve, regression_test) { { auto perturb = [](Similarity2F64& est) { est.translation() += Eigen::Vector2d(0.2, -0.1); - est.setRotation(est.rotation() * sophus::Rotation2F64(0.1)); + est.setRotation(est.rotation() * sophus2::Rotation2F64(0.1)); est.setScale(est.scale() * 2.0); }; auto is_near = [](Similarity2F64 const& truth, @@ -700,7 +700,7 @@ TEST(ceres_solve, regression_test) { { auto perturb = [](Isometry3F64& est) { est.translation() += Eigen::Vector3d(0.3, -0.1, 0.1); - est.setRotation(est.rotation() * sophus::Rotation3F64::fromRx(0.1)); + est.setRotation(est.rotation() * sophus2::Rotation3F64::fromRx(0.1)); }; auto is_near = [](Isometry3F64 const& truth, Isometry3F64 const& est) -> Expected { @@ -730,7 +730,7 @@ TEST(ceres_solve, regression_test) { [](int kNumPoses) -> std::vector { std::vector path; for (int i = 0; i < kNumPoses; ++i) { - path.push_back(sophus::Isometry3F64::fromTx(0.1 * i)); + path.push_back(sophus2::Isometry3F64::fromTx(0.1 * i)); } return path; }); @@ -741,7 +741,7 @@ TEST(ceres_solve, regression_test) { { auto perturb = [](Similarity3F64& est) { est.translation() += Eigen::Vector3d(0.3, -0.1, 0.1); - est.setRotation(est.rotation() * sophus::Rotation3F64::fromRx(0.1)); + est.setRotation(est.rotation() * sophus2::Rotation3F64::fromRx(0.1)); est.setScale(est.scale() * 2.0); }; auto is_near = [](Similarity3F64 const& truth, @@ -1094,25 +1094,25 @@ struct JetLieGroupTests { }; TEST(jet_lie_group, compile_test) { - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); - JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); + JetLieGroupTests::testAll(); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/ceres/jet_helpers.h b/cpp/sophus2/ceres/jet_helpers.h similarity index 94% rename from cpp/sophus/ceres/jet_helpers.h rename to cpp/sophus2/ceres/jet_helpers.h index 67829b75..e33c5b0b 100644 --- a/cpp/sophus/ceres/jet_helpers.h +++ b/cpp/sophus2/ceres/jet_helpers.h @@ -15,7 +15,7 @@ template struct Jet; } // namespace ceres -namespace sophus { +namespace sophus2 { namespace jet_helpers { @@ -30,4 +30,4 @@ struct GetValue<::ceres::Jet> { }; } // namespace jet_helpers -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/ceres/manifold.h b/cpp/sophus2/ceres/manifold.h similarity index 96% rename from cpp/sophus/ceres/manifold.h rename to cpp/sophus2/ceres/manifold.h index 2cb8e282..50009b5d 100644 --- a/cpp/sophus/ceres/manifold.h +++ b/cpp/sophus2/ceres/manifold.h @@ -8,9 +8,9 @@ #pragma once -#include "sophus/ceres/typetraits.h" +#include "sophus2/ceres/typetraits.h" -namespace sophus::ceres { +namespace sophus2::ceres { /// Templated local parameterization for LieGroup [with implemented /// LieGroup::Dx_this_mul_exp_x_at_0() ] @@ -81,4 +81,4 @@ class Manifold : public ::ceres::Manifold { [[nodiscard]] int TangentSize() const override { return LieGroupF64::kDof; } }; -} // namespace sophus::ceres +} // namespace sophus2::ceres diff --git a/cpp/sophus/ceres/manifold_test.cpp b/cpp/sophus2/ceres/manifold_test.cpp similarity index 58% rename from cpp/sophus/ceres/manifold_test.cpp rename to cpp/sophus2/ceres/manifold_test.cpp index f7fa864d..723e91e6 100644 --- a/cpp/sophus/ceres/manifold_test.cpp +++ b/cpp/sophus2/ceres/manifold_test.cpp @@ -6,126 +6,126 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/ceres/manifold.h" - -#include "sophus/common/common.h" -#include "sophus/lie/isometry2.h" -#include "sophus/lie/isometry3.h" -#include "sophus/lie/scaling.h" -#include "sophus/lie/scaling_translation.h" -#include "sophus/lie/similarity2.h" -#include "sophus/lie/similarity3.h" -#include "sophus/lie/translation.h" -#include "sophus/linalg/vector_space.h" +#include "sophus2/ceres/manifold.h" + +#include "sophus2/common/common.h" +#include "sophus2/lie/isometry2.h" +#include "sophus2/lie/isometry3.h" +#include "sophus2/lie/scaling.h" +#include "sophus2/lie/scaling_translation.h" +#include "sophus2/lie/similarity2.h" +#include "sophus2/lie/similarity3.h" +#include "sophus2/lie/translation.h" +#include "sophus2/linalg/vector_space.h" #include #include #include -namespace sophus::test { +namespace sophus2::test { template struct RotationalPart; template <> -struct RotationalPart { - static double norm(sophus::Rotation2F64::Tangent const& t) { +struct RotationalPart { + static double norm(sophus2::Rotation2F64::Tangent const& t) { return t.template tail<1>().norm(); } }; template <> -struct RotationalPart { - static double norm(sophus::Rotation3F64::Tangent const& t) { +struct RotationalPart { + static double norm(sophus2::Rotation3F64::Tangent const& t) { return t.template tail<3>().norm(); } }; template <> -struct RotationalPart { - static double norm(sophus::Isometry2F64::Tangent const& t) { +struct RotationalPart { + static double norm(sophus2::Isometry2F64::Tangent const& t) { return t.template tail<1>().norm(); } }; template <> -struct RotationalPart { - static double norm(sophus::Isometry3F64::Tangent const& t) { +struct RotationalPart { + static double norm(sophus2::Isometry3F64::Tangent const& t) { return t.template tail<3>().norm(); } }; template <> -struct RotationalPart { - static double norm(sophus::SpiralSimilarity2F64::Tangent const& t) { +struct RotationalPart { + static double norm(sophus2::SpiralSimilarity2F64::Tangent const& t) { return t.template segment<1>(0).norm(); } }; template <> -struct RotationalPart { - static double norm(sophus::SpiralSimilarity3F64::Tangent const& t) { +struct RotationalPart { + static double norm(sophus2::SpiralSimilarity3F64::Tangent const& t) { return t.template segment<3>(0).norm(); } }; template <> -struct RotationalPart { - static double norm(sophus::Similarity2F64::Tangent const& t) { +struct RotationalPart { + static double norm(sophus2::Similarity2F64::Tangent const& t) { return t.template segment<1>(2).norm(); } }; template <> -struct RotationalPart { - static double norm(sophus::Similarity3F64::Tangent const& t) { +struct RotationalPart { + static double norm(sophus2::Similarity3F64::Tangent const& t) { return t.template segment<3>(3).norm(); } }; template <> -struct RotationalPart> { - static double norm(typename sophus::Scaling::Tangent const& t) { +struct RotationalPart> { + static double norm(typename sophus2::Scaling::Tangent const& t) { return 0.0; } }; template <> -struct RotationalPart> { - static double norm(typename sophus::Scaling::Tangent const& t) { +struct RotationalPart> { + static double norm(typename sophus2::Scaling::Tangent const& t) { return 0.0; } }; template <> -struct RotationalPart> { +struct RotationalPart> { static double norm( - typename sophus::ScalingTranslation::Tangent const& t) { + typename sophus2::ScalingTranslation::Tangent const& t) { return 0.0; } }; template <> -struct RotationalPart> { +struct RotationalPart> { static double norm( - typename sophus::ScalingTranslation::Tangent const& t) { + typename sophus2::ScalingTranslation::Tangent const& t) { return 0.0; } }; template <> -struct RotationalPart> { +struct RotationalPart> { static double norm( - typename sophus::Translation::Tangent const& t) { + typename sophus2::Translation::Tangent const& t) { return 0.0; } }; template <> -struct RotationalPart> { +struct RotationalPart> { static double norm( - typename sophus::Translation::Tangent const& t) { + typename sophus2::Translation::Tangent const& t) { return 0.0; } }; @@ -163,7 +163,7 @@ struct CeresManifoldTests { const TangentF64 delta = (x.inverse() * y).log(); TangentF64 o; o.setZero(); - ::sophus::ceres::Manifold manifold; + ::sophus2::ceres::Manifold manifold; LieGroupF64 test_group; @@ -194,14 +194,14 @@ struct CeresManifoldTests { void xPlusZeroIsXAt(LieGroupF64 const& x) { TangentF64 o; o.setZero(); - sophus::ceres::Manifold manifold; + sophus2::ceres::Manifold manifold; LieGroupF64 test_group; SOPHUS_ASSERT(manifold.Plus(x.ptr(), o.data(), test_group.unsafeMutPtr())); double const error = ((x.inverse() * test_group).log()).squaredNorm(); SOPHUS_ASSERT_LE( error, - sophus::kEpsilonF64, + sophus2::kEpsilonF64, "{}\nx:\n {}\ntg:\n {}", group_name, x.matrix(), @@ -209,21 +209,21 @@ struct CeresManifoldTests { } void xMinusXIsZeroAt(LieGroupF64 const& x) { - sophus::ceres::Manifold manifold; + sophus2::ceres::Manifold manifold; LieGroupF64 test_group; TangentF64 test_tangent; SOPHUS_ASSERT(manifold.Minus(x.ptr(), x.ptr(), test_tangent.data())); double const error = test_tangent.squaredNorm(); - SOPHUS_ASSERT(error < sophus::kEpsilonF64); + SOPHUS_ASSERT(error < sophus2::kEpsilonF64); } void minusPlusIsIdentityAt(LieGroupF64 const& x, TangentF64 const& delta) { if (RotationalPart::norm(delta) > - sophus::kPi * (1. - sophus::kEpsilonF64)) { + sophus2::kPi * (1. - sophus2::kEpsilonF64)) { return; } - sophus::ceres::Manifold manifold; + sophus2::ceres::Manifold manifold; LieGroupF64 test_group; TangentF64 test_tangent; @@ -237,11 +237,11 @@ struct CeresManifoldTests { const TangentF64 diff = test_tangent - delta; double const error = diff.squaredNorm(); - SOPHUS_ASSERT_LE(error, 10 * sophus::kEpsilonF64, "{}", group_name); + SOPHUS_ASSERT_LE(error, 10 * sophus2::kEpsilonF64, "{}", group_name); } void plusMinusIsIdentityAt(LieGroupF64 const& x, LieGroupF64 const& y) { - sophus::ceres::Manifold manifold; + sophus2::ceres::Manifold manifold; LieGroupF64 test_group; TangentF64 test_tangent; @@ -251,11 +251,11 @@ struct CeresManifoldTests { manifold.Plus(x.ptr(), test_tangent.data(), test_group.unsafeMutPtr())); double const error = ((y.inverse() * test_group).log()).squaredNorm(); - SOPHUS_ASSERT_LE(error, 10.0 * sophus::kEpsilonF64); + SOPHUS_ASSERT_LE(error, 10.0 * sophus2::kEpsilonF64); } void minusPlusJacobianIsIdentityAt(LieGroupF64 const& x) { - sophus::ceres::Manifold manifold; + sophus2::ceres::Manifold manifold; LieGroupF64 test_group; Eigen::Matrix< @@ -275,7 +275,7 @@ struct CeresManifoldTests { std::cerr << diff << std::endl; double const error = diff.squaredNorm(); - SOPHUS_ASSERT(error < sophus::kEpsilonF64); + SOPHUS_ASSERT(error < sophus2::kEpsilonF64); } std::vector group_vec; @@ -285,75 +285,75 @@ struct CeresManifoldTests { }; TEST(ceres_manifold, prop_test2) { - auto points2 = sophus::pointExamples(); + auto points2 = sophus2::pointExamples(); - CeresManifoldTests( - sophus::Rotation2F64::elementExamples(), points2, "Rotation2") + CeresManifoldTests( + sophus2::Rotation2F64::elementExamples(), points2, "Rotation2") .testAll(); - CeresManifoldTests( - sophus::Isometry2F64::elementExamples(), points2, "Isometry2") + CeresManifoldTests( + sophus2::Isometry2F64::elementExamples(), points2, "Isometry2") .testAll(); - CeresManifoldTests( - sophus::SpiralSimilarity2F64::elementExamples(), + CeresManifoldTests( + sophus2::SpiralSimilarity2F64::elementExamples(), points2, "SpiralSimilarity2") .testAll(); - CeresManifoldTests( - sophus::Similarity2F64::elementExamples(), points2, "Similarity2") + CeresManifoldTests( + sophus2::Similarity2F64::elementExamples(), points2, "Similarity2") .testAll(); - CeresManifoldTests( - sophus::Scaling2::elementExamples(), points2, "Scaling2") + CeresManifoldTests( + sophus2::Scaling2::elementExamples(), points2, "Scaling2") .testAll(); - CeresManifoldTests( - sophus::Translation2::elementExamples(), points2, "Translation2") + CeresManifoldTests( + sophus2::Translation2::elementExamples(), points2, "Translation2") .testAll(); - CeresManifoldTests( - sophus::ScalingTranslation2::elementExamples(), + CeresManifoldTests( + sophus2::ScalingTranslation2::elementExamples(), points2, "ScalingTranslation2") .testAll(); } TEST(ceres_manifold, prop_test3) { - auto points3 = sophus::pointExamples(); + auto points3 = sophus2::pointExamples(); - CeresManifoldTests( - sophus::Rotation3F64::elementExamples(), points3, "Rotation3") + CeresManifoldTests( + sophus2::Rotation3F64::elementExamples(), points3, "Rotation3") .testAll(); - CeresManifoldTests( - sophus::Isometry3F64::elementExamples(), points3, "Isometry3") + CeresManifoldTests( + sophus2::Isometry3F64::elementExamples(), points3, "Isometry3") .testAll(); - CeresManifoldTests( - sophus::SpiralSimilarity3F64::elementExamples(), + CeresManifoldTests( + sophus2::SpiralSimilarity3F64::elementExamples(), points3, "SpiralSimilarity3") .testAll(); - CeresManifoldTests( - sophus::Similarity3F64::elementExamples(), points3, "Similarity3") + CeresManifoldTests( + sophus2::Similarity3F64::elementExamples(), points3, "Similarity3") .testAll(); - CeresManifoldTests( - sophus::Scaling3::elementExamples(), points3, "Scaling3") + CeresManifoldTests( + sophus2::Scaling3::elementExamples(), points3, "Scaling3") .testAll(); - CeresManifoldTests( - sophus::Translation3::elementExamples(), points3, "Translation3") + CeresManifoldTests( + sophus2::Translation3::elementExamples(), points3, "Translation3") .testAll(); - CeresManifoldTests( - sophus::ScalingTranslation3::elementExamples(), + CeresManifoldTests( + sophus2::ScalingTranslation3::elementExamples(), points3, "ScalingTranslation3") .testAll(); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/ceres/typetraits.h b/cpp/sophus2/ceres/typetraits.h similarity index 96% rename from cpp/sophus/ceres/typetraits.h rename to cpp/sophus2/ceres/typetraits.h index 1266532c..28f2f6fa 100644 --- a/cpp/sophus/ceres/typetraits.h +++ b/cpp/sophus2/ceres/typetraits.h @@ -7,13 +7,13 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/common/common.h" +#include "sophus2/common/common.h" #include #include -namespace sophus { +namespace sophus2 { template constexpr std::true_type complete(TScalar*); @@ -64,4 +64,4 @@ struct Mapper>::type> { static ConstMap map(Scalar const* ptr) noexcept { return ConstMap(ptr); } }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus2/color/CMakeLists.txt b/cpp/sophus2/color/CMakeLists.txt new file mode 100644 index 00000000..cbfb010a --- /dev/null +++ b/cpp/sophus2/color/CMakeLists.txt @@ -0,0 +1,26 @@ +#[[ +sophus2_color + + + +]] + +farm_ng_add_library(sophus2_color + NAMESPACE ${PROJECT_NAME} + INCLUDE_DIR ../../ + HEADERS + color.h + SOURCES + color.cpp +) +target_link_libraries(sophus2_color PUBLIC sophus2_calculus) + +if(${BUILD_SOPHUS_TESTS}) + foreach(test_basename + color) + farm_ng_add_test(${test_basename} + PARENT_LIBRARY sophus2_color + LINK_LIBRARIES sophus2_color + LABELS small) + endforeach() +endif() diff --git a/cpp/sophus/color/color.cpp b/cpp/sophus2/color/color.cpp similarity index 89% rename from cpp/sophus/color/color.cpp rename to cpp/sophus2/color/color.cpp index 23e479dc..01c2b8bc 100644 --- a/cpp/sophus/color/color.cpp +++ b/cpp/sophus2/color/color.cpp @@ -6,4 +6,4 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/color/color.h" +#include "sophus2/color/color.h" diff --git a/cpp/sophus/color/color.h b/cpp/sophus2/color/color.h similarity index 99% rename from cpp/sophus/color/color.h rename to cpp/sophus2/color/color.h index db486005..dfa1c29e 100644 --- a/cpp/sophus/color/color.h +++ b/cpp/sophus2/color/color.h @@ -9,13 +9,13 @@ #pragma once #include -#include +#include #include #include #include -namespace sophus { +namespace sophus2 { SOPHUS_ENUM( ColorHue, @@ -374,4 +374,4 @@ struct Color { static_assert(sizeof(Color) == 4 * sizeof(float)); -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/color/color_test.cpp b/cpp/sophus2/color/color_test.cpp similarity index 84% rename from cpp/sophus/color/color_test.cpp rename to cpp/sophus2/color/color_test.cpp index d7549fd5..f96ba642 100644 --- a/cpp/sophus/color/color_test.cpp +++ b/cpp/sophus2/color/color_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/color/color.h" +#include "sophus2/color/color.h" #include -using namespace sophus; +using namespace sophus2; TEST(color, smoke) {} diff --git a/cpp/sophus/common/CMakeLists.txt b/cpp/sophus2/common/CMakeLists.txt similarity index 55% rename from cpp/sophus/common/CMakeLists.txt rename to cpp/sophus2/common/CMakeLists.txt index ffd145d0..ceef3fe1 100644 --- a/cpp/sophus/common/CMakeLists.txt +++ b/cpp/sophus2/common/CMakeLists.txt @@ -1,11 +1,11 @@ #[[ -sophus_common +sophus2_common ]] -farm_ng_add_library(sophus_common - NAMESPACE Sophus +farm_ng_add_library(sophus2_common + NAMESPACE ${PROJECT_NAME} INCLUDE_DIR ../../ HEADERS common.h @@ -13,12 +13,12 @@ farm_ng_add_library(sophus_common ) farm_ng_add_test(common - PARENT_LIBRARY sophus_common - LINK_LIBRARIES sophus_common + PARENT_LIBRARY sophus2_common + LINK_LIBRARIES sophus2_common LABELS small) target_link_libraries( - sophus_common + sophus2_common INTERFACE Eigen3::Eigen farm_ng_core::farm_ng_core_logging diff --git a/cpp/sophus/common/common.h b/cpp/sophus2/common/common.h similarity index 97% rename from cpp/sophus/common/common.h rename to cpp/sophus2/common/common.h index 9d295f56..0a170fa6 100644 --- a/cpp/sophus/common/common.h +++ b/cpp/sophus2/common/common.h @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -49,7 +50,7 @@ #define SOPHUS_UNEXPECTED(...) FARM_UNEXPECTED(__VA_ARGS__) // END(exclude from doxygen) \endcond -namespace sophus { +namespace sophus2 { // from using ::farm_ng::AlwaysFalse; @@ -116,4 +117,4 @@ template bool constexpr kIsUniformRandomBitGeneratorV = IsUniformRandomBitGenerator::kValue; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/common/common_test.cpp b/cpp/sophus2/common/common_test.cpp similarity index 92% rename from cpp/sophus/common/common_test.cpp rename to cpp/sophus2/common/common_test.cpp index 2b5ca49c..f761b043 100644 --- a/cpp/sophus/common/common_test.cpp +++ b/cpp/sophus2/common/common_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/common/common.h" +#include "sophus2/common/common.h" #include -using namespace sophus; +using namespace sophus2; TEST(check_near, unit) { SOPHUS_ASSERT_WITHIN_REL( diff --git a/cpp/sophus/common/enum.h b/cpp/sophus2/common/enum.h similarity index 91% rename from cpp/sophus/common/enum.h rename to cpp/sophus2/common/enum.h index a5442151..6d799e6c 100644 --- a/cpp/sophus/common/enum.h +++ b/cpp/sophus2/common/enum.h @@ -8,7 +8,7 @@ #pragma once -#include "sophus/common/common.h" +#include "sophus2/common/common.h" #include diff --git a/cpp/sophus/concepts/CMakeLists.txt b/cpp/sophus2/concepts/CMakeLists.txt similarity index 81% rename from cpp/sophus/concepts/CMakeLists.txt rename to cpp/sophus2/concepts/CMakeLists.txt index 5984911e..f3905ddc 100644 --- a/cpp/sophus/concepts/CMakeLists.txt +++ b/cpp/sophus2/concepts/CMakeLists.txt @@ -1,11 +1,11 @@ #[[ -sophus_common +sophus2_common ]] farm_ng_add_library(sophus_concept - NAMESPACE Sophus + NAMESPACE ${PROJECT_NAME} INCLUDE_DIR ../../ HEADERS image.h @@ -18,7 +18,7 @@ farm_ng_add_library(sophus_concept utils.h ) -target_link_libraries(sophus_concept INTERFACE sophus_common) +target_link_libraries(sophus_concept INTERFACE sophus2_common) foreach(test_basename group_accessors diff --git a/cpp/sophus2/concepts/division_ring.h b/cpp/sophus2/concepts/division_ring.h new file mode 100644 index 00000000..6896425a --- /dev/null +++ b/cpp/sophus2/concepts/division_ring.h @@ -0,0 +1,100 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#pragma once +#include "sophus2/concepts/params.h" +#include "sophus2/concepts/utils.h" + +namespace sophus2 { +namespace concepts { + +template +class UnitVector; + +template +concept DivisionRingImpl = + ::sophus2::concepts::ParamsImpl // or homogeneous point representation + && requires( + typename TT::Params params, + Eigen::Vector, TT::kNumParams> + compatible_params) { + // constructors and factories + { TT::one() } -> ::sophus2::concepts::ConvertibleTo; + + { TT::zero() } -> ::sophus2::concepts::ConvertibleTo; + + // operations + + { + TT::addition(params, params) + } -> ::sophus2::concepts::ConvertibleTo; + + { + TT::multiplication(params, params) + } -> ::sophus2::concepts::ConvertibleTo; + +#if __cplusplus >= 202002L + { + TT::template addition>( + params, compatible_params) + } -> ::sophus2::concepts::ConvertibleTo>>; + + { + TT::template multiplication>( + params, compatible_params) + } -> ::sophus2::concepts::ConvertibleTo>>; +#endif + + { + TT::conjugate(params) + } -> ::sophus2::concepts::ConvertibleTo; + + { + TT::inverse(params) + } -> ::sophus2::concepts::ConvertibleTo; + + // reduction + { + TT::norm(params) + } -> ::sophus2::concepts::ConvertibleTo; + + { + TT::squaredNorm(params) + } -> ::sophus2::concepts::ConvertibleTo; +}; + +template +concept DivisionRingConcept = DivisionRingImpl && requires( + TT r, + typename TT::Scalar real, + typename TT::Imag imag, + typename TT::Params params) { + // operations + { r.operator+(r) } -> ::sophus2::concepts::ConvertibleTo; + + { r.operator*(r) } -> ::sophus2::concepts::ConvertibleTo; + + { r.conjugate() } -> ::sophus2::concepts::ConvertibleTo; + + { r.inverse() } -> ::sophus2::concepts::ConvertibleTo; + + // reduction + { r.norm() } -> ::sophus2::concepts::ConvertibleTo; + + { + r.squaredNorm() + } -> ::sophus2::concepts::ConvertibleTo; + + { r.real() } -> ::sophus2::concepts::ConvertibleTo; + + { r.imag() } -> ::sophus2::concepts::ConvertibleTo; +}; +} // namespace concepts +} // namespace sophus2 diff --git a/cpp/sophus/concepts/division_ring_prop_tests.h b/cpp/sophus2/concepts/division_ring_prop_tests.h similarity index 79% rename from cpp/sophus/concepts/division_ring_prop_tests.h rename to cpp/sophus2/concepts/division_ring_prop_tests.h index b63f8af3..de1ea362 100644 --- a/cpp/sophus/concepts/division_ring_prop_tests.h +++ b/cpp/sophus2/concepts/division_ring_prop_tests.h @@ -8,9 +8,9 @@ #pragma once -#include "sophus/concepts/division_ring.h" +#include "sophus2/concepts/division_ring.h" -namespace sophus { +namespace sophus2 { namespace test { template @@ -42,7 +42,7 @@ struct DivisionRingTestSuite { SOPHUS_ASSERT_WITHIN_REL( left_hugging.params(), right_hugging.params(), - 10.0 * kEpsilonSqrt, + 10.0 * kEpsilonSqrt::Scalar>, "`(g1*g2)*g3 == g1*(g2*g3)` Test for {}, #{}/#{}/#{}", ring_name, params_id, @@ -70,7 +70,7 @@ struct DivisionRingTestSuite { SOPHUS_ASSERT_WITHIN_REL( left_hugging.params(), right_hugging.params(), - kEpsilonSqrt, + kEpsilonSqrt::Scalar>, "`g1 * g2 == g2 * g3` Test for {}, #{}/#{}", ring_name, params_id, @@ -91,16 +91,16 @@ struct DivisionRingTestSuite { Ring left_hugging = g1 * g2; Ring right_hugging = g2 * g1; ++num_cases; - if ((left_hugging.params() - right_hugging.params()).norm() < - kEpsilonSqrt) { - ++num_commutativity; - } + // if ((left_hugging.params() - right_hugging.params()).norm() < + // kEpsilonSqrt::Scalar>) { + // ++num_commutativity; + // } } } if (num_cases > 0) { Scalar commutativity_percentage = Scalar(num_commutativity) / Scalar(num_cases); - SOPHUS_ASSERT_LE(commutativity_percentage, 0.75); + // SOPHUS_ASSERT_LE(commutativity_percentage, 0.75); } } } @@ -114,7 +114,7 @@ struct DivisionRingTestSuite { SOPHUS_ASSERT_WITHIN_REL( g.params(), (g + Ring::zero()).params(), - kEpsilonSqrt, + kEpsilonSqrt::Scalar>, "`g + 0 == g` Test for {}, #{}", ring_name, params_id); @@ -127,25 +127,26 @@ struct DivisionRingTestSuite { Params params = SOPHUS_AT(kParamsExamples, params_id); Ring g = Ring::fromParams(params); - if (g.params().norm() < kEpsilonSqrt) { - continue; - } - - SOPHUS_ASSERT_WITHIN_REL( - g.params(), - (g * Ring::one()).params(), - kEpsilonSqrt, - "`g * 1 == g` Test for {}, #{}", - ring_name, - params_id); - - SOPHUS_ASSERT_WITHIN_REL( - (g * g.inverse()).params(), - Ring::one().params(), - kEpsilonSqrt, - "`g * 1 == g` Test for {}, #{}", - ring_name, - params_id); + // if (g.params().norm() < kEpsilonSqrt::Scalar>) { + // continue; + // } + + // SOPHUS_ASSERT_WITHIN_REL( + // g.params(), + // (g * Ring::one()).params(), + // kEpsilonSqrt, + // "`g * 1 == g` Test for {}, #{}", + // ring_name, + // params_id); + + // SOPHUS_ASSERT_WITHIN_REL( + // (g * g.inverse()).params(), + // Ring::one().params(), + // kEpsilonSqrt, + // "`g * 1 == g` Test for {}, #{}", + // ring_name, + // params_id); } } @@ -165,4 +166,4 @@ decltype(TRing::Impl::paramsExamples()) //! @endcond } // namespace test -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/group_accessors.h b/cpp/sophus2/concepts/group_accessors.h similarity index 98% rename from cpp/sophus/concepts/group_accessors.h rename to cpp/sophus2/concepts/group_accessors.h index 9c551767..a82323f5 100644 --- a/cpp/sophus/concepts/group_accessors.h +++ b/cpp/sophus2/concepts/group_accessors.h @@ -7,9 +7,9 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/concepts/lie_group.h" +#include "sophus2/concepts/lie_group.h" -namespace sophus { +namespace sophus2 { template class Complex; @@ -193,4 +193,4 @@ template concept Translation = accessors::Translation; } // namespace concepts -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/group_accessors_test.cpp b/cpp/sophus2/concepts/group_accessors_test.cpp similarity index 82% rename from cpp/sophus/concepts/group_accessors_test.cpp rename to cpp/sophus2/concepts/group_accessors_test.cpp index 2f326d39..d057d3d2 100644 --- a/cpp/sophus/concepts/group_accessors_test.cpp +++ b/cpp/sophus2/concepts/group_accessors_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/concepts/group_accessors.h" +#include "sophus2/concepts/group_accessors.h" #include -using namespace sophus; +using namespace sophus2; TEST(group_accessors_test, compile_test) {} diff --git a/cpp/sophus/concepts/group_accessors_unit_tests.h b/cpp/sophus2/concepts/group_accessors_unit_tests.h similarity index 85% rename from cpp/sophus/concepts/group_accessors_unit_tests.h rename to cpp/sophus2/concepts/group_accessors_unit_tests.h index ab04d39a..df05e9d1 100644 --- a/cpp/sophus/concepts/group_accessors_unit_tests.h +++ b/cpp/sophus2/concepts/group_accessors_unit_tests.h @@ -8,19 +8,19 @@ #pragma once -#include "sophus/calculus/num_diff.h" -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/isometry2.h" -#include "sophus/lie/isometry3.h" -#include "sophus/lie/rotation2.h" -#include "sophus/lie/rotation3.h" -#include "sophus/lie/similarity2.h" -#include "sophus/lie/similarity3.h" -#include "sophus/lie/spiral_similarity2.h" -#include "sophus/lie/spiral_similarity3.h" -#include "sophus/linalg/vector_space.h" - -namespace sophus { +#include "sophus2/calculus/num_diff.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/isometry2.h" +#include "sophus2/lie/isometry3.h" +#include "sophus2/lie/rotation2.h" +#include "sophus2/lie/rotation3.h" +#include "sophus2/lie/similarity2.h" +#include "sophus2/lie/similarity3.h" +#include "sophus2/lie/spiral_similarity2.h" +#include "sophus2/lie/spiral_similarity3.h" +#include "sophus2/linalg/vector_space.h" + +namespace sophus2 { namespace test { template @@ -47,19 +47,19 @@ template void runRotationAccessorTests() { using Scalar = typename TGroup::Scalar; if constexpr (TGroup::kPointDim == 2) { - auto kElems = sophus::Rotation2::elementExamples(); + auto kElems = sophus2::Rotation2::elementExamples(); for (size_t g_id = 0; g_id < kElems.size(); ++g_id) { - sophus::Rotation2 rot = SOPHUS_AT(kElems, g_id); + sophus2::Rotation2 rot = SOPHUS_AT(kElems, g_id); TGroup g = TGroup::fromRotationMatrix(rot.matrix()); SOPHUS_ASSERT_WITHIN_REL( g.rotationMatrix(), rot.matrix(), kEpsilonSqrt); } } else if constexpr (TGroup::kPointDim == 3) { - auto kElems = sophus::Rotation3::elementExamples(); + auto kElems = sophus2::Rotation3::elementExamples(); for (size_t g_id = 0; g_id < kElems.size(); ++g_id) { - sophus::Rotation3 rot = SOPHUS_AT(kElems, g_id); + sophus2::Rotation3 rot = SOPHUS_AT(kElems, g_id); TGroup g = TGroup::fromRotationMatrix(rot.matrix()); SOPHUS_ASSERT_WITHIN_REL( @@ -147,19 +147,19 @@ template void runIsometryAccessorTests() { using Scalar = typename TGroup::Scalar; if constexpr (TGroup::kPointDim == 2) { - auto kElems = sophus::Isometry2::elementExamples(); + auto kElems = sophus2::Isometry2::elementExamples(); for (size_t g_id = 0; g_id < kElems.size(); ++g_id) { - sophus::Isometry2 iso = SOPHUS_AT(kElems, g_id); + sophus2::Isometry2 iso = SOPHUS_AT(kElems, g_id); TGroup rot_g = TGroup::fromRotationMatrix(iso.rotationMatrix()); SOPHUS_ASSERT_WITHIN_REL( rot_g.rotationMatrix(), iso.rotationMatrix(), kEpsilon); } } else if constexpr (TGroup::kPointDim == 3) { - auto kElems = sophus::Isometry3::elementExamples(); + auto kElems = sophus2::Isometry3::elementExamples(); for (size_t g_id = 0; g_id < kElems.size(); ++g_id) { - sophus::Isometry3 iso = SOPHUS_AT(kElems, g_id); + sophus2::Isometry3 iso = SOPHUS_AT(kElems, g_id); TGroup rot_g = TGroup::fromRotationMatrix(iso.rotationMatrix()); SOPHUS_ASSERT_WITHIN_REL( @@ -175,9 +175,9 @@ void runSpiralSimilarityAccessorTests() { using Scalar = typename TGroup::Scalar; if constexpr (TGroup::kPointDim == 2) { - auto kElems = sophus::SpiralSimilarity2::elementExamples(); + auto kElems = sophus2::SpiralSimilarity2::elementExamples(); for (size_t g_id = 0; g_id < kElems.size(); ++g_id) { - sophus::SpiralSimilarity2 spiral_sim = SOPHUS_AT(kElems, g_id); + sophus2::SpiralSimilarity2 spiral_sim = SOPHUS_AT(kElems, g_id); double scale = spiral_sim.scale(); TGroup scale_g = TGroup::fromScale(scale); @@ -188,10 +188,10 @@ void runSpiralSimilarityAccessorTests() { SOPHUS_ASSERT_WITHIN_REL(scale_g2.scale(), scale, kEpsilon); } } else if constexpr (TGroup::kPointDim == 3) { - auto kElems = sophus::SpiralSimilarity3::elementExamples(); + auto kElems = sophus2::SpiralSimilarity3::elementExamples(); for (size_t g_id = 0; g_id < kElems.size(); ++g_id) { - sophus::SpiralSimilarity3 spiral_sim = SOPHUS_AT(kElems, g_id); + sophus2::SpiralSimilarity3 spiral_sim = SOPHUS_AT(kElems, g_id); double scale = spiral_sim.scale(); TGroup scale_g = TGroup::fromScale(scale); @@ -209,9 +209,9 @@ void runSimilarityAccessorTests() { using Scalar = typename TGroup::Scalar; if constexpr (TGroup::kPointDim == 2) { - auto kElems = sophus::Similarity2::elementExamples(); + auto kElems = sophus2::Similarity2::elementExamples(); for (size_t g_id = 0; g_id < kElems.size(); ++g_id) { - sophus::Similarity2 sim = SOPHUS_AT(kElems, g_id); + sophus2::Similarity2 sim = SOPHUS_AT(kElems, g_id); double scale = sim.scale(); TGroup scale_g = TGroup::fromScale(scale); @@ -222,10 +222,10 @@ void runSimilarityAccessorTests() { SOPHUS_ASSERT_WITHIN_REL(scale_g2.scale(), scale, kEpsilon); } } else if constexpr (TGroup::kPointDim == 3) { - auto kElems = sophus::Similarity3::elementExamples(); + auto kElems = sophus2::Similarity3::elementExamples(); for (size_t g_id = 0; g_id < kElems.size(); ++g_id) { - sophus::Similarity3 sim = SOPHUS_AT(kElems, g_id); + sophus2::Similarity3 sim = SOPHUS_AT(kElems, g_id); double scale = sim.scale(); TGroup scale_g = TGroup::fromScale(scale); @@ -357,4 +357,4 @@ void runSimilarity3UnitTests() { runSpiralSimilarity3AccessorTests(); } } // namespace test -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/image.h b/cpp/sophus2/concepts/image.h similarity index 80% rename from cpp/sophus/concepts/image.h rename to cpp/sophus2/concepts/image.h index 506d870f..f20b62c0 100644 --- a/cpp/sophus/concepts/image.h +++ b/cpp/sophus2/concepts/image.h @@ -7,10 +7,10 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/common/enum.h" -#include "sophus/concepts/params.h" +#include "sophus2/common/enum.h" +#include "sophus2/concepts/params.h" -namespace sophus { +namespace sophus2 { struct ImageSize; class ImageLayout; @@ -40,19 +40,19 @@ concept ImageLayoutTrait = ImageSizeTrait && requires(TT self) { { self.isEmpty() } -> SameAs; - { self.imageSize() } -> ConvertibleTo; + { self.imageSize() } -> ConvertibleTo; }; template concept ImageView = ImageLayoutTrait && requires(TT self) { - { self.layout() } -> ConvertibleTo; + { self.layout() } -> ConvertibleTo; }; template concept DynImageView = ImageLayoutTrait && requires(TT self) { - { self.layout() } -> ConvertibleTo; + { self.layout() } -> ConvertibleTo; { self.pixelFormat() } -> ConvertibleTo; }; } // namespace concepts -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/image_test.cpp b/cpp/sophus2/concepts/image_test.cpp similarity index 84% rename from cpp/sophus/concepts/image_test.cpp rename to cpp/sophus2/concepts/image_test.cpp index b314e648..1a4c9a27 100644 --- a/cpp/sophus/concepts/image_test.cpp +++ b/cpp/sophus2/concepts/image_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/concepts/image.h" +#include "sophus2/concepts/image.h" #include -using namespace sophus; +using namespace sophus2; TEST(image, compile_test) {} diff --git a/cpp/sophus/concepts/lie_group.h b/cpp/sophus2/concepts/lie_group.h similarity index 98% rename from cpp/sophus/concepts/lie_group.h rename to cpp/sophus2/concepts/lie_group.h index 088ab30c..0dca467a 100644 --- a/cpp/sophus/concepts/lie_group.h +++ b/cpp/sophus2/concepts/lie_group.h @@ -7,9 +7,9 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/concepts/params.h" +#include "sophus2/concepts/params.h" -namespace sophus { +namespace sophus2 { template class UnitVector; @@ -275,4 +275,4 @@ concept LieGroup = LieGroupImpl && Params && }; } // namespace concepts -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/lie_group_prop_tests.h b/cpp/sophus2/concepts/lie_group_prop_tests.h similarity index 99% rename from cpp/sophus/concepts/lie_group_prop_tests.h rename to cpp/sophus2/concepts/lie_group_prop_tests.h index 90d4b4cf..fd2e9295 100644 --- a/cpp/sophus/concepts/lie_group_prop_tests.h +++ b/cpp/sophus2/concepts/lie_group_prop_tests.h @@ -8,13 +8,13 @@ #pragma once -#include "sophus/calculus/num_diff.h" -#include "sophus/concepts/lie_group.h" -#include "sophus/linalg/vector_space.h" +#include "sophus2/calculus/num_diff.h" +#include "sophus2/concepts/lie_group.h" +#include "sophus2/linalg/vector_space.h" #include // for matrix exp -namespace sophus { +namespace sophus2 { namespace test { template @@ -491,7 +491,7 @@ decltype(pointExamples()) pointExamples(); //! @endcond -// using namespace sophus; +// using namespace sophus2; // // bool contructorAndAssignmentTest() { // // bool passed = true; @@ -650,4 +650,4 @@ decltype(pointExamples()) // // return true; // // } } // namespace test -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/lie_group_test.cpp b/cpp/sophus2/concepts/lie_group_test.cpp similarity index 83% rename from cpp/sophus/concepts/lie_group_test.cpp rename to cpp/sophus2/concepts/lie_group_test.cpp index 3cb77338..f45b9601 100644 --- a/cpp/sophus/concepts/lie_group_test.cpp +++ b/cpp/sophus2/concepts/lie_group_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/concepts/lie_group.h" +#include "sophus2/concepts/lie_group.h" #include -using namespace sophus; +using namespace sophus2; TEST(lie_group, compile_test) {} diff --git a/cpp/sophus/concepts/manifold.h b/cpp/sophus2/concepts/manifold.h similarity index 92% rename from cpp/sophus/concepts/manifold.h rename to cpp/sophus2/concepts/manifold.h index 3e3be656..91ceef5a 100644 --- a/cpp/sophus/concepts/manifold.h +++ b/cpp/sophus2/concepts/manifold.h @@ -7,9 +7,9 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/concepts/params.h" +#include "sophus2/concepts/params.h" -namespace sophus { +namespace sophus2 { namespace concepts { template @@ -36,4 +36,4 @@ concept Manifold = Params && Tangent && BaseManifold && }; } // namespace concepts -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/manifold_prop_tests.h b/cpp/sophus2/concepts/manifold_prop_tests.h similarity index 95% rename from cpp/sophus/concepts/manifold_prop_tests.h rename to cpp/sophus2/concepts/manifold_prop_tests.h index 61043a33..58a79b80 100644 --- a/cpp/sophus/concepts/manifold_prop_tests.h +++ b/cpp/sophus2/concepts/manifold_prop_tests.h @@ -8,9 +8,9 @@ #pragma once -#include "sophus/concepts/manifold.h" +#include "sophus2/concepts/manifold.h" -namespace sophus { +namespace sophus2 { namespace test { template @@ -54,4 +54,4 @@ struct ManifoldPropTestSuite { }; } // namespace test -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/manifold_test.cpp b/cpp/sophus2/concepts/manifold_test.cpp similarity index 83% rename from cpp/sophus/concepts/manifold_test.cpp rename to cpp/sophus2/concepts/manifold_test.cpp index e98b83a7..b858a879 100644 --- a/cpp/sophus/concepts/manifold_test.cpp +++ b/cpp/sophus2/concepts/manifold_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/concepts/manifold.h" +#include "sophus2/concepts/manifold.h" #include -using namespace sophus; +using namespace sophus2; TEST(manifold, compile_test) {} diff --git a/cpp/sophus/concepts/params.h b/cpp/sophus2/concepts/params.h similarity index 74% rename from cpp/sophus/concepts/params.h rename to cpp/sophus2/concepts/params.h index 358cdfd4..da43adea 100644 --- a/cpp/sophus/concepts/params.h +++ b/cpp/sophus2/concepts/params.h @@ -8,11 +8,11 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/concepts/point.h" -#include "sophus/concepts/utils.h" +#include "sophus2/common/common.h" +#include "sophus2/concepts/point.h" +#include "sophus2/concepts/utils.h" -namespace sophus { +namespace sophus2 { namespace concepts { template @@ -21,7 +21,7 @@ concept ParamsImpl = std::is_same_v< Eigen::Vector> && requires(typename TT::Params params) { // constructors and factories - { TT::areParamsValid(params) } -> ConvertibleTo>; + { TT::areParamsValid(params) } -> ConvertibleTo>; { TT::paramsExamples() } -> ConvertibleTo>; @@ -67,23 +67,23 @@ struct CompatScalarEx { }; } // namespace concepts -} // namespace sophus +} // namespace sophus2 namespace Eigen { // This is mirrored from ceres::Jet. template -struct NumTraits> { - using Real = sophus::concepts::CompatScalarEx; - using NonInteger = sophus::concepts::CompatScalarEx; - using Nested = sophus::concepts::CompatScalarEx; - using Literal = sophus::concepts::CompatScalarEx; +struct NumTraits> { + using Real = sophus2::concepts::CompatScalarEx; + using NonInteger = sophus2::concepts::CompatScalarEx; + using Nested = sophus2::concepts::CompatScalarEx; + using Literal = sophus2::concepts::CompatScalarEx; static bool constexpr IsComplex = false; static bool constexpr IsInteger = false; - static typename sophus::concepts::CompatScalarEx dummy_precision() { - return sophus::concepts::CompatScalarEx(1e-12); + static typename sophus2::concepts::CompatScalarEx dummy_precision() { + return sophus2::concepts::CompatScalarEx(1e-12); } inline static Real epsilon() { @@ -102,17 +102,17 @@ struct NumTraits> { template struct ScalarBinaryOpTraits< - sophus::concepts::CompatScalarEx, + sophus2::concepts::CompatScalarEx, TT, BinaryOp> { - using ReturnType = sophus::concepts::CompatScalarEx; + using ReturnType = sophus2::concepts::CompatScalarEx; }; template struct ScalarBinaryOpTraits< TT, - sophus::concepts::CompatScalarEx, + sophus2::concepts::CompatScalarEx, BinaryOp> { - using ReturnType = sophus::concepts::CompatScalarEx; + using ReturnType = sophus2::concepts::CompatScalarEx; }; } // namespace Eigen diff --git a/cpp/sophus/concepts/params_test.cpp b/cpp/sophus2/concepts/params_test.cpp similarity index 84% rename from cpp/sophus/concepts/params_test.cpp rename to cpp/sophus2/concepts/params_test.cpp index ce932063..e9bffdf9 100644 --- a/cpp/sophus/concepts/params_test.cpp +++ b/cpp/sophus2/concepts/params_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/concepts/params.h" +#include "sophus2/concepts/params.h" #include -using namespace sophus; +using namespace sophus2; TEST(params, compile_test) {} diff --git a/cpp/sophus/concepts/point.h b/cpp/sophus2/concepts/point.h similarity index 97% rename from cpp/sophus/concepts/point.h rename to cpp/sophus2/concepts/point.h index b234a2a0..d90843f4 100644 --- a/cpp/sophus/concepts/point.h +++ b/cpp/sophus2/concepts/point.h @@ -9,11 +9,11 @@ #pragma once -#include "sophus/concepts/utils.h" +#include "sophus2/concepts/utils.h" #include -namespace sophus { +namespace sophus2 { namespace concepts { // These concept let us match Eigen's CRTP pattern and capture the true Derived @@ -52,7 +52,6 @@ concept EigenWithDimOrDynamic = EigenDenseType && TT::RowsAtCompileTime == kRows) && (TT::ColsAtCompileTime == Eigen::Dynamic || TT::ColsAtCompileTime == kCols); -; template concept RealScalarType = std::is_floating_point_v; @@ -81,4 +80,4 @@ template concept PointType = RealPointType || IntegerPointType; } // namespace concepts -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/point_test.cpp b/cpp/sophus2/concepts/point_test.cpp similarity index 84% rename from cpp/sophus/concepts/point_test.cpp rename to cpp/sophus2/concepts/point_test.cpp index 05627ecd..5448fe58 100644 --- a/cpp/sophus/concepts/point_test.cpp +++ b/cpp/sophus2/concepts/point_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/concepts/point.h" +#include "sophus2/concepts/point.h" #include -using namespace sophus; +using namespace sophus2; TEST(point, compile_test) {} diff --git a/cpp/sophus/concepts/utils.h b/cpp/sophus2/concepts/utils.h similarity index 96% rename from cpp/sophus/concepts/utils.h rename to cpp/sophus2/concepts/utils.h index 57966a40..9d8b78cd 100644 --- a/cpp/sophus/concepts/utils.h +++ b/cpp/sophus2/concepts/utils.h @@ -11,7 +11,7 @@ #include #include -namespace sophus { +namespace sophus2 { namespace concepts { template @@ -45,4 +45,4 @@ concept Range = requires(T& t) { }; } // namespace concepts -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/concepts/utils_test.cpp b/cpp/sophus2/concepts/utils_test.cpp similarity index 84% rename from cpp/sophus/concepts/utils_test.cpp rename to cpp/sophus2/concepts/utils_test.cpp index 6097d143..8fc66a7e 100644 --- a/cpp/sophus/concepts/utils_test.cpp +++ b/cpp/sophus2/concepts/utils_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/concepts/utils.h" +#include "sophus2/concepts/utils.h" #include -using namespace sophus; +using namespace sophus2; TEST(utils, compile_test) {} diff --git a/cpp/sophus/geometry/CMakeLists.txt b/cpp/sophus2/geometry/CMakeLists.txt similarity index 57% rename from cpp/sophus/geometry/CMakeLists.txt rename to cpp/sophus2/geometry/CMakeLists.txt index 61495239..d987a8e6 100644 --- a/cpp/sophus/geometry/CMakeLists.txt +++ b/cpp/sophus2/geometry/CMakeLists.txt @@ -1,12 +1,12 @@ #[[ -sophus_geometry +sophus2_geometry ]] -farm_ng_add_library(sophus_geometry - NAMESPACE Sophus +farm_ng_add_library(sophus2_geometry + NAMESPACE ${PROJECT_NAME} INCLUDE_DIR ../../ HEADERS fit_plane.h @@ -15,7 +15,7 @@ farm_ng_add_library(sophus_geometry point_transform.h ray.h ) -target_link_libraries(sophus_geometry INTERFACE sophus_lie) +target_link_libraries(sophus2_geometry INTERFACE sophus2_lie) if(${BUILD_SOPHUS_TESTS}) foreach(test_basename @@ -25,8 +25,8 @@ if(${BUILD_SOPHUS_TESTS}) point_transform ray) farm_ng_add_test(${test_basename} - PARENT_LIBRARY sophus_geometry - LINK_LIBRARIES sophus_geometry + PARENT_LIBRARY sophus2_geometry + LINK_LIBRARIES sophus2_geometry LABELS small) endforeach() endif() diff --git a/cpp/sophus/geometry/fit_plane.h b/cpp/sophus2/geometry/fit_plane.h similarity index 96% rename from cpp/sophus/geometry/fit_plane.h rename to cpp/sophus2/geometry/fit_plane.h index dfc6eef9..97b4c8a6 100644 --- a/cpp/sophus/geometry/fit_plane.h +++ b/cpp/sophus2/geometry/fit_plane.h @@ -11,7 +11,7 @@ #include #include -namespace sophus { +namespace sophus2 { inline auto fitPlaneToPoints(Eigen::Matrix3Xd const& points) -> Eigen::Hyperplane { @@ -38,4 +38,4 @@ auto fitPlaneToPoints(std::vector const& points) return fitPlaneToPoints(map.cast().eval()); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/geometry/fit_plane_test.cpp b/cpp/sophus2/geometry/fit_plane_test.cpp similarity index 76% rename from cpp/sophus/geometry/fit_plane_test.cpp rename to cpp/sophus2/geometry/fit_plane_test.cpp index 8f514d9a..e7a528c0 100644 --- a/cpp/sophus/geometry/fit_plane_test.cpp +++ b/cpp/sophus2/geometry/fit_plane_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/common/common.h" +#include "sophus2/common/common.h" #include -#include +#include -using namespace sophus; +using namespace sophus2; TEST(fit_plane, smoke) {} diff --git a/cpp/sophus/geometry/inverse_depth.h b/cpp/sophus2/geometry/inverse_depth.h similarity index 93% rename from cpp/sophus/geometry/inverse_depth.h rename to cpp/sophus2/geometry/inverse_depth.h index dab33f7f..1d0767de 100644 --- a/cpp/sophus/geometry/inverse_depth.h +++ b/cpp/sophus2/geometry/inverse_depth.h @@ -8,9 +8,9 @@ #pragma once -#include "sophus/linalg/homogeneous.h" +#include "sophus2/linalg/homogeneous.h" -namespace sophus { +namespace sophus2 { /// Inverse depth point representation /// @@ -60,7 +60,7 @@ class InverseDepthPoint3 { static auto fromEuclideanPoint3(Eigen::Matrix const& p) -> InverseDepthPoint3 { using std::abs; - SOPHUS_ASSERT_GE(abs(p.z()), sophus::kEpsilon); + SOPHUS_ASSERT_GE(abs(p.z()), sophus2::kEpsilon); return InverseDepthPoint3(p.x() / p.z(), p.y() / p.z(), 1.0 / p.z()); } @@ -74,7 +74,7 @@ class InverseDepthPoint3 { InverseDepthPoint3( Eigen::Matrix const& proj_in_z1_plane, TT const& one_by_z) : ab_and_psi_(proj_in_z1_plane[0], proj_in_z1_plane[1], one_by_z) { - SOPHUS_ASSERT_GE(ab_and_psi_.norm(), sophus::kEpsilon); + SOPHUS_ASSERT_GE(ab_and_psi_.norm(), sophus2::kEpsilon); } InverseDepthPoint3(TT const& x_by_z, TT const& y_by_z, TT const& one_by_z) @@ -102,7 +102,7 @@ class InverseDepthPoint3 { /// infinity. [[nodiscard]] auto toEuclideanPoint3() const -> Eigen::Matrix { using std::abs; - SOPHUS_ASSERT_GE(abs(psi()), sophus::kEpsilon); + SOPHUS_ASSERT_GE(abs(psi()), sophus2::kEpsilon); return Eigen::Matrix( ab_and_psi_.x() / psi(), ab_and_psi_.y() / psi(), TT(1) / psi()); @@ -114,4 +114,4 @@ class InverseDepthPoint3 { using InverseDepthPoint3F64 = InverseDepthPoint3; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/geometry/inverse_depth_test.cpp b/cpp/sophus2/geometry/inverse_depth_test.cpp similarity index 86% rename from cpp/sophus/geometry/inverse_depth_test.cpp rename to cpp/sophus2/geometry/inverse_depth_test.cpp index a1132c6a..5ff1b300 100644 --- a/cpp/sophus/geometry/inverse_depth_test.cpp +++ b/cpp/sophus2/geometry/inverse_depth_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/geometry/inverse_depth.h" +#include "sophus2/geometry/inverse_depth.h" -#include "sophus/lie/se3.h" +#include "sophus2/lie/se3.h" #include -using namespace sophus; +using namespace sophus2; TEST(inverse_depth, integrations) { for (Eigen::Vector3d const &point : {Eigen::Vector3d(0.1, 0.3, 2.0)}) { diff --git a/cpp/sophus/geometry/plane_conv.h b/cpp/sophus2/geometry/plane_conv.h similarity index 97% rename from cpp/sophus/geometry/plane_conv.h rename to cpp/sophus2/geometry/plane_conv.h index dc5a0e83..488eb5a2 100644 --- a/cpp/sophus/geometry/plane_conv.h +++ b/cpp/sophus2/geometry/plane_conv.h @@ -11,11 +11,11 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/lie/isometry2.h" -#include "sophus/lie/isometry3.h" +#include "sophus2/common/common.h" +#include "sophus2/lie/isometry2.h" +#include "sophus2/lie/isometry3.h" -namespace sophus { +namespace sophus2 { /// Takes in a rotation ``foo_rotation_plane`` and returns the corresponding /// line normal along the y-axis (in reference frame ``foo``). @@ -208,4 +208,4 @@ auto makeHyperplaneUnique(Eigen::Hyperplane const& plane) -plane.normal(), -plane.offset()); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/geometry/plane_conv_test.cpp b/cpp/sophus2/geometry/plane_conv_test.cpp similarity index 93% rename from cpp/sophus/geometry/plane_conv_test.cpp rename to cpp/sophus2/geometry/plane_conv_test.cpp index 8e8b2cc6..a51a9c5c 100644 --- a/cpp/sophus/geometry/plane_conv_test.cpp +++ b/cpp/sophus2/geometry/plane_conv_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/geometry/plane_conv.h" +#include "sophus2/geometry/plane_conv.h" #include -using namespace sophus; +using namespace sophus2; namespace { @@ -23,7 +23,7 @@ auto test2dGeometry() -> bool { // Roundtrip test: Eigen::Vector2 normal_in_foo = Eigen::Vector2::Random().normalized(); - sophus::Rotation2 foo_rotation_plane = + sophus2::Rotation2 foo_rotation_plane = rotation2FromNormal(normal_in_foo); Eigen::Vector2 result_normal_foo = normalFromRotation2(foo_rotation_plane); @@ -36,7 +36,7 @@ auto test2dGeometry() -> bool { makeHyperplaneUnique(Eigen::Hyperplane( Eigen::Vector2::Random().normalized(), Eigen::Vector2::Random())); - sophus::Isometry2 foo_from_plane = isometryFromLine(line_in_foo); + sophus2::Isometry2 foo_from_plane = isometryFromLine(line_in_foo); Eigen::Hyperplane result_plane_foo = lineFromIsometry(foo_from_plane); SOPHUS_ASSERT_WITHIN_REL( @@ -105,7 +105,7 @@ auto test3dGeometry() -> bool { // Roundtrip test: Eigen::Vector3 normal_in_foo = Eigen::Vector3::Random().normalized(); - sophus::Rotation3 foo_rotation_plane = + sophus2::Rotation3 foo_rotation_plane = rotation3FromPlane(normal_in_foo); Eigen::Vector3 result_normal_foo = normalFromRotation3(foo_rotation_plane); @@ -118,7 +118,8 @@ auto test3dGeometry() -> bool { makeHyperplaneUnique(Eigen::Hyperplane( Eigen::Vector3::Random().normalized(), Eigen::Vector3::Random())); - sophus::Isometry3 foo_from_plane = isometryFromPlane(plane_in_foo); + sophus2::Isometry3 foo_from_plane = + isometryFromPlane(plane_in_foo); Eigen::Hyperplane result_plane_foo = planeFromIsometry(foo_from_plane); SOPHUS_ASSERT_WITHIN_REL( @@ -154,4 +155,4 @@ TEST(plane_conv, integrations) { test3dGeometry(); } -// int main() { sophus::runAll(); } +// int main() { sophus2::runAll(); } diff --git a/cpp/sophus/geometry/point_transform.h b/cpp/sophus2/geometry/point_transform.h similarity index 92% rename from cpp/sophus/geometry/point_transform.h rename to cpp/sophus2/geometry/point_transform.h index 05356947..a030b64b 100644 --- a/cpp/sophus/geometry/point_transform.h +++ b/cpp/sophus2/geometry/point_transform.h @@ -8,11 +8,11 @@ #pragma once -#include "sophus/geometry/inverse_depth.h" -#include "sophus/lie/se3.h" -#include "sophus/sensor/camera_projection/projection_z1.h" +#include "sophus2/geometry/inverse_depth.h" +#include "sophus2/lie/se3.h" +#include "sophus2/sensor/camera_projection/projection_z1.h" -namespace sophus { +namespace sophus2 { /// Projects 3-point (a,b,psi) = (x/z,y/z,1/z) through the origin (0,0,0) onto /// the plane z=1. Hence it returns (a,b) = (x/z, y/z). @@ -65,7 +65,7 @@ auto dxProjExpXPointAt0(InverseDepthPoint3 const& inverse_depth_point) /// for psi!=0. template auto scaledTransform( - sophus::Isometry3 const& foo_from_bar, + sophus2::Isometry3 const& foo_from_bar, InverseDepthPoint3 const& inverse_depth_point_in_bar) -> Eigen::Matrix { return foo_from_bar.so3() * @@ -85,7 +85,7 @@ auto scaledTransform( /// +/- infinity. template auto projTransform( - sophus::Isometry3 const& foo_from_bar, + sophus2::Isometry3 const& foo_from_bar, InverseDepthPoint3 const& inverse_depth_point_in_bar) -> Eigen::Matrix { // R * (x,y,z) + t @@ -107,7 +107,7 @@ auto projTransform( /// Functor to efficiently transform a number of point given a Isometry3 pose. /// -/// When transforming a point `point_in_bar` given a sophus::Isometry3 pose +/// When transforming a point `point_in_bar` given a sophus2::Isometry3 pose /// `foo_from_bar`, one can simply use /// /// ``Eigen::Vector3d = foo_from_bar * point_in_bar;`` @@ -128,7 +128,7 @@ template class PointTransformer { public: PointTransformer() = default; - explicit PointTransformer(sophus::Isometry3 const& foo_from_bar) + explicit PointTransformer(sophus2::Isometry3 const& foo_from_bar) : foo_from_bar_(foo_from_bar), foo_rotation_bar_(foo_from_bar.so3().matrix()), bar_origin_in_foo_(foo_from_bar.translation()) {} @@ -203,7 +203,7 @@ class PointTransformer { mat_j; } - [[nodiscard]] auto fooFromBar() const -> sophus::Isometry3 const& { + [[nodiscard]] auto fooFromBar() const -> sophus2::Isometry3 const& { return foo_from_bar_; } [[nodiscard]] auto fooRotationBar() const -> Eigen::Matrix const& { @@ -214,8 +214,8 @@ class PointTransformer { } private: - sophus::Isometry3 foo_from_bar_; + sophus2::Isometry3 foo_from_bar_; Eigen::Matrix foo_rotation_bar_; Eigen::Matrix bar_origin_in_foo_; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/geometry/point_transform_test.cpp b/cpp/sophus2/geometry/point_transform_test.cpp similarity index 93% rename from cpp/sophus/geometry/point_transform_test.cpp rename to cpp/sophus2/geometry/point_transform_test.cpp index 4aefdb7f..6ff60e0d 100644 --- a/cpp/sophus/geometry/point_transform_test.cpp +++ b/cpp/sophus2/geometry/point_transform_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/geometry/point_transform.h" +#include "sophus2/geometry/point_transform.h" -#include "sophus/calculus/num_diff.h" -#include "sophus/sensor/camera_projection/projection_z1.h" +#include "sophus2/calculus/num_diff.h" +#include "sophus2/sensor/camera_projection/projection_z1.h" #include -using namespace sophus; +using namespace sophus2; TEST(inverse_depth, integrations) { for (InverseDepthPoint3F64 const& inv_dept_point : @@ -91,10 +91,10 @@ TEST(point_transform, integrations) { point_vec.push_back(Eigen::Vector3d(1, -3, 0.5)); point_vec.push_back(Eigen::Vector3d(-5, -6, 7)); - double const eps = sophus::kEpsilon; + double const eps = sophus2::kEpsilon; for (Eigen::Vector const& t : tangent_vec) { - sophus::SE3d foo_from_bar_isometry = sophus::SE3d::exp(t); + sophus2::SE3d foo_from_bar_isometry = sophus2::SE3d::exp(t); PointTransformer foo_from_bar(foo_from_bar_isometry); // For points not at infinity @@ -129,14 +129,14 @@ TEST(point_transform, integrations) { vectorFieldNumDiff( [&](Eigen::Vector const& vec_a) { return proj( - sophus::Isometry3F64::exp(vec_a) * + sophus2::Isometry3F64::exp(vec_a) * foo_from_bar.transform(point_in_bar)); }, zero); Eigen::Matrix const num_dx2 = vectorFieldNumDiff( [&](Eigen::Vector const& vec_a) { - sophus::Isometry3F64 exp_a = sophus::Isometry3F64::exp(vec_a); + sophus2::Isometry3F64 exp_a = sophus2::Isometry3F64::exp(vec_a); return proj( exp_a.so3().matrix() * foo_from_bar.scaledTransform(inverse_depth_in_bar) + @@ -183,7 +183,7 @@ TEST(point_transform, integrations) { Eigen::Matrix const num_dx2 = vectorFieldNumDiff( [&](Eigen::Vector const& vec_a) { - sophus::Isometry3F64 exp_a = sophus::Isometry3F64::exp(vec_a); + sophus2::Isometry3F64 exp_a = sophus2::Isometry3F64::exp(vec_a); return proj( exp_a.so3().matrix() * foo_from_bar.scaledTransform(inverse_depth_in_bar) + diff --git a/cpp/sophus/geometry/ray.h b/cpp/sophus2/geometry/ray.h similarity index 94% rename from cpp/sophus/geometry/ray.h rename to cpp/sophus2/geometry/ray.h index a9dba552..346640f6 100644 --- a/cpp/sophus/geometry/ray.h +++ b/cpp/sophus2/geometry/ray.h @@ -8,18 +8,18 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/lie/isometry2.h" -#include "sophus/lie/isometry3.h" -#include "sophus/lie/similarity2.h" -#include "sophus/lie/similarity3.h" -#include "sophus/manifold/unit_vector.h" +#include "sophus2/common/common.h" +#include "sophus2/lie/isometry2.h" +#include "sophus2/lie/isometry3.h" +#include "sophus2/lie/similarity2.h" +#include "sophus2/lie/similarity3.h" +#include "sophus2/manifold/unit_vector.h" #include #include -namespace sophus { +namespace sophus2 { // Forward declarations template @@ -72,7 +72,7 @@ class Ray { const -> std::optional { using std::abs; TScalar dot_prod = plane.normal().dot(this->direction_.vector()); - if (abs(dot_prod) < sophus::kEpsilon) { + if (abs(dot_prod) < sophus2::kEpsilon) { return std::nullopt; } IntersectionResult result; @@ -203,4 +203,4 @@ auto closestApproach(Ray3 const& line_0, Ray3 const& line_1) static_cast(2.0); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/geometry/ray_test.cpp b/cpp/sophus2/geometry/ray_test.cpp similarity index 95% rename from cpp/sophus/geometry/ray_test.cpp rename to cpp/sophus2/geometry/ray_test.cpp index d2e1d96b..6052c71c 100644 --- a/cpp/sophus/geometry/ray_test.cpp +++ b/cpp/sophus2/geometry/ray_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/geometry/ray.h" +#include "sophus2/geometry/ray.h" #include -using namespace sophus; +using namespace sophus2; TEST(ray, simple_ray3) { auto const dir1 = UnitVector3::fromUnitVector({0.0, 0.0, 1.0}); diff --git a/cpp/sophus2/image/CMakeLists.txt b/cpp/sophus2/image/CMakeLists.txt new file mode 100644 index 00000000..411c9b73 --- /dev/null +++ b/cpp/sophus2/image/CMakeLists.txt @@ -0,0 +1,51 @@ +#[[ +sophus2_image + + + +]] + +set(sophus2_image_src_prefixes + interpolation + image + image_size + layout + image_types + image_view + mut_image + mut_image_view + mut_dyn_image + mut_dyn_image_view + dyn_image + dyn_image_view + dyn_image_types + pixel_format +) + +set(sophus2_image_cpp) +set(sophus2_image_h) +foreach(x ${sophus2_image_src_prefixes} ) + list(APPEND sophus2_image_cpp ${x}.cpp) + list(APPEND sophus2_image_h ${x}.h) +endforeach() + +farm_ng_add_library(sophus2_image + NAMESPACE ${PROJECT_NAME} + INCLUDE_DIR ../../ + HEADERS + ${sophus2_image_h} + SOURCES + ${sophus2_image_cpp} +) +target_link_libraries(sophus2_image sophus2_geometry) + + + +if(${BUILD_SOPHUS_TESTS}) + foreach(test_basename ${sophus2_image_src_prefixes}) + farm_ng_add_test(${test_basename} + PARENT_LIBRARY sophus2_image + LINK_LIBRARIES sophus2_image + LABELS small) + endforeach() +endif() diff --git a/cpp/sophus/image/dyn_image.cpp b/cpp/sophus2/image/dyn_image.cpp similarity index 77% rename from cpp/sophus/image/dyn_image.cpp rename to cpp/sophus2/image/dyn_image.cpp index d5f2963c..ff4bcb40 100644 --- a/cpp/sophus/image/dyn_image.cpp +++ b/cpp/sophus2/image/dyn_image.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/dyn_image.h" +#include "sophus2/image/dyn_image.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/image/dyn_image.h b/cpp/sophus2/image/dyn_image.h similarity index 97% rename from cpp/sophus/image/dyn_image.h rename to cpp/sophus2/image/dyn_image.h index aaa929fc..2a578f2c 100644 --- a/cpp/sophus/image/dyn_image.h +++ b/cpp/sophus2/image/dyn_image.h @@ -8,12 +8,12 @@ #pragma once -#include "sophus/image/dyn_image_view.h" -#include "sophus/image/mut_dyn_image.h" +#include "sophus2/image/dyn_image_view.h" +#include "sophus2/image/mut_dyn_image.h" #include -namespace sophus { +namespace sophus2 { /// Type-erased image with shared ownership, and read-only access to pixels. /// Type is nullable. @@ -160,4 +160,4 @@ class DynImage : public DynImageView { std::shared_ptr shared_; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/dyn_image_test.cpp b/cpp/sophus2/image/dyn_image_test.cpp similarity index 99% rename from cpp/sophus/image/dyn_image_test.cpp rename to cpp/sophus2/image/dyn_image_test.cpp index 2a4811d1..b7802d7c 100644 --- a/cpp/sophus/image/dyn_image_test.cpp +++ b/cpp/sophus2/image/dyn_image_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/dyn_image_types.h" +#include "sophus2/image/dyn_image_types.h" #include -using namespace sophus; +using namespace sophus2; #define SOPHUS_TEST_IMG_EQ(lhs, rhs) /* NOLINT*/ \ do { \ diff --git a/cpp/sophus/image/dyn_image_types.cpp b/cpp/sophus2/image/dyn_image_types.cpp similarity index 75% rename from cpp/sophus/image/dyn_image_types.cpp rename to cpp/sophus2/image/dyn_image_types.cpp index 92c4eefe..35e37b18 100644 --- a/cpp/sophus/image/dyn_image_types.cpp +++ b/cpp/sophus2/image/dyn_image_types.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/dyn_image_types.h" +#include "sophus2/image/dyn_image_types.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/image/dyn_image_types.h b/cpp/sophus2/image/dyn_image_types.h similarity index 97% rename from cpp/sophus/image/dyn_image_types.h rename to cpp/sophus2/image/dyn_image_types.h index cb471c79..d33a366d 100644 --- a/cpp/sophus/image/dyn_image_types.h +++ b/cpp/sophus2/image/dyn_image_types.h @@ -8,10 +8,10 @@ #pragma once -#include "sophus/image/dyn_image.h" -#include "sophus/image/mut_dyn_image.h" +#include "sophus2/image/dyn_image.h" +#include "sophus2/image/mut_dyn_image.h" -namespace sophus { +namespace sophus2 { /// Image representing any number of channels (>=1) and any floating and /// unsigned integral channel type. @@ -127,4 +127,4 @@ void visitImage(TUserFunc&& func, DynImageView const& image) { visit(std::forward(func), image); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/dyn_image_types_test.cpp b/cpp/sophus2/image/dyn_image_types_test.cpp similarity index 81% rename from cpp/sophus/image/dyn_image_types_test.cpp rename to cpp/sophus2/image/dyn_image_types_test.cpp index 8227ff3c..86c9a36c 100644 --- a/cpp/sophus/image/dyn_image_types_test.cpp +++ b/cpp/sophus2/image/dyn_image_types_test.cpp @@ -6,8 +6,8 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/dyn_image_types.h" +#include "sophus2/image/dyn_image_types.h" #include -using namespace sophus; +using namespace sophus2; diff --git a/cpp/sophus/image/dyn_image_view.cpp b/cpp/sophus2/image/dyn_image_view.cpp similarity index 76% rename from cpp/sophus/image/dyn_image_view.cpp rename to cpp/sophus2/image/dyn_image_view.cpp index af22c4cc..1a5fabf2 100644 --- a/cpp/sophus/image/dyn_image_view.cpp +++ b/cpp/sophus2/image/dyn_image_view.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/dyn_image_view.h" +#include "sophus2/image/dyn_image_view.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/image/dyn_image_view.h b/cpp/sophus2/image/dyn_image_view.h similarity index 94% rename from cpp/sophus/image/dyn_image_view.h rename to cpp/sophus2/image/dyn_image_view.h index c57963c4..74df1831 100644 --- a/cpp/sophus/image/dyn_image_view.h +++ b/cpp/sophus2/image/dyn_image_view.h @@ -8,13 +8,13 @@ #pragma once -#include "sophus/image/image.h" -#include "sophus/image/image_types.h" -#include "sophus/image/pixel_format.h" +#include "sophus2/image/image.h" +#include "sophus2/image/image_types.h" +#include "sophus2/image/pixel_format.h" #include -namespace sophus { +namespace sophus2 { struct AnyImagePredicate { template @@ -80,7 +80,7 @@ class DynImageView { } /// Returns subview with shared ownership semantics of whole image. - [[nodiscard]] auto subview(Eigen::Vector2i uv, sophus::ImageSize size) const + [[nodiscard]] auto subview(Eigen::Vector2i uv, sophus2::ImageSize size) const -> DynImageView { SOPHUS_ASSERT(imageSize().contains(uv)); SOPHUS_ASSERT_LE(uv.x() + size.width, this->layout_.width()); @@ -147,4 +147,4 @@ class DynImageView { PixelFormat pixel_format_; uint8_t const* ptr_ = nullptr; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/dyn_image_view_test.cpp b/cpp/sophus2/image/dyn_image_view_test.cpp similarity index 97% rename from cpp/sophus/image/dyn_image_view_test.cpp rename to cpp/sophus2/image/dyn_image_view_test.cpp index d7d8bb7c..afd07e0e 100644 --- a/cpp/sophus/image/dyn_image_view_test.cpp +++ b/cpp/sophus2/image/dyn_image_view_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/dyn_image_types.h" +#include "sophus2/image/dyn_image_types.h" #include -using namespace sophus; +using namespace sophus2; #define SOPHUS_TEST_IMG_EQ(lhs, rhs) /* NOLINT*/ \ do { \ diff --git a/cpp/sophus/image/image.cpp b/cpp/sophus2/image/image.cpp similarity index 78% rename from cpp/sophus/image/image.cpp rename to cpp/sophus2/image/image.cpp index 6de59504..ccd55125 100644 --- a/cpp/sophus/image/image.cpp +++ b/cpp/sophus2/image/image.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/image.h" +#include "sophus2/image/image.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/image/image.h b/cpp/sophus2/image/image.h similarity index 90% rename from cpp/sophus/image/image.h rename to cpp/sophus2/image/image.h index 9e5c212e..d2d8ae1b 100644 --- a/cpp/sophus/image/image.h +++ b/cpp/sophus2/image/image.h @@ -13,12 +13,12 @@ /// for details. #pragma once -#include "sophus/common/enum.h" -#include "sophus/image/mut_image.h" +#include "sophus2/common/enum.h" +#include "sophus2/image/mut_image.h" #include -namespace sophus { +namespace sophus2 { template class DynImage; @@ -104,6 +104,10 @@ class Image : public ImageView { this->setViewToEmpty(); } + /// This shadows implementation from ImageView to avoid the copy. We already + /// have a shared Image. + Image toShared() const { return *this; } + private: template friend class MutImage; @@ -119,4 +123,9 @@ class Image : public ImageView { std::shared_ptr shared_; }; -} // namespace sophus +template +Image ImageView::toShared() const { + return Image::makeCopyFrom(*this); +} + +} // namespace sophus2 diff --git a/cpp/sophus/image/image_size.cpp b/cpp/sophus2/image/image_size.cpp similarity index 95% rename from cpp/sophus/image/image_size.cpp rename to cpp/sophus2/image/image_size.cpp index e288c61d..6d7895b8 100644 --- a/cpp/sophus/image/image_size.cpp +++ b/cpp/sophus2/image/image_size.cpp @@ -6,9 +6,9 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/image_size.h" +#include "sophus2/image/image_size.h" -namespace sophus { +namespace sophus2 { auto ImageSize::contains(Eigen::Vector2i const& obs, int border) const -> bool { return obs.x() >= border && obs.x() < this->iwidth() - border && @@ -49,4 +49,4 @@ auto operator<<(std::ostream& os, ImageSize const& image_size) return os; } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/image_size.h b/cpp/sophus2/image/image_size.h similarity index 91% rename from cpp/sophus/image/image_size.h rename to cpp/sophus2/image/image_size.h index 4e0d3f27..f411cb1c 100644 --- a/cpp/sophus/image/image_size.h +++ b/cpp/sophus2/image/image_size.h @@ -8,14 +8,15 @@ #pragma once -#include "sophus/calculus/region.h" -#include "sophus/common/common.h" +#include "sophus2/calculus/region.h" +#include "sophus2/common/common.h" #include +#include #include -namespace sophus { +namespace sophus2 { /// Image size, hence its width and height. struct ImageSize { @@ -83,4 +84,7 @@ auto operator<(ImageSize const& lhs, ImageSize const& rhs) -> bool; /// Ostream operator. auto operator<<(std::ostream& os, ImageSize const& image_size) -> std::ostream&; -} // namespace sophus +} // namespace sophus2 + +template <> +struct fmt::formatter : ostream_formatter {}; diff --git a/cpp/sophus/image/image_size_test.cpp b/cpp/sophus2/image/image_size_test.cpp similarity index 83% rename from cpp/sophus/image/image_size_test.cpp rename to cpp/sophus2/image/image_size_test.cpp index e390607b..de330293 100644 --- a/cpp/sophus/image/image_size_test.cpp +++ b/cpp/sophus2/image/image_size_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/image_size.h" +#include "sophus2/image/image_size.h" #include -using namespace sophus; +using namespace sophus2; TEST(image_size, smoke) {} diff --git a/cpp/sophus/image/image_test.cpp b/cpp/sophus2/image/image_test.cpp similarity index 93% rename from cpp/sophus/image/image_test.cpp rename to cpp/sophus2/image/image_test.cpp index 0bfae4ef..6e7d9674 100644 --- a/cpp/sophus/image/image_test.cpp +++ b/cpp/sophus2/image/image_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/image.h" +#include "sophus2/image/image.h" -#include "sophus/image/image_types.h" +#include "sophus2/image/image_types.h" #include -using namespace sophus; +using namespace sophus2; TEST(Image, empty_and_non_empty) { const ImageSize size64{6, 4}; diff --git a/cpp/sophus/image/image_types.cpp b/cpp/sophus2/image/image_types.cpp similarity index 95% rename from cpp/sophus/image/image_types.cpp rename to cpp/sophus2/image/image_types.cpp index 57094c77..50929a26 100644 --- a/cpp/sophus/image/image_types.cpp +++ b/cpp/sophus2/image/image_types.cpp @@ -6,9 +6,9 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/image_types.h" +#include "sophus2/image/image_types.h" -namespace sophus { +namespace sophus2 { auto count(ImageViewBool mask, bool truth_value) -> int { return mask.reduce( @@ -71,4 +71,4 @@ auto firstFalsePixel(ImageViewBool mask) -> std::optional { return firstPixel(mask, false); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/image_types.h b/cpp/sophus2/image/image_types.h similarity index 86% rename from cpp/sophus/image/image_types.h rename to cpp/sophus2/image/image_types.h index d7e6855c..fa54f85d 100644 --- a/cpp/sophus/image/image_types.h +++ b/cpp/sophus2/image/image_types.h @@ -8,10 +8,10 @@ #pragma once -#include "sophus/common/enum.h" -#include "sophus/image/image.h" +#include "sophus2/common/enum.h" +#include "sophus2/image/image.h" -namespace sophus { +namespace sophus2 { // Pixel type defs: template @@ -132,6 +132,27 @@ struct ImageTraits> { std::is_floating_point_v || std::is_unsigned_v); }; +/// Visits the corresponding pixels of two ImageViews +/// +/// Precondition: `lhs` and `rhs` are the same size and not empty +template +auto visit( + ImageView lhs, ImageView rhs, TFunc const& user_function) { + SOPHUS_ASSERT(!lhs.isEmpty()); + SOPHUS_ASSERT(!rhs.isEmpty()); + SOPHUS_ASSERT_EQ(lhs.imageSize(), rhs.imageSize()); + + for (int v = 0; v < lhs.height(); ++v) { + TPixel const* lhs_p = lhs.rowPtr(v); + TPixel const* rhs_p = rhs.rowPtr(v); + + TPixel const* end_of_row = lhs_p + lhs.width(); + for (; lhs_p != end_of_row; ++lhs_p, ++rhs_p) { + user_function(*lhs_p, *rhs_p); + } + } +} + /// Returns boolean image with the result per pixel: /// /// mask(..) = lhs(..) == rhs (..) @@ -206,32 +227,32 @@ auto firstFalsePixel(ImageViewBool mask) -> std::optional; /// If it is false that `left_image` == `right_image`, print formatted error /// message and then panic. -#define SOPHUS_ASSERT_IMAGE_EQ(left_image, right_image, ...) \ - SOPHUS_ASSERT_EQ( \ - (left_image).imageSize(), \ - (right_image).imageSize(), \ - "Inside: SOPHUS_ASSERT_IMAGE_EQ."); \ - do { \ - if (!(left_image).hasSameData(right_image)) { \ - ::sophus::MutImageBool mask = isEqualMask((left_image), (right_image)); \ - FARM_IMPL_LOG_HEADER("SOPHUS_ASSERT_IMAGE_EQ failed"); \ - FARM_IMPL_LOG_PRINTLN( \ - "Number of pixel failing: {} / {}", \ - countFalse(mask), \ - mask.imageSize().area()); \ - auto maybe_uv = firstFalsePixel(mask); \ - ::Eigen::Vector2i uv = SOPHUS_UNWRAP(maybe_uv); \ - int u = uv[0]; \ - int v = uv[1]; \ - FARM_IMPL_LOG_PRINTLN( \ - "First failed pixel: ({},{}).\nLeft:\n{}\nRigth:\n{}", \ - u, \ - v, \ - left_image(u, v), \ - right_image(u, v)); \ - FARM_IMPL_LOG_PRINTLN(__VA_ARGS__); \ - FARM_IMPL_ABORT(); \ - } \ +#define SOPHUS_ASSERT_IMAGE_EQ(left_image, right_image, ...) \ + SOPHUS_ASSERT_EQ( \ + (left_image).imageSize(), \ + (right_image).imageSize(), \ + "Inside: SOPHUS_ASSERT_IMAGE_EQ."); \ + do { \ + if (!(left_image).hasSameData(right_image)) { \ + ::sophus2::MutImageBool mask = isEqualMask((left_image), (right_image)); \ + FARM_IMPL_LOG_HEADER("SOPHUS_ASSERT_IMAGE_EQ failed"); \ + FARM_IMPL_LOG_PRINTLN( \ + "Number of pixel failing: {} / {}", \ + countFalse(mask), \ + mask.imageSize().area()); \ + auto maybe_uv = firstFalsePixel(mask); \ + ::Eigen::Vector2i uv = SOPHUS_UNWRAP(maybe_uv); \ + int u = uv[0]; \ + int v = uv[1]; \ + FARM_IMPL_LOG_PRINTLN( \ + "First failed pixel: ({},{}).\nLeft:\n{}\nRigth:\n{}", \ + u, \ + v, \ + left_image(u, v), \ + right_image(u, v)); \ + FARM_IMPL_LOG_PRINTLN(__VA_ARGS__); \ + FARM_IMPL_ABORT(); \ + } \ } while (false) -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/image_types_test.cpp b/cpp/sophus2/image/image_types_test.cpp similarity index 76% rename from cpp/sophus/image/image_types_test.cpp rename to cpp/sophus2/image/image_types_test.cpp index ca68012e..024b4ceb 100644 --- a/cpp/sophus/image/image_types_test.cpp +++ b/cpp/sophus2/image/image_types_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/image_types.h" +#include "sophus2/image/image_types.h" #include -using namespace sophus; +using namespace sophus2; TEST(ImageBool, logic) { // clang-format off @@ -41,6 +41,19 @@ TEST(ImageBool, logic) { SOPHUS_ASSERT_EQ(first_true_pixel, Eigen::Vector2i(0, 0)); } +TEST(image_types, visit) { + auto const kImageSize = sophus2::ImageSize(2, 2); + sophus2::ImageF32 img_a = sophus2::ImageF32::makeGenerative( + kImageSize, [&](int u, int v) { return v * kImageSize.width + u; }); + sophus2::ImageF32 img_b = sophus2::ImageF32::makeGenerative( + kImageSize, [&](int u, int v) { return -(v * kImageSize.width + u); }); + visit(img_a, img_b, [](float a, float b) { + EXPECT_GE(a, 0); + EXPECT_LE(b, 0); + EXPECT_EQ(a, -b); + }); +} + TEST(SOPHUS_ASSERT_IMAGE_EQ, death_test) { MutImage mut_image({2, 3}); mut_image.fill(0.25f); diff --git a/cpp/sophus/image/image_view.cpp b/cpp/sophus2/image/image_view.cpp similarity index 76% rename from cpp/sophus/image/image_view.cpp rename to cpp/sophus2/image/image_view.cpp index f195ece0..08cecbaf 100644 --- a/cpp/sophus/image/image_view.cpp +++ b/cpp/sophus2/image/image_view.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/image_view.h" +#include "sophus2/image/image_view.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/image/image_view.h b/cpp/sophus2/image/image_view.h similarity index 90% rename from cpp/sophus/image/image_view.h rename to cpp/sophus2/image/image_view.h index 31938b17..59404792 100644 --- a/cpp/sophus/image/image_view.h +++ b/cpp/sophus2/image/image_view.h @@ -21,10 +21,10 @@ #pragma once -#include "sophus/calculus/region.h" -#include "sophus/image/layout.h" +#include "sophus2/calculus/region.h" +#include "sophus2/image/layout.h" -namespace sophus { +namespace sophus2 { // Types are largely inspired / derived from Pangolin. @@ -64,7 +64,7 @@ struct ImageView { /// Creates view from image size and pointer to first pixel. The image is /// assumed to be contiguous and the pitch is set accordingly. - explicit ImageView(sophus::ImageSize image_size, TPixel const* ptr) noexcept + explicit ImageView(sophus2::ImageSize image_size, TPixel const* ptr) noexcept : ImageView(ImageLayout::makeFromSize(image_size), ptr) {} /// Returns true if view is empty. @@ -77,7 +77,7 @@ struct ImageView { /// Returns ImageSize. /// It is {0,0} if view is empty. - [[nodiscard]] auto imageSize() const -> sophus::ImageSize const& { + [[nodiscard]] auto imageSize() const -> sophus2::ImageSize const& { return layout_.imageSize(); } @@ -141,7 +141,7 @@ struct ImageView { [[nodiscard]] auto ptr() const -> TPixel const* { return ptr_; } /// Returns subview. - [[nodiscard]] auto subview(Eigen::Vector2i uv, sophus::ImageSize size) const + [[nodiscard]] auto subview(Eigen::Vector2i uv, sophus2::ImageSize size) const -> ImageView { SOPHUS_ASSERT(colInBounds(uv[0])); SOPHUS_ASSERT(rowInBounds(uv[1])); @@ -235,6 +235,17 @@ struct ImageView { return true; } + /// Returns an owned and shared Image. This often requires a copy of the data. + /// + /// The implementation is in image.h + Image> toShared() const; + + /// Creates a copy of the data in the image view and returns a owned and + /// mutable MutImage. + /// + /// The implementation is in mut_image.h + MutImage> cloneAsMut() const; + protected: /// Resets view such that it is empty. void setViewToEmpty() { *this = {}; } @@ -253,7 +264,7 @@ struct ImageView { namespace details { template -auto finiteInterval(sophus::ImageView const& image) -> Region { +auto finiteInterval(sophus2::ImageView const& image) -> Region { return image.reduce( [](TPixel v, auto& min_max) { if (isFinite(v)) { @@ -266,7 +277,7 @@ auto finiteInterval(sophus::ImageView const& image) -> Region { // TODO: make member function? template inline auto imageCoordsInterval( - sophus::ImageView const& image, int border = 0) -> Region2I { + sophus2::ImageView const& image, int border = 0) -> Region2I { return imageCoordsInterval(image.imageSize(), border); } @@ -295,8 +306,8 @@ template } } // namespace details -} // namespace sophus +} // namespace sophus2 -#define SOPHUS_PIXEL_MUT(img, u, v, ...) \ - ::sophus::details::checkedPixelAccessMut( \ +#define SOPHUS_PIXEL_MUT(img, u, v, ...) \ + ::sophus2::details::checkedPixelAccessMut( \ img, u, v, __FILE__, __LINE__, SOPHUS_FORMAT(__VA_ARGS__)) diff --git a/cpp/sophus/image/image_view_test.cpp b/cpp/sophus2/image/image_view_test.cpp similarity index 95% rename from cpp/sophus/image/image_view_test.cpp rename to cpp/sophus2/image/image_view_test.cpp index 3de4e2bd..f5d20f01 100644 --- a/cpp/sophus/image/image_view_test.cpp +++ b/cpp/sophus2/image/image_view_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/image_view.h" +#include "sophus2/image/image_view.h" -#include "sophus/image/image_types.h" +#include "sophus2/image/image_types.h" #include -using namespace sophus; +using namespace sophus2; TEST(ImageView, empty) { ImageView view; diff --git a/cpp/sophus/image/interpolation.cpp b/cpp/sophus2/image/interpolation.cpp similarity index 76% rename from cpp/sophus/image/interpolation.cpp rename to cpp/sophus2/image/interpolation.cpp index 89b416f1..447936ea 100644 --- a/cpp/sophus/image/interpolation.cpp +++ b/cpp/sophus2/image/interpolation.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/interpolation.h" +#include "sophus2/image/interpolation.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/image/interpolation.h b/cpp/sophus2/image/interpolation.h similarity index 82% rename from cpp/sophus/image/interpolation.h rename to cpp/sophus2/image/interpolation.h index 36aca777..15ac9155 100644 --- a/cpp/sophus/image/interpolation.h +++ b/cpp/sophus2/image/interpolation.h @@ -8,18 +8,19 @@ #pragma once -#include "sophus/image/image.h" -#include "sophus/image/image_view.h" +#include "sophus2/image/image.h" +#include "sophus2/image/image_view.h" -namespace sophus { +namespace sophus2 { // Bilinearly interpolates ``img`` at ``uv``. // // Preconditions: // - uv must be inside the image boundary template -auto interpolate(sophus::ImageView const& image, Eigen::Vector2f uv) -> TT { - sophus::ImageSize image_size = image.imageSize(); +auto interpolate(sophus2::ImageView const& image, Eigen::Vector2f uv) + -> TT { + sophus2::ImageSize image_size = image.imageSize(); SOPHUS_ASSERT( image_size.contains(uv), "{} vs {}", image_size, uv.transpose()); float iu = NAN; @@ -44,4 +45,4 @@ auto interpolate(sophus::ImageView const& image, Eigen::Vector2f uv) -> TT { return val; } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/interpolation_test.cpp b/cpp/sophus2/image/interpolation_test.cpp similarity index 92% rename from cpp/sophus/image/interpolation_test.cpp rename to cpp/sophus2/image/interpolation_test.cpp index c224c566..aa91c310 100644 --- a/cpp/sophus/image/interpolation_test.cpp +++ b/cpp/sophus2/image/interpolation_test.cpp @@ -6,17 +6,17 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/interpolation.h" +#include "sophus2/image/interpolation.h" #include -using namespace sophus; +using namespace sophus2; TEST(interpolation, unit) { int w = 3; int h = 2; - sophus::MutImage img({3, 2}); + sophus2::MutImage img({3, 2}); img.fill(0.f); for (float v = 0; v <= h - 1.f; v += 0.1) { diff --git a/cpp/sophus/image/layout.cpp b/cpp/sophus2/image/layout.cpp similarity index 91% rename from cpp/sophus/image/layout.cpp rename to cpp/sophus2/image/layout.cpp index 5b1eacd0..a8172ffe 100644 --- a/cpp/sophus/image/layout.cpp +++ b/cpp/sophus2/image/layout.cpp @@ -6,9 +6,9 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/layout.h" +#include "sophus2/image/layout.h" -namespace sophus { +namespace sophus2 { auto operator==(ImageLayout const& lhs, ImageLayout const& rhs) -> bool { return lhs.imageSize() == rhs.imageSize() && @@ -25,4 +25,4 @@ auto operator<<(std::ostream& os, ImageLayout const& layout) -> std::ostream& { return os; } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/layout.h b/cpp/sophus2/image/layout.h similarity index 82% rename from cpp/sophus/image/layout.h rename to cpp/sophus2/image/layout.h index 504f7f79..9617800a 100644 --- a/cpp/sophus/image/layout.h +++ b/cpp/sophus2/image/layout.h @@ -8,15 +8,16 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/concepts/image.h" -#include "sophus/image/image_size.h" +#include "sophus2/common/common.h" +#include "sophus2/concepts/image.h" +#include "sophus2/image/image_size.h" #include +#include #include -namespace sophus { +namespace sophus2 { /// Layout of the image: width, height and pitch in bytes. /// @@ -42,13 +43,13 @@ class ImageLayout { } template - [[nodiscard]] static auto makeFromSize(sophus::ImageSize image_size) + [[nodiscard]] static auto makeFromSize(sophus2::ImageSize image_size) -> ImageLayout { return makeFromSizeAndPitch( image_size, image_size.width * sizeof(TPixelFormat)); } - [[nodiscard]] auto imageSize() const -> sophus::ImageSize const& { + [[nodiscard]] auto imageSize() const -> sophus2::ImageSize const& { return image_size_; } [[nodiscard]] auto width() const -> int { return image_size_.width; } @@ -66,7 +67,7 @@ class ImageLayout { [[nodiscard]] auto isEmpty() const -> bool { return sizeBytes() == 0; } private: - sophus::ImageSize image_size_ = {0, 0}; + sophus2::ImageSize image_size_ = {0, 0}; size_t pitch_bytes_ = 0; }; @@ -80,4 +81,7 @@ auto operator!=(ImageLayout const& lhs, ImageLayout const& rhs) -> bool; /// Ostream operator. auto operator<<(std::ostream& os, ImageLayout const& layout) -> std::ostream&; -} // namespace sophus +} // namespace sophus2 + +template <> +struct fmt::formatter : ostream_formatter {}; diff --git a/cpp/sophus/image/layout_test.cpp b/cpp/sophus2/image/layout_test.cpp similarity index 84% rename from cpp/sophus/image/layout_test.cpp rename to cpp/sophus2/image/layout_test.cpp index 7c2e6aa4..37a13111 100644 --- a/cpp/sophus/image/layout_test.cpp +++ b/cpp/sophus2/image/layout_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/layout.h" +#include "sophus2/image/layout.h" #include -using namespace sophus; +using namespace sophus2; TEST(image_size, smoke) {} diff --git a/cpp/sophus/image/mut_dyn_image.cpp b/cpp/sophus2/image/mut_dyn_image.cpp similarity index 76% rename from cpp/sophus/image/mut_dyn_image.cpp rename to cpp/sophus2/image/mut_dyn_image.cpp index 441d3fb7..949cb28b 100644 --- a/cpp/sophus/image/mut_dyn_image.cpp +++ b/cpp/sophus2/image/mut_dyn_image.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/mut_dyn_image.h" +#include "sophus2/image/mut_dyn_image.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/image/mut_dyn_image.h b/cpp/sophus2/image/mut_dyn_image.h similarity index 98% rename from cpp/sophus/image/mut_dyn_image.h rename to cpp/sophus2/image/mut_dyn_image.h index 072e3aff..74bd241a 100644 --- a/cpp/sophus/image/mut_dyn_image.h +++ b/cpp/sophus2/image/mut_dyn_image.h @@ -8,11 +8,11 @@ #pragma once -#include "sophus/image/mut_dyn_image_view.h" +#include "sophus2/image/mut_dyn_image_view.h" #include -namespace sophus { +namespace sophus2 { template class DynImage; @@ -154,4 +154,4 @@ class MutDynImage : public MutDynImageView { UniqueDataArea unique_; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/mut_dyn_image_test.cpp b/cpp/sophus2/image/mut_dyn_image_test.cpp similarity index 96% rename from cpp/sophus/image/mut_dyn_image_test.cpp rename to cpp/sophus2/image/mut_dyn_image_test.cpp index 4a65c227..23fe9c4d 100644 --- a/cpp/sophus/image/mut_dyn_image_test.cpp +++ b/cpp/sophus2/image/mut_dyn_image_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/dyn_image_types.h" +#include "sophus2/image/dyn_image_types.h" #include -using namespace sophus; +using namespace sophus2; #define SOPHUS_TEST_IMG_EQ(lhs, rhs) /* NOLINT*/ \ do { \ diff --git a/cpp/sophus/image/mut_dyn_image_view.cpp b/cpp/sophus2/image/mut_dyn_image_view.cpp similarity index 75% rename from cpp/sophus/image/mut_dyn_image_view.cpp rename to cpp/sophus2/image/mut_dyn_image_view.cpp index 839f6a08..dcb56943 100644 --- a/cpp/sophus/image/mut_dyn_image_view.cpp +++ b/cpp/sophus2/image/mut_dyn_image_view.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/mut_dyn_image_view.h" +#include "sophus2/image/mut_dyn_image_view.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/image/mut_dyn_image_view.h b/cpp/sophus2/image/mut_dyn_image_view.h similarity index 95% rename from cpp/sophus/image/mut_dyn_image_view.h rename to cpp/sophus2/image/mut_dyn_image_view.h index 45cf697b..2ac20cfb 100644 --- a/cpp/sophus/image/mut_dyn_image_view.h +++ b/cpp/sophus2/image/mut_dyn_image_view.h @@ -8,11 +8,11 @@ #pragma once -#include "sophus/image/dyn_image_view.h" +#include "sophus2/image/dyn_image_view.h" #include -namespace sophus { +namespace sophus2 { template class MutDynImageView : public DynImageView { @@ -47,7 +47,7 @@ class MutDynImageView : public DynImageView { /// Returns subview with shared ownership semantics of whole image. [[nodiscard]] auto mutSubview( - Eigen::Vector2i uv, sophus::ImageSize size) const -> MutDynImageView { + Eigen::Vector2i uv, sophus2::ImageSize size) const -> MutDynImageView { SOPHUS_ASSERT(this->imageSize().contains(uv)); SOPHUS_ASSERT_LE(uv.x() + size.width, this->layout_.width()); SOPHUS_ASSERT_LE(uv.y() + size.height, this->layout_.height()); @@ -113,4 +113,4 @@ class MutDynImageView : public DynImageView { MutDynImageView() = default; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/mut_dyn_image_view_test.cpp b/cpp/sophus2/image/mut_dyn_image_view_test.cpp similarity index 92% rename from cpp/sophus/image/mut_dyn_image_view_test.cpp rename to cpp/sophus2/image/mut_dyn_image_view_test.cpp index 4385d1d7..58d0e731 100644 --- a/cpp/sophus/image/mut_dyn_image_view_test.cpp +++ b/cpp/sophus2/image/mut_dyn_image_view_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/dyn_image_view.h" +#include "sophus2/image/dyn_image_view.h" #include -using namespace sophus; +using namespace sophus2; #define SOPHUS_TEST_IMG_EQ(lhs, rhs) /* NOLINT*/ \ do { \ diff --git a/cpp/sophus/image/mut_image.cpp b/cpp/sophus2/image/mut_image.cpp similarity index 77% rename from cpp/sophus/image/mut_image.cpp rename to cpp/sophus2/image/mut_image.cpp index bbbd0e6c..8a4d3cc6 100644 --- a/cpp/sophus/image/mut_image.cpp +++ b/cpp/sophus2/image/mut_image.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/mut_image.h" +#include "sophus2/image/mut_image.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/image/mut_image.h b/cpp/sophus2/image/mut_image.h similarity index 94% rename from cpp/sophus/image/mut_image.h rename to cpp/sophus2/image/mut_image.h index 4f333136..9921507d 100644 --- a/cpp/sophus/image/mut_image.h +++ b/cpp/sophus2/image/mut_image.h @@ -13,12 +13,12 @@ /// for details. #pragma once -#include "sophus/common/enum.h" -#include "sophus/image/mut_image_view.h" +#include "sophus2/common/enum.h" +#include "sophus2/image/mut_image_view.h" #include -namespace sophus { +namespace sophus2 { // Types are largely inspired / derived from Pangolin. @@ -115,7 +115,7 @@ class MutImage : public MutImageView { /// Creates new contiguous image with given size. /// /// If layout is not empty, memory allocation will happen. - explicit MutImage(sophus::ImageSize size) + explicit MutImage(sophus2::ImageSize size) : MutImage(ImageLayout::makeFromSize(size)) {} /// Creates contiguous copy from view. @@ -229,4 +229,12 @@ class MutImage : public MutImageView { /// MutImage has unique ownership. UniqueDataArea unique_; }; -} // namespace sophus + +template +MutImage> +ImageView::cloneAsMut() const { + return MutImage>::makeCopyFrom( + *this); +} + +} // namespace sophus2 diff --git a/cpp/sophus/image/mut_image_test.cpp b/cpp/sophus2/image/mut_image_test.cpp similarity index 97% rename from cpp/sophus/image/mut_image_test.cpp rename to cpp/sophus2/image/mut_image_test.cpp index d13a422e..bae4f1c2 100644 --- a/cpp/sophus/image/mut_image_test.cpp +++ b/cpp/sophus2/image/mut_image_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/mut_image.h" +#include "sophus2/image/mut_image.h" -#include "sophus/image/image_types.h" +#include "sophus2/image/image_types.h" #include -using namespace sophus; +using namespace sophus2; TEST(MutImage, empty) { { diff --git a/cpp/sophus/image/mut_image_view.cpp b/cpp/sophus2/image/mut_image_view.cpp similarity index 89% rename from cpp/sophus/image/mut_image_view.cpp rename to cpp/sophus2/image/mut_image_view.cpp index 5d3f5129..25573c8d 100644 --- a/cpp/sophus/image/mut_image_view.cpp +++ b/cpp/sophus2/image/mut_image_view.cpp @@ -6,9 +6,9 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/mut_image_view.h" +#include "sophus2/image/mut_image_view.h" -namespace sophus { +namespace sophus2 { namespace details { @@ -17,7 +17,7 @@ void pitchedCopy( size_t dst_pitch_bytes, uint8_t const* src, size_t src_pitch_bytes, - sophus::ImageSize size, + sophus2::ImageSize size, uint8_t size_of_pixel) { size_t width_bytes = size.width * size_of_pixel; #if 0 @@ -44,4 +44,4 @@ void pitchedCopy( } } // namespace details -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/mut_image_view.h b/cpp/sophus2/image/mut_image_view.h similarity index 95% rename from cpp/sophus/image/mut_image_view.h rename to cpp/sophus2/image/mut_image_view.h index 2c2e56f7..dd31c8a4 100644 --- a/cpp/sophus/image/mut_image_view.h +++ b/cpp/sophus2/image/mut_image_view.h @@ -21,10 +21,10 @@ #pragma once -#include "sophus/calculus/region.h" -#include "sophus/image/image_view.h" +#include "sophus2/calculus/region.h" +#include "sophus2/image/image_view.h" -namespace sophus { +namespace sophus2 { namespace details { @@ -33,7 +33,7 @@ void pitchedCopy( size_t dst_pitch_bytes, uint8_t const* src, size_t src_pitch_bytes, - sophus::ImageSize size, + sophus2::ImageSize size, uint8_t size_of_pixel); } @@ -69,7 +69,7 @@ class MutImageView : public ImageView { /// Creates view from image size and pointer to first pixel. The image is /// assumed to be contiguous and the pitch is set accordingly. - MutImageView(sophus::ImageSize image_size, TPixel* ptr) noexcept + MutImageView(sophus2::ImageSize image_size, TPixel* ptr) noexcept : ImageView(image_size, ptr) {} /// Creates mutable view from view. @@ -248,7 +248,7 @@ class MutImageView : public ImageView { /// Returns mutable subview. [[nodiscard]] auto mutSubview( - Eigen::Vector2i uv, sophus::ImageSize size) const -> MutImageView { + Eigen::Vector2i uv, sophus2::ImageSize size) const -> MutImageView { SOPHUS_ASSERT(this->colInBounds(uv[0])); SOPHUS_ASSERT(this->rowInBounds(uv[1])); SOPHUS_ASSERT_LE(uv.x() + size.width, this->layout().width()); @@ -288,8 +288,8 @@ auto checkedPixelAccessMut( } } // namespace details -} // namespace sophus +} // namespace sophus2 -#define SOPHUS_PIXEL_MUT(img, u, v, ...) \ - ::sophus::details::checkedPixelAccessMut( \ +#define SOPHUS_PIXEL_MUT(img, u, v, ...) \ + ::sophus2::details::checkedPixelAccessMut( \ img, u, v, __FILE__, __LINE__, SOPHUS_FORMAT(__VA_ARGS__)) diff --git a/cpp/sophus/image/mut_image_view_test.cpp b/cpp/sophus2/image/mut_image_view_test.cpp similarity index 95% rename from cpp/sophus/image/mut_image_view_test.cpp rename to cpp/sophus2/image/mut_image_view_test.cpp index 0731caca..2f48e0c6 100644 --- a/cpp/sophus/image/mut_image_view_test.cpp +++ b/cpp/sophus2/image/mut_image_view_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/mut_image_view.h" +#include "sophus2/image/mut_image_view.h" -#include "sophus/image/image_types.h" +#include "sophus2/image/image_types.h" #include -using namespace sophus; +using namespace sophus2; TEST(MutImageView, empty) { MutImageView mut_view; diff --git a/cpp/sophus/image/pixel_format.cpp b/cpp/sophus2/image/pixel_format.cpp similarity index 91% rename from cpp/sophus/image/pixel_format.cpp rename to cpp/sophus2/image/pixel_format.cpp index 50974ac7..48e0982f 100644 --- a/cpp/sophus/image/pixel_format.cpp +++ b/cpp/sophus2/image/pixel_format.cpp @@ -6,9 +6,9 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/pixel_format.h" +#include "sophus2/image/pixel_format.h" -namespace sophus { +namespace sophus2 { auto operator==(PixelFormat const& lhs, PixelFormat const& rhs) -> bool { return lhs.number_type == rhs.number_type && @@ -36,4 +36,4 @@ auto operator<<(std::ostream& os, PixelFormat const& type) -> std::ostream& { return os; } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/image/pixel_format.h b/cpp/sophus2/image/pixel_format.h similarity index 87% rename from cpp/sophus/image/pixel_format.h rename to cpp/sophus2/image/pixel_format.h index 1609bcfc..e069b90a 100644 --- a/cpp/sophus/image/pixel_format.h +++ b/cpp/sophus2/image/pixel_format.h @@ -8,9 +8,11 @@ #pragma once -#include "sophus/image/image_types.h" +#include "sophus2/image/image_types.h" -namespace sophus { +#include + +namespace sophus2 { struct PixelFormat { template @@ -46,4 +48,7 @@ auto operator==(PixelFormat const& lhs, PixelFormat const& rhs) -> bool; /// PixelFormat::fromTemplate>() outputs: /// "4U8"; auto operator<<(std::ostream& os, PixelFormat const& type) -> std::ostream&; -} // namespace sophus +} // namespace sophus2 + +template <> +struct fmt::formatter : ostream_formatter {}; diff --git a/cpp/sophus/image/pixel_format_test.cpp b/cpp/sophus2/image/pixel_format_test.cpp similarity index 81% rename from cpp/sophus/image/pixel_format_test.cpp rename to cpp/sophus2/image/pixel_format_test.cpp index f39272fd..b12a9a39 100644 --- a/cpp/sophus/image/pixel_format_test.cpp +++ b/cpp/sophus2/image/pixel_format_test.cpp @@ -6,8 +6,8 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/image/pixel_format.h" +#include "sophus2/image/pixel_format.h" #include -using namespace sophus; +using namespace sophus2; diff --git a/cpp/sophus2/interp/CMakeLists.txt b/cpp/sophus2/interp/CMakeLists.txt new file mode 100644 index 00000000..6daeba2f --- /dev/null +++ b/cpp/sophus2/interp/CMakeLists.txt @@ -0,0 +1,20 @@ +#[[ +sophus2_interp + +]] + +farm_ng_add_library(sophus2_interp + NAMESPACE ${PROJECT_NAME} + INCLUDE_DIR ../../ + HEADERS + average.h + interpolate.h +) +target_link_libraries(sophus2_interp INTERFACE sophus2_lie) + +farm_ng_add_test(interpolate + PARENT_LIBRARY sophus2_interp + LINK_LIBRARIES sophus2_interp + LABELS small) + +add_subdirectory(spline) diff --git a/cpp/sophus/interp/average.h b/cpp/sophus2/interp/average.h similarity index 96% rename from cpp/sophus/interp/average.h rename to cpp/sophus2/interp/average.h index 2ac4b936..23172a3c 100644 --- a/cpp/sophus/interp/average.h +++ b/cpp/sophus2/interp/average.h @@ -11,21 +11,21 @@ #pragma once -#include "sophus/lie/isometry2.h" -#include "sophus/lie/isometry3.h" -#include "sophus/lie/rotation2.h" -#include "sophus/lie/rotation3.h" -#include "sophus/lie/scaling.h" -#include "sophus/lie/scaling_translation.h" -#include "sophus/lie/similarity2.h" -#include "sophus/lie/similarity3.h" -#include "sophus/lie/spiral_similarity2.h" -#include "sophus/lie/spiral_similarity3.h" -#include "sophus/lie/translation.h" +#include "sophus2/lie/isometry2.h" +#include "sophus2/lie/isometry3.h" +#include "sophus2/lie/rotation2.h" +#include "sophus2/lie/rotation3.h" +#include "sophus2/lie/scaling.h" +#include "sophus2/lie/scaling_translation.h" +#include "sophus2/lie/similarity2.h" +#include "sophus2/lie/similarity3.h" +#include "sophus2/lie/spiral_similarity2.h" +#include "sophus2/lie/spiral_similarity3.h" +#include "sophus2/lie/translation.h" #include -namespace sophus { +namespace sophus2 { /// Calculates mean iteratively. /// @@ -349,4 +349,4 @@ auto average( #endif // DOXYGEN_SHOULD_SKIP_THIS -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/interp/interpolate.h b/cpp/sophus2/interp/interpolate.h similarity index 94% rename from cpp/sophus/interp/interpolate.h rename to cpp/sophus2/interp/interpolate.h index 99fc2bf8..dd4bc2c9 100644 --- a/cpp/sophus/interp/interpolate.h +++ b/cpp/sophus2/interp/interpolate.h @@ -11,11 +11,11 @@ #pragma once -#include "sophus/lie/lie_group.h" +#include "sophus2/lie/lie_group.h" #include -namespace sophus { +namespace sophus2 { /// This function interpolates between two Lie group elements /// ``foo_from_bar`` and ``foo_from_daz`` with an interpolation factor @@ -47,4 +47,4 @@ auto interpolate( TGroup::exp(inter_p * (foo_from_bar.inverse() * foo_from_daz).log()); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/interp/interpolate_test.cpp b/cpp/sophus2/interp/interpolate_test.cpp similarity index 98% rename from cpp/sophus/interp/interpolate_test.cpp rename to cpp/sophus2/interp/interpolate_test.cpp index 4d2551cf..c53889ed 100644 --- a/cpp/sophus/interp/interpolate_test.cpp +++ b/cpp/sophus2/interp/interpolate_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/interp/interpolate.h" +#include "sophus2/interp/interpolate.h" -#include "sophus/interp/average.h" -#include "sophus/lie/scaling_translation.h" +#include "sophus2/interp/average.h" +#include "sophus2/lie/scaling_translation.h" #include -namespace sophus::test { +namespace sophus2::test { template < template @@ -267,4 +267,4 @@ TEST(lie_groups, linterpolate_prop_tests) { InterpolatePropTestSuite::runAllTests( "ScalingTranslation3"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus2/interp/spline/CMakeLists.txt b/cpp/sophus2/interp/spline/CMakeLists.txt new file mode 100644 index 00000000..3c0d5324 --- /dev/null +++ b/cpp/sophus2/interp/spline/CMakeLists.txt @@ -0,0 +1,22 @@ +#[[ +sophus2_interp_spline + +]] + +farm_ng_add_library(sophus2_interp_spline + NAMESPACE ${PROJECT_NAME} + INCLUDE_DIR ../../../ + HEADERS + bspline.h + group_bspline.h + details/bspline_segment.h + details/cubic_basis.h + details/group_bspline_segment.h +) +target_link_libraries(sophus2_interp_spline INTERFACE sophus2_lie) + + +farm_ng_add_test(spline + PARENT_LIBRARY sophus2_interp_spline + LINK_LIBRARIES sophus2_interp_spline + LABELS small) diff --git a/cpp/sophus/interp/spline/bspline.h b/cpp/sophus2/interp/spline/bspline.h similarity index 96% rename from cpp/sophus/interp/spline/bspline.h rename to cpp/sophus2/interp/spline/bspline.h index ee3e1f62..d64b0322 100644 --- a/cpp/sophus/interp/spline/bspline.h +++ b/cpp/sophus2/interp/spline/bspline.h @@ -8,10 +8,10 @@ #pragma once -#include "sophus/interp/spline/common.h" -#include "sophus/interp/spline/details/bspline_segment.h" +#include "sophus2/interp/spline/common.h" +#include "sophus2/interp/spline/details/bspline_segment.h" -namespace sophus { +namespace sophus2 { template class CubicBSplineImpl { public: @@ -154,7 +154,7 @@ class CubicBSpline { SegmentCoordinate index_and_u; index_and_u.fraction = std::modf(s, &segment_idx); index_and_u.segment_idx = int(segment_idx); - if (index_and_u.fraction > sophus::kEpsilonF64) { + if (index_and_u.fraction > sophus2::kEpsilonF64) { return index_and_u; } @@ -177,4 +177,4 @@ class CubicBSpline { double t0_; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/interp/spline/common.h b/cpp/sophus2/interp/spline/common.h similarity index 87% rename from cpp/sophus/interp/spline/common.h rename to cpp/sophus2/interp/spline/common.h index e391b4bd..8fec38f7 100644 --- a/cpp/sophus/interp/spline/common.h +++ b/cpp/sophus2/interp/spline/common.h @@ -14,13 +14,13 @@ #pragma once -#include "sophus/common/common.h" +#include "sophus2/common/common.h" -namespace sophus { +namespace sophus2 { struct SegmentCoordinate { int segment_idx; double fraction; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/interp/spline/details/bspline_segment.h b/cpp/sophus2/interp/spline/details/bspline_segment.h similarity index 97% rename from cpp/sophus/interp/spline/details/bspline_segment.h rename to cpp/sophus2/interp/spline/details/bspline_segment.h index acd99657..db6e73ed 100644 --- a/cpp/sophus/interp/spline/details/bspline_segment.h +++ b/cpp/sophus2/interp/spline/details/bspline_segment.h @@ -8,10 +8,10 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/interp/spline/details/cubic_basis.h" +#include "sophus2/common/common.h" +#include "sophus2/interp/spline/details/cubic_basis.h" -namespace sophus { +namespace sophus2 { namespace details { template @@ -158,4 +158,4 @@ struct CubicBSplineSegment { }; } // namespace details -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/interp/spline/details/cubic_basis.h b/cpp/sophus2/interp/spline/details/cubic_basis.h similarity index 94% rename from cpp/sophus/interp/spline/details/cubic_basis.h rename to cpp/sophus2/interp/spline/details/cubic_basis.h index d87476fe..b792cb9e 100644 --- a/cpp/sophus/interp/spline/details/cubic_basis.h +++ b/cpp/sophus2/interp/spline/details/cubic_basis.h @@ -13,10 +13,10 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/lie/lie_group.h" +#include "sophus2/common/common.h" +#include "sophus2/lie/lie_group.h" -namespace sophus { +namespace sophus2 { namespace details { template @@ -62,4 +62,4 @@ class CubicBSplineBasis { enum class SegmentCase { first, normal, last }; } // namespace details -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/interp/spline/details/group_bspline_segment.h b/cpp/sophus2/interp/spline/details/group_bspline_segment.h similarity index 98% rename from cpp/sophus/interp/spline/details/group_bspline_segment.h rename to cpp/sophus2/interp/spline/details/group_bspline_segment.h index 3046923d..2287885b 100644 --- a/cpp/sophus/interp/spline/details/group_bspline_segment.h +++ b/cpp/sophus2/interp/spline/details/group_bspline_segment.h @@ -13,11 +13,11 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/interp/spline/details/cubic_basis.h" -#include "sophus/lie/lie_group.h" +#include "sophus2/common/common.h" +#include "sophus2/interp/spline/details/cubic_basis.h" +#include "sophus2/lie/lie_group.h" -namespace sophus { +namespace sophus2 { namespace details { template @@ -283,4 +283,4 @@ struct CubicLieGroupBSplineSegment { }; } // namespace details -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/interp/spline/group_bspline.h b/cpp/sophus2/interp/spline/group_bspline.h similarity index 98% rename from cpp/sophus/interp/spline/group_bspline.h rename to cpp/sophus2/interp/spline/group_bspline.h index eed05920..00e1c00b 100644 --- a/cpp/sophus/interp/spline/group_bspline.h +++ b/cpp/sophus2/interp/spline/group_bspline.h @@ -13,10 +13,10 @@ #pragma once -#include "sophus/interp/spline/common.h" -#include "sophus/interp/spline/details/group_bspline_segment.h" +#include "sophus2/interp/spline/common.h" +#include "sophus2/interp/spline/details/group_bspline_segment.h" -namespace sophus { +namespace sophus2 { template class CubicLieGroupBSplineFn { @@ -326,7 +326,7 @@ class CubicLieGroupBSpline { SegmentCoordinate index_and_u; index_and_u.fraction = std::modf(s, &i); index_and_u.segment_idx = int(i); - if (index_and_u.fraction > sophus::kEpsilonF64) { + if (index_and_u.fraction > sophus2::kEpsilonF64) { return index_and_u; } @@ -352,4 +352,4 @@ class CubicLieGroupBSpline { template using BasisSpline = CubicLieGroupBSpline; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/interp/spline/spline_test.cpp b/cpp/sophus2/interp/spline/spline_test.cpp similarity index 92% rename from cpp/sophus/interp/spline/spline_test.cpp rename to cpp/sophus2/interp/spline/spline_test.cpp index 08320ba9..10ebf3b9 100644 --- a/cpp/sophus/interp/spline/spline_test.cpp +++ b/cpp/sophus2/interp/spline/spline_test.cpp @@ -6,25 +6,25 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/calculus/num_diff.h" -#include "sophus/interp/interpolate.h" -#include "sophus/interp/spline/bspline.h" -#include "sophus/interp/spline/group_bspline.h" -#include "sophus/lie/isometry2.h" -#include "sophus/lie/isometry3.h" -#include "sophus/lie/rotation2.h" -#include "sophus/lie/rotation3.h" -#include "sophus/lie/scaling.h" -#include "sophus/lie/scaling_translation.h" -#include "sophus/lie/similarity2.h" -#include "sophus/lie/similarity3.h" -#include "sophus/lie/spiral_similarity2.h" -#include "sophus/lie/spiral_similarity3.h" -#include "sophus/lie/translation.h" +#include "sophus2/calculus/num_diff.h" +#include "sophus2/interp/interpolate.h" +#include "sophus2/interp/spline/bspline.h" +#include "sophus2/interp/spline/group_bspline.h" +#include "sophus2/lie/isometry2.h" +#include "sophus2/lie/isometry3.h" +#include "sophus2/lie/rotation2.h" +#include "sophus2/lie/rotation3.h" +#include "sophus2/lie/scaling.h" +#include "sophus2/lie/scaling_translation.h" +#include "sophus2/lie/similarity2.h" +#include "sophus2/lie/similarity3.h" +#include "sophus2/lie/spiral_similarity2.h" +#include "sophus2/lie/spiral_similarity3.h" +#include "sophus2/lie/translation.h" #include -namespace sophus::test { +namespace sophus2::test { template struct SplinePropTestSuite { @@ -220,4 +220,4 @@ TEST(bspline, cartesian_vs_lie_group_bspline) { } } } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus2/lie/CMakeLists.txt b/cpp/sophus2/lie/CMakeLists.txt new file mode 100644 index 00000000..b3a81bef --- /dev/null +++ b/cpp/sophus2/lie/CMakeLists.txt @@ -0,0 +1,48 @@ +#[[ +sophus2_lie + + + +]] + +set(sophus2_lie_src_prefixes + group_manifold + identity + isometry2 + isometry3 + lie_group + pose3 + rotation2 + rotation3 + scaling_translation + scaling + similarity2 + similarity3 + spiral_similarity2 + spiral_similarity3 + translation +) + +set(sophus2_lie_h) + +foreach(x ${sophus2_lie_src_prefixes} ) + list(APPEND sophus2_lie_h ${x}.h) +endforeach() + + + +farm_ng_add_library(sophus2_lie + NAMESPACE ${PROJECT_NAME} + INCLUDE_DIR ../../ + HEADERS + ${sophus2_lie_h} +) +target_link_libraries(sophus2_lie INTERFACE sophus2_linalg) + + +foreach(test_basename ${sophus2_lie_src_prefixes}) + farm_ng_add_test(${test_basename} + PARENT_LIBRARY sophus2_lie + LINK_LIBRARIES sophus2_lie + LABELS small) +endforeach() diff --git a/cpp/sophus/lie/group_manifold.h b/cpp/sophus2/lie/group_manifold.h similarity index 93% rename from cpp/sophus/lie/group_manifold.h rename to cpp/sophus2/lie/group_manifold.h index b656383f..3f695372 100644 --- a/cpp/sophus/lie/group_manifold.h +++ b/cpp/sophus2/lie/group_manifold.h @@ -8,10 +8,10 @@ #pragma once -#include "sophus/interp/average.h" -#include "sophus/lie/lie_group.h" +#include "sophus2/interp/average.h" +#include "sophus2/lie/lie_group.h" -namespace sophus { +namespace sophus2 { SOPHUS_ENUM(ManifoldPlusType, (left_plus, right_plus)); template @@ -73,7 +73,7 @@ struct GroupManifold { groups.push_back(m.group); } - auto maybe_mean = sophus::average(groups); + auto maybe_mean = sophus2::average(groups); if (maybe_mean) { return GroupManifold(*maybe_mean); } @@ -89,4 +89,4 @@ using LeftPlus = GroupManifold; template using RightPlus = GroupManifold; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/group_manifold_test.cpp b/cpp/sophus2/lie/group_manifold_test.cpp similarity index 84% rename from cpp/sophus/lie/group_manifold_test.cpp rename to cpp/sophus2/lie/group_manifold_test.cpp index 6ca328da..8dab3b92 100644 --- a/cpp/sophus/lie/group_manifold_test.cpp +++ b/cpp/sophus2/lie/group_manifold_test.cpp @@ -6,15 +6,15 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/group_manifold.h" +#include "sophus2/lie/group_manifold.h" -#include "sophus/lie/isometry3.h" -#include "sophus/manifold/product_manifold.h" -#include "sophus/manifold/vector_manifold.h" +#include "sophus2/lie/isometry3.h" +#include "sophus2/manifold/product_manifold.h" +#include "sophus2/manifold/vector_manifold.h" #include -using namespace sophus; +using namespace sophus2; TEST(group_manifold, unit) { static_assert(concepts::Manifold>); diff --git a/cpp/sophus/lie/identity.h b/cpp/sophus2/lie/identity.h similarity index 90% rename from cpp/sophus/lie/identity.h rename to cpp/sophus2/lie/identity.h index 81b548fc..75cdbffd 100644 --- a/cpp/sophus/lie/identity.h +++ b/cpp/sophus2/lie/identity.h @@ -8,10 +8,10 @@ #pragma once -#include "sophus/lie/impl/identity.h" -#include "sophus/lie/lie_group.h" +#include "sophus2/lie/impl/identity.h" +#include "sophus2/lie/lie_group.h" -namespace sophus { +namespace sophus2 { // definition: identity mapping in R^n // <==> origin, direction and distance preserving mapping R^n @@ -39,4 +39,4 @@ using Identity2 = Identity; template using Identity3 = Identity; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/identity_test.cpp b/cpp/sophus2/lie/identity_test.cpp similarity index 72% rename from cpp/sophus/lie/identity_test.cpp rename to cpp/sophus2/lie/identity_test.cpp index 0267e8ba..f6e6b0c6 100644 --- a/cpp/sophus/lie/identity_test.cpp +++ b/cpp/sophus2/lie/identity_test.cpp @@ -6,17 +6,17 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/identity.h" +#include "sophus2/lie/identity.h" -#include "sophus/concepts/group_accessors.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(identity, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("IdentityF64"); LieGroupPropTestSuite>::runAllTests("IdentityF32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/impl/identity.h b/cpp/sophus2/lie/impl/identity.h similarity index 96% rename from cpp/sophus/lie/impl/identity.h rename to cpp/sophus2/lie/impl/identity.h index ef4efdbd..919c0910 100644 --- a/cpp/sophus/lie/impl/identity.h +++ b/cpp/sophus2/lie/impl/identity.h @@ -8,10 +8,10 @@ #pragma once -#include "sophus/concepts/lie_group.h" -#include "sophus/manifold/unit_vector.h" +#include "sophus2/concepts/lie_group.h" +#include "sophus2/manifold/unit_vector.h" -namespace sophus { +namespace sophus2 { namespace lie { template @@ -55,8 +55,8 @@ class IdentityImpl { static auto identityParams() -> Params { return Params::Zero(); } static auto areParamsValid(Params const& scale_factors) - -> sophus::Expected { - return sophus::Expected{}; + -> sophus2::Expected { + return sophus2::Expected{}; } static auto adj(Params const& /*unused*/) @@ -214,4 +214,4 @@ struct IdentityWithDim { } // namespace lie -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/impl/rotation2.h b/cpp/sophus2/lie/impl/rotation2.h similarity index 97% rename from cpp/sophus/lie/impl/rotation2.h rename to cpp/sophus2/lie/impl/rotation2.h index 31411868..4b35e711 100644 --- a/cpp/sophus/lie/impl/rotation2.h +++ b/cpp/sophus2/lie/impl/rotation2.h @@ -8,11 +8,12 @@ #pragma once -#include "sophus/concepts/lie_group.h" -#include "sophus/manifold/complex.h" -#include "sophus/manifold/unit_vector.h" +#include "farm_ng/core/logging/fmt_ceres.h" +#include "sophus2/concepts/lie_group.h" +#include "sophus2/manifold/complex.h" +#include "sophus2/manifold/unit_vector.h" -namespace sophus { +namespace sophus2 { namespace lie { template @@ -59,7 +60,7 @@ class Rotation2Impl { } static auto areParamsValid(Params const& unit_complex) - -> sophus::Expected { + -> sophus2::Expected { static const Scalar kThr = kEpsilon; const Scalar squared_norm = unit_complex.squaredNorm(); using std::abs; @@ -72,7 +73,7 @@ class Rotation2Impl { squared_norm, kThr); } - return sophus::Expected{}; + return sophus2::Expected{}; } static auto hasShortestPathAmbiguity(Params const& foo_from_bar) -> bool { @@ -203,6 +204,8 @@ class Rotation2Impl { Scalar halftheta_by_tan_of_halftheta; Scalar real_minus_one = z.x() - Scalar(1.); + using std::abs; + if (abs(real_minus_one) < kEpsilon) { halftheta_by_tan_of_halftheta = Scalar(1.) - Scalar(1. / 12) * theta[0] * theta[0]; diff --git a/cpp/sophus/lie/impl/rotation3.h b/cpp/sophus2/lie/impl/rotation3.h similarity index 98% rename from cpp/sophus/lie/impl/rotation3.h rename to cpp/sophus2/lie/impl/rotation3.h index 686e5898..7716edbb 100644 --- a/cpp/sophus/lie/impl/rotation3.h +++ b/cpp/sophus2/lie/impl/rotation3.h @@ -7,11 +7,11 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/concepts/lie_group.h" -#include "sophus/manifold/quaternion.h" -#include "sophus/manifold/unit_vector.h" +#include "sophus2/concepts/lie_group.h" +#include "sophus2/manifold/quaternion.h" +#include "sophus2/manifold/unit_vector.h" -namespace sophus { +namespace sophus2 { namespace lie { template @@ -59,7 +59,7 @@ class Rotation3Impl { } static auto areParamsValid(Params const& unit_quaternion) - -> sophus::Expected { + -> sophus2::Expected { static const Scalar kThr = kEpsilonSqrt; const Scalar squared_norm = unit_quaternion.squaredNorm(); using std::abs; @@ -67,12 +67,12 @@ class Rotation3Impl { return SOPHUS_UNEXPECTED( "quaternion number (({}), {}) is not unit length.\n" "Squared norm: {}, thr: {}", - unit_quaternion.template head<3>(), + unit_quaternion.template head<3>().eval(), unit_quaternion[3], squared_norm, kThr); } - return sophus::Expected{}; + return sophus2::Expected{}; } static auto hasShortestPathAmbiguity(Params const& foo_from_bar) -> bool { @@ -485,4 +485,4 @@ class Rotation3Impl { }; } // namespace lie -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/impl/scaling.h b/cpp/sophus2/lie/impl/scaling.h similarity index 97% rename from cpp/sophus/lie/impl/scaling.h rename to cpp/sophus2/lie/impl/scaling.h index f3ff56c1..da78bf20 100644 --- a/cpp/sophus/lie/impl/scaling.h +++ b/cpp/sophus2/lie/impl/scaling.h @@ -7,10 +7,10 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/concepts/lie_group.h" -#include "sophus/manifold/unit_vector.h" +#include "sophus2/concepts/lie_group.h" +#include "sophus2/manifold/unit_vector.h" -namespace sophus { +namespace sophus2 { namespace lie { template @@ -55,7 +55,7 @@ class ScalingImpl { } static auto areParamsValid(Params const& scale_factors) - -> sophus::Expected { + -> sophus2::Expected { static const Scalar kThr = kEpsilon; if (!(scale_factors.array() > kThr).all()) { @@ -72,7 +72,7 @@ class ScalingImpl { scale_factors.transpose(), 1.0 / kThr); } - return sophus::Expected{}; + return sophus2::Expected{}; } static auto hasShortestPathAmbiguity(Params const&) -> bool { return false; } @@ -318,4 +318,4 @@ struct ScalingWithDim { }; } // namespace lie -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/impl/sim_mat_w.h b/cpp/sophus2/lie/impl/sim_mat_w.h similarity index 99% rename from cpp/sophus/lie/impl/sim_mat_w.h rename to cpp/sophus2/lie/impl/sim_mat_w.h index 4c0c8670..b5bf66b0 100644 --- a/cpp/sophus/lie/impl/sim_mat_w.h +++ b/cpp/sophus2/lie/impl/sim_mat_w.h @@ -8,9 +8,9 @@ #pragma once -#include "sophus/common/common.h" +#include "sophus2/common/common.h" -namespace sophus { +namespace sophus2 { namespace details { template @@ -223,4 +223,4 @@ auto calcWInv( } } // namespace details -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/impl/spiral_similarity2.h b/cpp/sophus2/lie/impl/spiral_similarity2.h similarity index 96% rename from cpp/sophus/lie/impl/spiral_similarity2.h rename to cpp/sophus2/lie/impl/spiral_similarity2.h index dcddf452..a6386dc8 100644 --- a/cpp/sophus/lie/impl/spiral_similarity2.h +++ b/cpp/sophus2/lie/impl/spiral_similarity2.h @@ -7,13 +7,13 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/concepts/lie_group.h" -#include "sophus/lie/impl/rotation2.h" -#include "sophus/lie/impl/sim_mat_w.h" -#include "sophus/manifold/complex.h" -#include "sophus/manifold/unit_vector.h" +#include "sophus2/concepts/lie_group.h" +#include "sophus2/lie/impl/rotation2.h" +#include "sophus2/lie/impl/sim_mat_w.h" +#include "sophus2/manifold/complex.h" +#include "sophus2/manifold/unit_vector.h" -namespace sophus { +namespace sophus2 { namespace lie { template @@ -60,7 +60,7 @@ class SpiralSimilarity2Impl { } static auto areParamsValid(Params const& non_zero_complex) - -> sophus::Expected { + -> sophus2::Expected { static const Scalar kThr = kEpsilon * kEpsilon; const Scalar squared_norm = non_zero_complex.squaredNorm(); using std::abs; @@ -73,7 +73,7 @@ class SpiralSimilarity2Impl { squared_norm, kThr); } - return sophus::Expected{}; + return sophus2::Expected{}; } static auto hasShortestPathAmbiguity(Params const& non_zero_complex) -> bool { @@ -326,4 +326,4 @@ class SpiralSimilarity2Impl { }; } // namespace lie -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/impl/spiral_similarity3.h b/cpp/sophus2/lie/impl/spiral_similarity3.h similarity index 97% rename from cpp/sophus/lie/impl/spiral_similarity3.h rename to cpp/sophus2/lie/impl/spiral_similarity3.h index d390a094..5c43aac5 100644 --- a/cpp/sophus/lie/impl/spiral_similarity3.h +++ b/cpp/sophus2/lie/impl/spiral_similarity3.h @@ -7,13 +7,13 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/concepts/lie_group.h" -#include "sophus/lie/impl/rotation3.h" -#include "sophus/lie/impl/sim_mat_w.h" -#include "sophus/manifold/quaternion.h" -#include "sophus/manifold/unit_vector.h" +#include "sophus2/concepts/lie_group.h" +#include "sophus2/lie/impl/rotation3.h" +#include "sophus2/lie/impl/sim_mat_w.h" +#include "sophus2/manifold/quaternion.h" +#include "sophus2/manifold/unit_vector.h" -namespace sophus { +namespace sophus2 { namespace lie { template @@ -61,7 +61,7 @@ class SpiralSimilarity3Impl { } static auto areParamsValid(Params const& non_zero_quat) - -> sophus::Expected { + -> sophus2::Expected { static const Scalar kThr = kEpsilon * kEpsilon; const Scalar squared_norm = non_zero_quat.squaredNorm(); using std::abs; @@ -74,7 +74,7 @@ class SpiralSimilarity3Impl { squared_norm, kThr); } - return sophus::Expected{}; + return sophus2::Expected{}; } static auto hasShortestPathAmbiguity(Params const& non_zero_quat) -> bool { @@ -396,4 +396,4 @@ class SpiralSimilarity3Impl { }; } // namespace lie -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/impl/translation_factor_group_product.h b/cpp/sophus2/lie/impl/translation_factor_group_product.h similarity index 97% rename from cpp/sophus/lie/impl/translation_factor_group_product.h rename to cpp/sophus2/lie/impl/translation_factor_group_product.h index ef5aa225..997dea44 100644 --- a/cpp/sophus/lie/impl/translation_factor_group_product.h +++ b/cpp/sophus2/lie/impl/translation_factor_group_product.h @@ -8,12 +8,12 @@ #pragma once -#include "sophus/concepts/lie_group.h" -#include "sophus/linalg/homogeneous.h" -#include "sophus/linalg/vector_space.h" -#include "sophus/manifold/unit_vector.h" +#include "sophus2/concepts/lie_group.h" +#include "sophus2/linalg/homogeneous.h" +#include "sophus2/linalg/vector_space.h" +#include "sophus2/manifold/unit_vector.h" -namespace sophus { +namespace sophus2 { namespace lie { /// Semi direct product of a Lie group (factor group) and the @@ -82,7 +82,7 @@ class TranslationFactorGroupProduct { } static auto areParamsValid(Params const& params) - -> sophus::Expected { + -> sophus2::Expected { return FactorGroup::areParamsValid(factorGroupParams(params)); } @@ -378,7 +378,7 @@ class TranslationFactorGroupProduct { } static auto exampleTranslations() -> std::vector { - return ::sophus::pointExamples(); + return ::sophus2::pointExamples(); } }; @@ -390,4 +390,4 @@ struct WithDimAndSubgroup { }; } // namespace lie -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/isometry2.h b/cpp/sophus2/lie/isometry2.h similarity index 93% rename from cpp/sophus/lie/isometry2.h rename to cpp/sophus2/lie/isometry2.h index bf872654..aceea870 100644 --- a/cpp/sophus/lie/isometry2.h +++ b/cpp/sophus2/lie/isometry2.h @@ -8,13 +8,13 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/rotation2.h" -#include "sophus/lie/impl/translation_factor_group_product.h" -#include "sophus/lie/lie_group.h" -#include "sophus/lie/rotation2.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/rotation2.h" +#include "sophus2/lie/impl/translation_factor_group_product.h" +#include "sophus2/lie/lie_group.h" +#include "sophus2/lie/rotation2.h" -namespace sophus { +namespace sophus2 { // definition: distance preserving mapping in R^2 // <==> shape and size preserving mapping in R^2 @@ -140,4 +140,4 @@ using Isometry2F64 = Isometry2; static_assert(concepts::Isometry2); -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/isometry2_test.cpp b/cpp/sophus2/lie/isometry2_test.cpp similarity index 73% rename from cpp/sophus/lie/isometry2_test.cpp rename to cpp/sophus2/lie/isometry2_test.cpp index 35134d09..b85e04fb 100644 --- a/cpp/sophus/lie/isometry2_test.cpp +++ b/cpp/sophus2/lie/isometry2_test.cpp @@ -6,16 +6,16 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/isometry2.h" +#include "sophus2/lie/isometry2.h" -#include "sophus/concepts/group_accessors_unit_tests.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors_unit_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -using namespace sophus; +using namespace sophus2; -namespace sophus::test { +namespace sophus2::test { TEST(isometry2, unit_tests) { runIsometry2UnitTests>(); @@ -27,4 +27,4 @@ TEST(identity, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Isometry2F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/isometry3.h b/cpp/sophus2/lie/isometry3.h similarity index 92% rename from cpp/sophus/lie/isometry3.h rename to cpp/sophus2/lie/isometry3.h index 982fb66d..58f7a103 100644 --- a/cpp/sophus/lie/isometry3.h +++ b/cpp/sophus2/lie/isometry3.h @@ -8,13 +8,13 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/rotation3.h" -#include "sophus/lie/impl/translation_factor_group_product.h" -#include "sophus/lie/lie_group.h" -#include "sophus/lie/rotation3.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/rotation3.h" +#include "sophus2/lie/impl/translation_factor_group_product.h" +#include "sophus2/lie/lie_group.h" +#include "sophus2/lie/rotation3.h" -namespace sophus { +namespace sophus2 { // definition: distance preserving mapping in R^3 // <==> shape and size preserving mapping in R^3 @@ -171,14 +171,14 @@ static_assert(concepts::Isometry3); namespace details { template -class Cast> { +class Cast> { public: template - static auto impl(sophus::Isometry3 const& v) { + static auto impl(sophus2::Isometry3 const& v) { return v.template cast(); } template - static auto implScalar(sophus::Isometry3 const& v) { + static auto implScalar(sophus2::Isometry3 const& v) { return v.template cast(); } }; @@ -189,4 +189,4 @@ using SE3 = Isometry3; // NOLINT using SE3f = Isometry3; // NOLINT using SE3d = Isometry3; // NOLINT -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/isometry3_test.cpp b/cpp/sophus2/lie/isometry3_test.cpp similarity index 75% rename from cpp/sophus/lie/isometry3_test.cpp rename to cpp/sophus2/lie/isometry3_test.cpp index 5a6693b2..11537565 100644 --- a/cpp/sophus/lie/isometry3_test.cpp +++ b/cpp/sophus2/lie/isometry3_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/isometry3.h" +#include "sophus2/lie/isometry3.h" -#include "sophus/concepts/group_accessors_unit_tests.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors_unit_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(isometry3, unit_tests) { runIsometry3UnitTests>(); @@ -25,4 +25,4 @@ TEST(Isometry, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Isometry3F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/lie_group.h b/cpp/sophus2/lie/lie_group.h similarity index 97% rename from cpp/sophus/lie/lie_group.h rename to cpp/sophus2/lie/lie_group.h index 407a51d6..e1a727e2 100644 --- a/cpp/sophus/lie/lie_group.h +++ b/cpp/sophus2/lie/lie_group.h @@ -8,11 +8,11 @@ #pragma once -#include "sophus/common/enum.h" -#include "sophus/concepts/lie_group.h" -#include "sophus/lie/impl/translation_factor_group_product.h" +#include "sophus2/common/enum.h" +#include "sophus2/concepts/lie_group.h" +#include "sophus2/lie/impl/translation_factor_group_product.h" -namespace sophus { +namespace sophus2 { namespace lie { template < @@ -266,4 +266,4 @@ class Group { }; } // namespace lie -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/lie_group_test.cpp b/cpp/sophus2/lie/lie_group_test.cpp similarity index 84% rename from cpp/sophus/lie/lie_group_test.cpp rename to cpp/sophus2/lie/lie_group_test.cpp index 3fbbaeb2..e009bc98 100644 --- a/cpp/sophus/lie/lie_group_test.cpp +++ b/cpp/sophus2/lie/lie_group_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/lie_group.h" +#include "sophus2/lie/lie_group.h" #include -using namespace sophus; +using namespace sophus2; TEST(lie_group, unit) {} diff --git a/cpp/sophus/lie/pose3.h b/cpp/sophus2/lie/pose3.h similarity index 97% rename from cpp/sophus/lie/pose3.h rename to cpp/sophus2/lie/pose3.h index c1755836..26004f57 100644 --- a/cpp/sophus/lie/pose3.h +++ b/cpp/sophus2/lie/pose3.h @@ -8,9 +8,9 @@ #pragma once -#include "sophus/lie/isometry3.h" +#include "sophus2/lie/isometry3.h" -namespace sophus { +namespace sophus2 { template class Pose3 { @@ -61,7 +61,8 @@ class Pose3 { /// Changes origin frame of tangent vector from frame ``bar`` to frame /// ``foo``. static Tangent changeTangentOrigin( - sophus::Isometry3F64 const& foo_from_bar, Tangent const& tangent_in_bar) { + sophus2::Isometry3F64 const& foo_from_bar, + Tangent const& tangent_in_bar) { return foo_from_bar.adj() * tangent_in_bar; } @@ -192,4 +193,4 @@ class Pose3 { using Pose3F32 = Pose3; using Pose3F64 = Pose3; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/pose3_test.cpp b/cpp/sophus2/lie/pose3_test.cpp similarity index 97% rename from cpp/sophus/lie/pose3_test.cpp rename to cpp/sophus2/lie/pose3_test.cpp index 97210416..f95b10f7 100644 --- a/cpp/sophus/lie/pose3_test.cpp +++ b/cpp/sophus2/lie/pose3_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/pose3.h" +#include "sophus2/lie/pose3.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(pose3F64, unit_tests) { Pose3F64 a_from_b(Isometry3F64(), "a", "b"); @@ -85,4 +85,4 @@ TEST(robot_velocity, unit_tests) { "respect to itself be zero by definition."); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/rotation2.h b/cpp/sophus2/lie/rotation2.h similarity index 92% rename from cpp/sophus/lie/rotation2.h rename to cpp/sophus2/lie/rotation2.h index 69f8aaab..241b74df 100644 --- a/cpp/sophus/lie/rotation2.h +++ b/cpp/sophus2/lie/rotation2.h @@ -8,12 +8,12 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/rotation2.h" -#include "sophus/lie/lie_group.h" -#include "sophus/linalg/orthogonal.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/rotation2.h" +#include "sophus2/lie/lie_group.h" +#include "sophus2/linalg/orthogonal.h" -namespace sophus { +namespace sophus2 { // definition: origin and distance preserving mapping in R^2 template @@ -92,4 +92,4 @@ using Rotation2F32 = Rotation2; using Rotation2F64 = Rotation2; static_assert(concepts::Rotation2); -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/rotation2_test.cpp b/cpp/sophus2/lie/rotation2_test.cpp similarity index 75% rename from cpp/sophus/lie/rotation2_test.cpp rename to cpp/sophus2/lie/rotation2_test.cpp index 12e42dcc..6b705e23 100644 --- a/cpp/sophus/lie/rotation2_test.cpp +++ b/cpp/sophus2/lie/rotation2_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/rotation2.h" +#include "sophus2/lie/rotation2.h" -#include "sophus/concepts/group_accessors_unit_tests.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors_unit_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(rotation2, unit_tests) { runRotation2UnitTests>(); @@ -25,4 +25,4 @@ TEST(rotation2, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Rotation2F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/rotation3.h b/cpp/sophus2/lie/rotation3.h similarity index 91% rename from cpp/sophus/lie/rotation3.h rename to cpp/sophus2/lie/rotation3.h index eb891b56..07ddd491 100644 --- a/cpp/sophus/lie/rotation3.h +++ b/cpp/sophus2/lie/rotation3.h @@ -8,12 +8,12 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/rotation3.h" -#include "sophus/lie/lie_group.h" -#include "sophus/linalg/orthogonal.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/rotation3.h" +#include "sophus2/lie/lie_group.h" +#include "sophus2/linalg/orthogonal.h" -namespace sophus { +namespace sophus2 { // definition: origin and distance preserving mapping in R^2 template @@ -106,7 +106,7 @@ auto rotThroughPoints( using std::atan2; Eigen::Vector from_cross_to = from.vector().cross(to.vector()); TScalar n = from_cross_to.norm(); - if (abs(n) < sophus::kEpsilon) { + if (abs(n) < sophus2::kEpsilon) { return Rotation3(); } // https://stackoverflow.com/a/32724066 @@ -131,7 +131,7 @@ auto rotThroughPoints( using std::atan2; Eigen::Vector from_cross_to = from.cross(to); TScalar n = from_cross_to.norm(); - if (abs(n) < sophus::kEpsilon) { + if (abs(n) < sophus2::kEpsilon) { return Rotation3(); } // https://stackoverflow.com/a/32724066 @@ -146,14 +146,14 @@ static_assert(concepts::Rotation3); namespace details { template -class Cast> { +class Cast> { public: template - static auto impl(sophus::Rotation3 const& v) { + static auto impl(sophus2::Rotation3 const& v) { return v.template cast(); } template - static auto implScalar(sophus::Rotation3 const& v) { + static auto implScalar(sophus2::Rotation3 const& v) { return v.template cast(); } }; @@ -164,4 +164,4 @@ using SO3 = Rotation3; // NOLINT using SO3f = Rotation3; // NOLINT using SO3d = Rotation3; // NOLINT -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/rotation3_test.cpp b/cpp/sophus2/lie/rotation3_test.cpp similarity index 75% rename from cpp/sophus/lie/rotation3_test.cpp rename to cpp/sophus2/lie/rotation3_test.cpp index 5a8aad60..745eaf50 100644 --- a/cpp/sophus/lie/rotation3_test.cpp +++ b/cpp/sophus2/lie/rotation3_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/rotation3.h" +#include "sophus2/lie/rotation3.h" -#include "sophus/concepts/group_accessors_unit_tests.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors_unit_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(rotation3, unit_tests) { runRotation3UnitTests>(); @@ -24,4 +24,4 @@ TEST(rotation3, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Rotation3F64"); LieGroupPropTestSuite>::runAllTests("Rotation3F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/scaling.h b/cpp/sophus2/lie/scaling.h similarity index 89% rename from cpp/sophus/lie/scaling.h rename to cpp/sophus2/lie/scaling.h index 01985d61..104b19d3 100644 --- a/cpp/sophus/lie/scaling.h +++ b/cpp/sophus2/lie/scaling.h @@ -8,11 +8,11 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/scaling.h" -#include "sophus/lie/lie_group.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/scaling.h" +#include "sophus2/lie/lie_group.h" -namespace sophus { +namespace sophus2 { // origin, coordinate axis directions, and shape preserving mapping @@ -56,4 +56,4 @@ using Scaling2 = Scaling; template using Scaling3 = Scaling; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/scaling_test.cpp b/cpp/sophus2/lie/scaling_test.cpp similarity index 81% rename from cpp/sophus/lie/scaling_test.cpp rename to cpp/sophus2/lie/scaling_test.cpp index 4a708722..3e0f1b3d 100644 --- a/cpp/sophus/lie/scaling_test.cpp +++ b/cpp/sophus2/lie/scaling_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/scaling.h" +#include "sophus2/lie/scaling.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(Scaling, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Scaling2F64"); @@ -20,4 +20,4 @@ TEST(Scaling, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Scaling3F64"); LieGroupPropTestSuite>::runAllTests("Scaling3F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/scaling_translation.h b/cpp/sophus2/lie/scaling_translation.h similarity index 91% rename from cpp/sophus/lie/scaling_translation.h rename to cpp/sophus2/lie/scaling_translation.h index 03f16154..33e498c4 100644 --- a/cpp/sophus/lie/scaling_translation.h +++ b/cpp/sophus2/lie/scaling_translation.h @@ -8,12 +8,12 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/scaling.h" -#include "sophus/lie/impl/translation_factor_group_product.h" -#include "sophus/lie/lie_group.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/scaling.h" +#include "sophus2/lie/impl/translation_factor_group_product.h" +#include "sophus2/lie/lie_group.h" -namespace sophus { +namespace sophus2 { template class ScalingTranslation; @@ -87,4 +87,4 @@ using ScalingTranslation2F64 = ScalingTranslation2; using ScalingTranslation3F64 = ScalingTranslation3; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/scaling_translation_test.cpp b/cpp/sophus2/lie/scaling_translation_test.cpp similarity index 78% rename from cpp/sophus/lie/scaling_translation_test.cpp rename to cpp/sophus2/lie/scaling_translation_test.cpp index 61d09e6b..95db5a97 100644 --- a/cpp/sophus/lie/scaling_translation_test.cpp +++ b/cpp/sophus2/lie/scaling_translation_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/scaling_translation.h" +#include "sophus2/lie/scaling_translation.h" -#include "sophus/concepts/group_accessors_unit_tests.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors_unit_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(ScalingTranslation, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests( @@ -25,4 +25,4 @@ TEST(ScalingTranslation, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests( "ScalingTranslation3F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/se3.h b/cpp/sophus2/lie/se3.h similarity index 88% rename from cpp/sophus/lie/se3.h rename to cpp/sophus2/lie/se3.h index ac801995..266d4dec 100644 --- a/cpp/sophus/lie/se3.h +++ b/cpp/sophus2/lie/se3.h @@ -8,4 +8,4 @@ #pragma once -#include "sophus/lie/isometry3.h" +#include "sophus2/lie/isometry3.h" diff --git a/cpp/sophus/lie/similarity2.h b/cpp/sophus2/lie/similarity2.h similarity index 94% rename from cpp/sophus/lie/similarity2.h rename to cpp/sophus2/lie/similarity2.h index 51e84d4f..2e46f6a2 100644 --- a/cpp/sophus/lie/similarity2.h +++ b/cpp/sophus2/lie/similarity2.h @@ -8,14 +8,14 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/spiral_similarity2.h" -#include "sophus/lie/impl/translation_factor_group_product.h" -#include "sophus/lie/isometry2.h" -#include "sophus/lie/lie_group.h" -#include "sophus/lie/spiral_similarity2.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/spiral_similarity2.h" +#include "sophus2/lie/impl/translation_factor_group_product.h" +#include "sophus2/lie/isometry2.h" +#include "sophus2/lie/lie_group.h" +#include "sophus2/lie/spiral_similarity2.h" -namespace sophus { +namespace sophus2 { // origin, coordinate axis directions, and shape preserving mapping template @@ -175,4 +175,4 @@ using Similarity2F64 = Similarity2; static_assert(concepts::Similarity2); -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/similarity2_test.cpp b/cpp/sophus2/lie/similarity2_test.cpp similarity index 75% rename from cpp/sophus/lie/similarity2_test.cpp rename to cpp/sophus2/lie/similarity2_test.cpp index 7a298bc6..235cc8e8 100644 --- a/cpp/sophus/lie/similarity2_test.cpp +++ b/cpp/sophus2/lie/similarity2_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/similarity2.h" +#include "sophus2/lie/similarity2.h" -#include "sophus/concepts/group_accessors_unit_tests.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors_unit_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(similarity2, unit_tests) { runSimilarity2UnitTests>(); @@ -25,4 +25,4 @@ TEST(Similarity2, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Similarity2F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/similarity3.h b/cpp/sophus2/lie/similarity3.h similarity index 94% rename from cpp/sophus/lie/similarity3.h rename to cpp/sophus2/lie/similarity3.h index 9e4fb10f..1fa6c068 100644 --- a/cpp/sophus/lie/similarity3.h +++ b/cpp/sophus2/lie/similarity3.h @@ -8,14 +8,14 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/spiral_similarity3.h" -#include "sophus/lie/impl/translation_factor_group_product.h" -#include "sophus/lie/isometry3.h" -#include "sophus/lie/lie_group.h" -#include "sophus/lie/spiral_similarity3.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/spiral_similarity3.h" +#include "sophus2/lie/impl/translation_factor_group_product.h" +#include "sophus2/lie/isometry3.h" +#include "sophus2/lie/lie_group.h" +#include "sophus2/lie/spiral_similarity3.h" -namespace sophus { +namespace sophus2 { // origin, coordinate axis directions, and shape preserving mapping template @@ -190,4 +190,4 @@ using Similarity3F64 = Similarity3; static_assert(concepts::Similarity3); -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/similarity3_test.cpp b/cpp/sophus2/lie/similarity3_test.cpp similarity index 75% rename from cpp/sophus/lie/similarity3_test.cpp rename to cpp/sophus2/lie/similarity3_test.cpp index b45ac8da..522f5bd6 100644 --- a/cpp/sophus/lie/similarity3_test.cpp +++ b/cpp/sophus2/lie/similarity3_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/similarity3.h" +#include "sophus2/lie/similarity3.h" -#include "sophus/concepts/group_accessors_unit_tests.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors_unit_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(similarity3, unit_tests) { runSimilarity3UnitTests>(); @@ -25,4 +25,4 @@ TEST(identity, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Similarity3F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/so3.h b/cpp/sophus2/lie/so3.h similarity index 88% rename from cpp/sophus/lie/so3.h rename to cpp/sophus2/lie/so3.h index 8aaf40e2..92cda572 100644 --- a/cpp/sophus/lie/so3.h +++ b/cpp/sophus2/lie/so3.h @@ -8,4 +8,4 @@ #pragma once -#include "sophus/lie/rotation3.h" +#include "sophus2/lie/rotation3.h" diff --git a/cpp/sophus/lie/spiral_similarity2.h b/cpp/sophus2/lie/spiral_similarity2.h similarity index 93% rename from cpp/sophus/lie/spiral_similarity2.h rename to cpp/sophus2/lie/spiral_similarity2.h index 873eb446..d0fee616 100644 --- a/cpp/sophus/lie/spiral_similarity2.h +++ b/cpp/sophus2/lie/spiral_similarity2.h @@ -8,12 +8,12 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/spiral_similarity2.h" -#include "sophus/lie/lie_group.h" -#include "sophus/lie/rotation2.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/spiral_similarity2.h" +#include "sophus2/lie/lie_group.h" +#include "sophus2/lie/rotation2.h" -namespace sophus { +namespace sophus2 { // origin and shape preserving mapping template @@ -102,4 +102,4 @@ using SpiralSimilarity2F64 = SpiralSimilarity2; static_assert(concepts::SpiralSimilarity2); -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/spiral_similarity2_test.cpp b/cpp/sophus2/lie/spiral_similarity2_test.cpp similarity index 77% rename from cpp/sophus/lie/spiral_similarity2_test.cpp rename to cpp/sophus2/lie/spiral_similarity2_test.cpp index 15aced50..e35912ac 100644 --- a/cpp/sophus/lie/spiral_similarity2_test.cpp +++ b/cpp/sophus2/lie/spiral_similarity2_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/spiral_similarity2.h" +#include "sophus2/lie/spiral_similarity2.h" -#include "sophus/concepts/group_accessors_unit_tests.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors_unit_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(SpiralSimilarity2, unit_tests) { runSpiralSimilarity2UnitTests>(); @@ -26,4 +26,4 @@ TEST(SpiralSimilarity2, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests( "SpiralSimilarity2F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/spiral_similarity3.h b/cpp/sophus2/lie/spiral_similarity3.h similarity index 93% rename from cpp/sophus/lie/spiral_similarity3.h rename to cpp/sophus2/lie/spiral_similarity3.h index c827347f..2d271959 100644 --- a/cpp/sophus/lie/spiral_similarity3.h +++ b/cpp/sophus2/lie/spiral_similarity3.h @@ -8,12 +8,12 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/spiral_similarity3.h" -#include "sophus/lie/lie_group.h" -#include "sophus/lie/rotation3.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/spiral_similarity3.h" +#include "sophus2/lie/lie_group.h" +#include "sophus2/lie/rotation3.h" -namespace sophus { +namespace sophus2 { // origin and shape preserving mapping template @@ -111,4 +111,4 @@ using SpiralSimilarity3F64 = SpiralSimilarity3; static_assert(concepts::SpiralSimilarity3); -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/spiral_similarity3_test.cpp b/cpp/sophus2/lie/spiral_similarity3_test.cpp similarity index 77% rename from cpp/sophus/lie/spiral_similarity3_test.cpp rename to cpp/sophus2/lie/spiral_similarity3_test.cpp index fd302edb..8ecaeeae 100644 --- a/cpp/sophus/lie/spiral_similarity3_test.cpp +++ b/cpp/sophus2/lie/spiral_similarity3_test.cpp @@ -6,14 +6,14 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/spiral_similarity3.h" +#include "sophus2/lie/spiral_similarity3.h" -#include "sophus/concepts/group_accessors_unit_tests.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/group_accessors_unit_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(SpiralSimilarity3_lie_group_prop_tests_Test, unit_tests) { runSpiralSimilarity3UnitTests>(); @@ -26,4 +26,4 @@ TEST(SpiralSimilarity3, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests( "SpiralSimilarity3F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/lie/translation.h b/cpp/sophus2/lie/translation.h similarity index 90% rename from cpp/sophus/lie/translation.h rename to cpp/sophus2/lie/translation.h index 4663a1a4..905266d1 100644 --- a/cpp/sophus/lie/translation.h +++ b/cpp/sophus2/lie/translation.h @@ -8,12 +8,12 @@ #pragma once -#include "sophus/concepts/group_accessors.h" -#include "sophus/lie/impl/identity.h" -#include "sophus/lie/impl/translation_factor_group_product.h" -#include "sophus/lie/lie_group.h" +#include "sophus2/concepts/group_accessors.h" +#include "sophus2/lie/impl/identity.h" +#include "sophus2/lie/impl/translation_factor_group_product.h" +#include "sophus2/lie/lie_group.h" -namespace sophus { +namespace sophus2 { template class Translation; @@ -82,4 +82,4 @@ using Translation3F64 = Translation3; static_assert(concepts::Translation); -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/lie/translation_test.cpp b/cpp/sophus2/lie/translation_test.cpp similarity index 82% rename from cpp/sophus/lie/translation_test.cpp rename to cpp/sophus2/lie/translation_test.cpp index e14b402c..49d8d889 100644 --- a/cpp/sophus/lie/translation_test.cpp +++ b/cpp/sophus2/lie/translation_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/translation.h" +#include "sophus2/lie/translation.h" -#include "sophus/concepts/lie_group_prop_tests.h" +#include "sophus2/concepts/lie_group_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(translation, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Translation2F64"); @@ -20,4 +20,4 @@ TEST(translation, lie_group_prop_tests) { LieGroupPropTestSuite>::runAllTests("Translation3F64"); LieGroupPropTestSuite>::runAllTests("Translation3F32"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus2/linalg/CMakeLists.txt b/cpp/sophus2/linalg/CMakeLists.txt new file mode 100644 index 00000000..9fafb391 --- /dev/null +++ b/cpp/sophus2/linalg/CMakeLists.txt @@ -0,0 +1,40 @@ +#[[ +sophus2_linalg + + + +]] + +set(sophus2_linalg_src_prefixes + batch + cast + homogeneous + orthogonal + reduce + vector_space + vector_space_traits +) + +set(sophus2_linalg_h) + +foreach(x ${sophus2_linalg_src_prefixes} ) + list(APPEND sophus2_linalg_h ${x}.h) +endforeach() + +farm_ng_add_library(sophus2_linalg + NAMESPACE ${PROJECT_NAME} + INCLUDE_DIR ../../ + HEADERS + ${sophus2_linalg_h} +) + +target_link_libraries(sophus2_linalg INTERFACE sophus2_common sophus_concept) + +if(${BUILD_SOPHUS_TESTS}) + foreach(test_basename ${sophus2_linalg_src_prefixes}) + farm_ng_add_test(${test_basename} + PARENT_LIBRARY sophus2_linalg + LINK_LIBRARIES sophus2_linalg + LABELS small) + endforeach() +endif() diff --git a/cpp/sophus2/linalg/batch.h b/cpp/sophus2/linalg/batch.h new file mode 100644 index 00000000..98003ebf --- /dev/null +++ b/cpp/sophus2/linalg/batch.h @@ -0,0 +1,149 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#pragma once + +#include "sophus2/common/common.h" + +namespace sophus2 { + +// Following similar pattern as ceres::Jet +template +struct Batch { + Batch() {} + + explicit Batch(Eigen::Array const& b_) : b(b_) {} + + // Constructor from scalar: a + 0. + explicit Batch(const TT& value) { b.setConstant(TT(value)); } + + // Compound operators + Batch& operator+=(Batch const& y) { + *this = *this + y; + return *this; + } + + Batch operator-() const { return Batch(-this->b); } + + bool operator==(Batch const& y) { return (this->b == y.b).all(); } + + Eigen::Array b; +}; + +template +auto operator<<(std::ostream& os, Batch const& batch) + -> std::ostream& { + os << "[" << batch.b << "]"; + return os; +} + +template +inline Batch hypot(Batch const& f, Batch const& g) { + return Batch{((f.b * f.b) + (g.b * g.b)).sqrt().eval()}; +} + +template +inline Batch abs(Batch const& f) { + return Batch{f.b.abs().eval()}; +} + +template +inline Batch operator-(Batch const& f, Batch const& g) { + return Batch{(f.b - g.b).eval()}; +} + +template +inline Batch operator+(Batch const& f, Batch const& g) { + return Batch{(f.b + g.b).eval()}; +} +template +inline Batch operator*(Batch const& f, Batch const& g) { + return Batch{(f.b * g.b).eval()}; +} + +template +inline Batch operator/(Batch const& f, Batch const& g) { + return Batch{(f.b / g.b).eval()}; +} + +template +inline Batch operator*(Batch const& f, TT s) { + return Batch{(f.b * s).eval()}; +} + +template +inline Batch operator*(TT s, Batch const& f) { + return Batch{(s * f.b).eval()}; +} + +template +struct BatchTrait { + using Scalar = TScalar; + using ScalarBatch = TScalar; + static int constexpr kNumSingeScalars = 1; + static bool constexpr kIsBatch = false; +}; + +template +struct BatchTrait> { + using Scalar = TScalar; + using ScalarBatch = Batch; + static int constexpr kNumSingeScalars = kNum; + static bool constexpr kIsBatch = true; +}; + +} // namespace sophus + +namespace Eigen { + +// This is mirrored from ceres::Jet. +template +struct NumTraits> { + using Real = sophus2::Batch; + using NonInteger = sophus2::Batch; + using Nested = sophus2::Batch; + using Literal = sophus2::Batch; + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned, + ReadCost = 1, + AddCost = 1, + MulCost = 2, + HasFloatingPoint = 1, + RequireInitialization = 1 + }; + + static sophus2::Batch dummy_precision() { + return sophus2::Batch(1e-12); + } + + inline static Real epsilon() { + return Real(std::numeric_limits::epsilon()); + } + + inline static int digits10() { return NumTraits::digits10(); } + + inline static Real highest() { + return Real((std::numeric_limits::max)()); + } + inline static Real lowest() { + return Real(-(std::numeric_limits::max)()); + } +}; + +template +struct ScalarBinaryOpTraits, TT, BinaryOp> { + using ReturnType = sophus2::Batch; +}; +template +struct ScalarBinaryOpTraits, BinaryOp> { + using ReturnType = sophus2::Batch; +}; + +} // namespace Eigen diff --git a/cpp/sophus2/linalg/batch_test.cpp b/cpp/sophus2/linalg/batch_test.cpp new file mode 100644 index 00000000..5f2d2003 --- /dev/null +++ b/cpp/sophus2/linalg/batch_test.cpp @@ -0,0 +1,66 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include "sophus2/linalg/batch.h" + +#include "sophus2/linalg/orthogonal.h" + +#include + +using namespace sophus; + +TEST(batch, unit) { + int constexpr kNumBatches = 8; + int constexpr kM = 7; + int constexpr kN = 3; + int constexpr kO = 5; + + std::vector> mat7x3_vec; + std::vector> mat3x5_vec; + std::vector> mat7x5_vec; + + for (int b = 0; b < kNumBatches; ++b) { + mat7x3_vec.push_back(Eigen::Matrix::Random()); + mat3x5_vec.push_back(Eigen::Matrix::Random()); + } + + Eigen::Matrix, kM, kN> batched_mat_7x3; + for (int m = 0; m < kM; ++m) { + for (int n = 0; n < kN; ++n) { + Batch& batch = batched_mat_7x3(m, n); + for (int b = 0; b < kNumBatches; ++b) { + batch.b[b] = mat7x3_vec[b](m, n); + } + } + } + + Eigen::Matrix, kN, kO> batched_mat_3x5; + for (int n = 0; n < kN; ++n) { + for (int o = 0; o < kO; ++o) { + Batch& batch = batched_mat_3x5(n, o); + for (int b = 0; b < kNumBatches; ++b) { + batch.b[b] = mat3x5_vec[b](n, o); + } + } + } + + for (int b = 0; b < kNumBatches; ++b) { + mat7x5_vec.push_back(mat7x3_vec[b] * mat3x5_vec[b]); + } + Eigen::Matrix, kM, kO> batched_mat_7x5 = + batched_mat_7x3 * batched_mat_3x5; + + for (int m = 0; m < kM; ++m) { + for (int o = 0; o < kO; ++o) { + Batch& batch = batched_mat_7x5(m, o); + for (int b = 0; b < kNumBatches; ++b) { + FARM_ASSERT_NEAR(batch.b[b], mat7x5_vec[b](m, o), 0.001); + } + } + } +} diff --git a/cpp/sophus/linalg/cast.h b/cpp/sophus2/linalg/cast.h similarity index 90% rename from cpp/sophus/linalg/cast.h rename to cpp/sophus2/linalg/cast.h index fc66e5ca..568a8ef2 100644 --- a/cpp/sophus/linalg/cast.h +++ b/cpp/sophus2/linalg/cast.h @@ -9,7 +9,7 @@ #pragma once -#include "sophus/concepts/point.h" +#include "sophus2/concepts/point.h" #include @@ -17,7 +17,7 @@ #include #include -namespace sophus { +namespace sophus2 { namespace details { @@ -34,7 +34,7 @@ class Cast { } }; -template <::sophus::concepts::EigenType TT> +template <::sophus2::concepts::EigenType TT> class Cast { public: template @@ -79,9 +79,9 @@ auto cast(const TT& p) { return details::Cast::template impl(p); } -template <::sophus::concepts::Arithmetic TTo, class TT> +template <::sophus2::concepts::Arithmetic TTo, class TT> auto cast(const TT& p) { return details::Cast::template implScalar(p); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/linalg/cast_test.cpp b/cpp/sophus2/linalg/cast_test.cpp similarity index 84% rename from cpp/sophus/linalg/cast_test.cpp rename to cpp/sophus2/linalg/cast_test.cpp index b9a9485d..e1b22820 100644 --- a/cpp/sophus/linalg/cast_test.cpp +++ b/cpp/sophus2/linalg/cast_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/linalg/cast.h" +#include "sophus2/linalg/cast.h" #include -using namespace sophus; +using namespace sophus2; TEST(cast, unit) {} diff --git a/cpp/sophus/linalg/homogeneous.h b/cpp/sophus2/linalg/homogeneous.h similarity index 94% rename from cpp/sophus/linalg/homogeneous.h rename to cpp/sophus2/linalg/homogeneous.h index 3a6f988c..f2dd956e 100644 --- a/cpp/sophus/linalg/homogeneous.h +++ b/cpp/sophus2/linalg/homogeneous.h @@ -8,9 +8,9 @@ #pragma once -#include "sophus/common/common.h" +#include "sophus2/common/common.h" -namespace sophus { +namespace sophus2 { /// Projects 3-point (x,y,z) through the origin (0,0,0) onto the plane z=1. /// Hence it returns (x/z, y/z). @@ -40,4 +40,4 @@ auto unproj( return out; } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/linalg/homogeneous_test.cpp b/cpp/sophus2/linalg/homogeneous_test.cpp similarity index 88% rename from cpp/sophus/linalg/homogeneous_test.cpp rename to cpp/sophus2/linalg/homogeneous_test.cpp index cad2315d..1bafdd68 100644 --- a/cpp/sophus/linalg/homogeneous_test.cpp +++ b/cpp/sophus2/linalg/homogeneous_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/linalg/homogeneous.h" +#include "sophus2/linalg/homogeneous.h" -#include "sophus/linalg/vector_space.h" +#include "sophus2/linalg/vector_space.h" #include -using namespace sophus; +using namespace sophus2; template void unprojProjRountripTest() { diff --git a/cpp/sophus/linalg/orthogonal.h b/cpp/sophus2/linalg/orthogonal.h similarity index 97% rename from cpp/sophus/linalg/orthogonal.h rename to cpp/sophus2/linalg/orthogonal.h index f1182ba8..53f094f9 100644 --- a/cpp/sophus/linalg/orthogonal.h +++ b/cpp/sophus2/linalg/orthogonal.h @@ -11,12 +11,12 @@ #pragma once -#include "sophus/common/common.h" +#include "sophus2/common/common.h" #include #include -namespace sophus { +namespace sophus2 { /// Takes in arbitrary square matrix and returns true if it is /// orthogonal. @@ -90,4 +90,4 @@ auto makeRotationMatrix(Eigen::MatrixBase const& r) -> std::enable_if_t< return svd.matrixU() * diag * svd.matrixV().transpose(); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/linalg/orthogonal_test.cpp b/cpp/sophus2/linalg/orthogonal_test.cpp similarity index 83% rename from cpp/sophus/linalg/orthogonal_test.cpp rename to cpp/sophus2/linalg/orthogonal_test.cpp index c6b86ffe..ed790586 100644 --- a/cpp/sophus/linalg/orthogonal_test.cpp +++ b/cpp/sophus2/linalg/orthogonal_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/linalg/orthogonal.h" +#include "sophus2/linalg/orthogonal.h" #include -using namespace sophus; +using namespace sophus2; TEST(orthogonal, unit) {} diff --git a/cpp/sophus/linalg/reduce.h b/cpp/sophus2/linalg/reduce.h similarity index 95% rename from cpp/sophus/linalg/reduce.h rename to cpp/sophus2/linalg/reduce.h index 6454ab70..21826dfb 100644 --- a/cpp/sophus/linalg/reduce.h +++ b/cpp/sophus2/linalg/reduce.h @@ -9,7 +9,7 @@ #pragma once -#include "sophus/concepts/point.h" +#include "sophus2/concepts/point.h" #include @@ -17,7 +17,7 @@ #include #include -namespace sophus { +namespace sophus2 { namespace concepts { namespace details { @@ -44,7 +44,7 @@ class Reduce { }; /// Reduction specialization over Eigen matrices -template <::sophus::concepts::EigenDenseType TT> +template <::sophus2::concepts::EigenDenseType TT> class Reduce { public: /// Unary reduction operator @@ -99,4 +99,4 @@ auto reduce( } } // namespace concepts -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/linalg/reduce_test.cpp b/cpp/sophus2/linalg/reduce_test.cpp similarity index 84% rename from cpp/sophus/linalg/reduce_test.cpp rename to cpp/sophus2/linalg/reduce_test.cpp index 0faf046f..38747f73 100644 --- a/cpp/sophus/linalg/reduce_test.cpp +++ b/cpp/sophus2/linalg/reduce_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/linalg/reduce.h" +#include "sophus2/linalg/reduce.h" #include -using namespace sophus; +using namespace sophus2; TEST(reduce, unit) {} diff --git a/cpp/sophus/linalg/vector_space.h b/cpp/sophus2/linalg/vector_space.h similarity index 74% rename from cpp/sophus/linalg/vector_space.h rename to cpp/sophus2/linalg/vector_space.h index 08975f6b..abdf6f51 100644 --- a/cpp/sophus/linalg/vector_space.h +++ b/cpp/sophus2/linalg/vector_space.h @@ -9,9 +9,9 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/linalg/cast.h" -#include "sophus/linalg/reduce.h" +#include "sophus2/common/common.h" +#include "sophus2/linalg/cast.h" +#include "sophus2/linalg/reduce.h" #include @@ -19,177 +19,177 @@ #include #include -namespace sophus { +namespace sophus2 { namespace details { // EigenDenseType may be a map or view or abstract base class or something. // This is an alias for the corresponding concrete type with storage -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> using EigenConcreteType = std::decay_t().eval())>; } // namespace details -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> [[nodiscard]] auto zero() -> TPoint { return 0; } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> [[nodiscard]] auto zero() -> TPoint { return TPoint::Zero(); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto eval(TPoint const& p) { return p; } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto eval(TPoint const& p) { return p.eval(); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto allTrue(TPoint const& p) -> bool { return bool(p); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto allTrue(TPoint const& p) -> bool { return p.all(); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto anyTrue(TPoint const& p) -> bool { return bool(p); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto anyTrue(TPoint const& p) -> bool { return p.any(); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto isFinite(TPoint const& p) -> bool { return std::isfinite(p); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto isFinite(TPoint const& p) -> bool { return p.array().isFinite().all(); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto isNan(TPoint const& p) -> bool { return std::isnan(p); } -template <::sophus::concepts::PointType TPoint> +template <::sophus2::concepts::PointType TPoint> auto isNan(TPoint const& p) -> bool { return p.array().isNaN().all(); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto square(TPoint const& v) { return v * v; } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto square(TPoint const& v) { return v.squaredNorm(); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto min(TPoint const& a, TPoint const& b) -> TPoint { return std::min(a, b); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto min(TPoint const& a, TPoint const& b) -> TPoint { return a.cwiseMin(b); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto max(TPoint const& a, TPoint const& b) -> TPoint { return std::max(a, b); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto max(TPoint const& a, TPoint const& b) -> TPoint { return a.cwiseMax(b); } -template <::sophus::concepts::PointType TPoint> +template <::sophus2::concepts::PointType TPoint> auto clamp(TPoint const& val, TPoint const& lo, TPoint const& hi) -> TPoint { - return sophus::max(lo, sophus::min(val, hi)); + return sophus2::max(lo, sophus2::min(val, hi)); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto floor(TPoint s) { using std::floor; return floor(s); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto floor(TPoint p) { for (auto& e : p.reshaped()) { - e = sophus::floor(e); + e = sophus2::floor(e); } return p; } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto ceil(TPoint s) { using std::ceil; return ceil(s); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto ceil(TPoint p) { for (auto& e : p.reshaped()) { - e = sophus::ceil(e); + e = sophus2::ceil(e); } return p; } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> auto round(TPoint s) { using std::ceil; return ceil(s); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> auto round(TPoint p) { for (auto& e : p.reshaped()) { - e = sophus::round(e); + e = sophus2::round(e); } return p; } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> [[nodiscard]] auto plus(TPoint p, TPoint s) { p += s; return p; } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> [[nodiscard]] auto plus(TPoint p, typename TPoint::Scalar s) { p.array() += s; return p; } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> [[nodiscard]] auto isLessEqual(TPoint const& lhs, TPoint const& rhs) -> bool { return lhs <= rhs; } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> [[nodiscard]] auto isLessEqual(TPoint const& lhs, TPoint const& rhs) -> bool { return allTrue(eval(lhs.array() <= rhs.array())); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> [[nodiscard]] auto tryGetElem(TPoint const& p, size_t row, size_t col = 0) -> Expected { if (row == 0 && col == 0) { @@ -198,7 +198,7 @@ template <::sophus::concepts::ScalarType TPoint> return SOPHUS_UNEXPECTED("row ({}) and col ({}) must be 0", row, col); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> [[nodiscard]] auto tryGetElem(TPoint const& p, size_t row, size_t col = 0) -> Expected { if (row < p.rows() && col < p.cols()) { @@ -208,7 +208,7 @@ template <::sophus::concepts::EigenDenseType TPoint> "({}, {}) access of array of size {} x {}", row, col, p.rows(), p.cols()); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> [[nodiscard]] auto trySetElem(TPoint& p, TPoint s, size_t row, size_t col = 0) -> Expected { if (row == 0 && col == 0) { @@ -218,7 +218,7 @@ template <::sophus::concepts::ScalarType TPoint> return SOPHUS_UNEXPECTED("row ({}) and col ({}) must be 0", row, col); } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> [[nodiscard]] auto trySetElem( TPoint& p, typename TPoint::Scalar s, size_t row, size_t col = 0) -> Expected { @@ -229,12 +229,12 @@ template <::sophus::concepts::EigenDenseType TPoint> return SOPHUS_UNEXPECTED("row ({}) and col ({}) must be 0", row, col); } -template <::sophus::concepts::ScalarType TPoint> +template <::sophus2::concepts::ScalarType TPoint> [[nodiscard]] auto transpose(TPoint p) { return p; } -template <::sophus::concepts::EigenDenseType TPoint> +template <::sophus2::concepts::EigenDenseType TPoint> [[nodiscard]] auto transpose(TPoint p) { return p.transpose(); } @@ -308,4 +308,4 @@ template return details::PointExamples::impl(); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/linalg/vector_space_test.cpp b/cpp/sophus2/linalg/vector_space_test.cpp similarity index 83% rename from cpp/sophus/linalg/vector_space_test.cpp rename to cpp/sophus2/linalg/vector_space_test.cpp index a7773df6..17bb072b 100644 --- a/cpp/sophus/linalg/vector_space_test.cpp +++ b/cpp/sophus2/linalg/vector_space_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/linalg/vector_space.h" +#include "sophus2/linalg/vector_space.h" #include -using namespace sophus; +using namespace sophus2; TEST(vector_space, unit) {} diff --git a/cpp/sophus/linalg/vector_space_traits.h b/cpp/sophus2/linalg/vector_space_traits.h similarity index 97% rename from cpp/sophus/linalg/vector_space_traits.h rename to cpp/sophus2/linalg/vector_space_traits.h index e7371cd4..31af169b 100644 --- a/cpp/sophus/linalg/vector_space_traits.h +++ b/cpp/sophus2/linalg/vector_space_traits.h @@ -8,13 +8,13 @@ #pragma once -#include "sophus/concepts/point.h" +#include "sophus2/concepts/point.h" #include #include -namespace sophus { +namespace sophus2 { /// Point trait class template. /// @@ -95,4 +95,4 @@ struct PointTraits { }; // END(exclude from doxygen) \endcond -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/linalg/vector_space_traits_test.cpp b/cpp/sophus2/linalg/vector_space_traits_test.cpp similarity index 81% rename from cpp/sophus/linalg/vector_space_traits_test.cpp rename to cpp/sophus2/linalg/vector_space_traits_test.cpp index b35d9ef7..ccf95306 100644 --- a/cpp/sophus/linalg/vector_space_traits_test.cpp +++ b/cpp/sophus2/linalg/vector_space_traits_test.cpp @@ -6,10 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/linalg/vector_space_traits.h" +#include "sophus2/linalg/vector_space_traits.h" #include -using namespace sophus; +using namespace sophus2; TEST(vector_space_traits, unit) {} diff --git a/cpp/sophus/manifold/CMakeLists.txt b/cpp/sophus2/manifold/CMakeLists.txt similarity index 84% rename from cpp/sophus/manifold/CMakeLists.txt rename to cpp/sophus2/manifold/CMakeLists.txt index 4c900da3..d700a95a 100644 --- a/cpp/sophus/manifold/CMakeLists.txt +++ b/cpp/sophus2/manifold/CMakeLists.txt @@ -20,19 +20,17 @@ foreach(x ${sophus_manifold_src_prefixes} ) endforeach() farm_ng_add_library(sophus_manifold - NAMESPACE Sophus + NAMESPACE ${PROJECT_NAME} INCLUDE_DIR ../../ HEADERS ${sophus_manifold_h} ) -target_link_libraries(sophus_manifold INTERFACE sophus_linalg) +target_link_libraries(sophus_manifold INTERFACE sophus2_linalg) -if(${BUILD_SOPHUS_TESTS}) foreach(test_basename ${sophus_manifold_src_prefixes}) farm_ng_add_test(${test_basename} PARENT_LIBRARY sophus_manifold LINK_LIBRARIES sophus_manifold LABELS small) endforeach() -endif() diff --git a/cpp/sophus/manifold/complex.h b/cpp/sophus2/manifold/complex.h similarity index 93% rename from cpp/sophus/manifold/complex.h rename to cpp/sophus2/manifold/complex.h index f6c8f9a3..0ad7d148 100644 --- a/cpp/sophus/manifold/complex.h +++ b/cpp/sophus2/manifold/complex.h @@ -7,20 +7,24 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/common/common.h" -#include "sophus/concepts/division_ring.h" -#include "sophus/linalg/vector_space.h" +#include "sophus2/common/common.h" +#include "sophus2/concepts/division_ring.h" +#include "sophus2/linalg/vector_space.h" +#include "sophus2/linalg/batch.h" -namespace sophus { + +namespace sophus2 { /// Generic complex number implementation /// /// Impl class without any storage, but only static methods. -template +template class ComplexImpl { public: /// The underlying scalar type. - using Scalar = TScalar; + using Scalar = typename BatchTrait::ScalarBatch; + static int constexpr kNumSingles = BatchTrait::kNumSingles; + /// A complex number is a tuple. static int constexpr kNumParams = 2; /// Complex multiplication is commutative (and it is a field itself). @@ -54,8 +58,8 @@ class ComplexImpl { /// There are no particular constraints on the complex space and this function /// returns always true. static auto areParamsValid(Params const& /*unused*/) - -> sophus::Expected { - return sophus::Expected{}; + -> sophus2::Expected { + return sophus2::Expected{}; } /// Returns examples of valid parameters. @@ -125,7 +129,7 @@ class ComplexImpl { /// /// A complex number is manifold with additional structure. In particular, /// it is - like the quaternion numbers - a division ring and fulfills the -/// sophus::concepts::DivisionRingConcept. Furthermore, it is has commutative +/// sophus2::concepts::DivisionRingConcept. Furthermore, it is has commutative /// multiplication (as opposed to the quaternion numbers) and hence is a field. /// The complex numbers are the only other field of real numbers in addition to /// the real numbers itself. @@ -234,4 +238,4 @@ class Complex { Eigen::Vector2 params_; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/manifold/complex_test.cpp b/cpp/sophus2/manifold/complex_test.cpp similarity index 67% rename from cpp/sophus/manifold/complex_test.cpp rename to cpp/sophus2/manifold/complex_test.cpp index c3937ba4..838ce984 100644 --- a/cpp/sophus/manifold/complex_test.cpp +++ b/cpp/sophus2/manifold/complex_test.cpp @@ -6,17 +6,22 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/manifold/complex.h" +#include "sophus2/manifold/complex.h" -#include "sophus/concepts/division_ring_prop_tests.h" +#include "sophus2/concepts/division_ring_prop_tests.h" #include -using namespace sophus; +using namespace sophus2; TEST(complex, unit) { Complex z; } TEST(complex, prop_test) { test::DivisionRingTestSuite>::runAllTests("Complex"); test::DivisionRingTestSuite>::runAllTests("Complex"); + + test::DivisionRingTestSuite>>::runAllTests( + "Batch"); + + Complex> complex; } diff --git a/cpp/sophus/manifold/product_manifold.h b/cpp/sophus2/manifold/product_manifold.h similarity index 99% rename from cpp/sophus/manifold/product_manifold.h rename to cpp/sophus2/manifold/product_manifold.h index 5bd4d1ee..ae302d8a 100644 --- a/cpp/sophus/manifold/product_manifold.h +++ b/cpp/sophus2/manifold/product_manifold.h @@ -8,9 +8,9 @@ #pragma once -#include "sophus/concepts/manifold.h" +#include "sophus2/concepts/manifold.h" -namespace sophus { +namespace sophus2 { // Credit: @bogdan at http://stackoverflow.com/q/37373602/6367128 template @@ -284,4 +284,4 @@ auto ProductManifold::averageImpl( return std::nullopt; } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/manifold/product_manifold_test.cpp b/cpp/sophus2/manifold/product_manifold_test.cpp similarity index 79% rename from cpp/sophus/manifold/product_manifold_test.cpp rename to cpp/sophus2/manifold/product_manifold_test.cpp index 685a7476..a3b298eb 100644 --- a/cpp/sophus/manifold/product_manifold_test.cpp +++ b/cpp/sophus2/manifold/product_manifold_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/manifold/product_manifold.h" +#include "sophus2/manifold/product_manifold.h" -#include "sophus/manifold/unit_vector.h" +#include "sophus2/manifold/unit_vector.h" #include -using namespace sophus; +using namespace sophus2; TEST(product_manifold, unit) { using Product = ProductManifold; diff --git a/cpp/sophus/manifold/quaternion.h b/cpp/sophus2/manifold/quaternion.h similarity index 96% rename from cpp/sophus/manifold/quaternion.h rename to cpp/sophus2/manifold/quaternion.h index 952e3afc..5baa74eb 100644 --- a/cpp/sophus/manifold/quaternion.h +++ b/cpp/sophus2/manifold/quaternion.h @@ -7,11 +7,11 @@ // https://opensource.org/licenses/MIT. #pragma once -#include "sophus/common/common.h" -#include "sophus/concepts/division_ring.h" -#include "sophus/linalg/vector_space.h" +#include "sophus2/common/common.h" +#include "sophus2/concepts/division_ring.h" +#include "sophus2/linalg/vector_space.h" -namespace sophus { +namespace sophus2 { /// Generic quaternion number implementation /// @@ -61,8 +61,8 @@ class QuaternionImpl { /// There are no particular constraints on the quaternion space and this /// function returns always true. static auto areParamsValid(Params const& /*unused*/) - -> sophus::Expected { - return sophus::Expected{}; + -> sophus2::Expected { + return sophus2::Expected{}; } /// Returns examples of valid parameters. @@ -131,7 +131,7 @@ class QuaternionImpl { /// /// A quaternion number is manifold with additional structure. In particular, /// it is - like the Quaternion numbers - a division ring and fulfills the -/// sophus::concepts::DivisionRingConcept. +/// sophus2::concepts::DivisionRingConcept. template class Quaternion { public: @@ -254,4 +254,4 @@ class Quaternion { using QuaternionF64 = Quaternion; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/manifold/quaternion_test.cpp b/cpp/sophus2/manifold/quaternion_test.cpp similarity index 77% rename from cpp/sophus/manifold/quaternion_test.cpp rename to cpp/sophus2/manifold/quaternion_test.cpp index c09f49ba..3e90004d 100644 --- a/cpp/sophus/manifold/quaternion_test.cpp +++ b/cpp/sophus2/manifold/quaternion_test.cpp @@ -6,13 +6,13 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/manifold/quaternion.h" +#include "sophus2/manifold/quaternion.h" -#include "sophus/concepts/division_ring_prop_tests.h" +#include "sophus2/concepts/division_ring_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(quaternion, unit) { Quaternion q; } @@ -20,4 +20,4 @@ TEST(quaternion, prop_test) { DivisionRingTestSuite>::runAllTests("Quaternion"); DivisionRingTestSuite>::runAllTests("Quaternion"); } -} // namespace sophus::test +} // namespace sophus2::test diff --git a/cpp/sophus/manifold/unit_vector.h b/cpp/sophus2/manifold/unit_vector.h similarity index 97% rename from cpp/sophus/manifold/unit_vector.h rename to cpp/sophus2/manifold/unit_vector.h index c6ad5114..ab53020f 100644 --- a/cpp/sophus/manifold/unit_vector.h +++ b/cpp/sophus2/manifold/unit_vector.h @@ -8,12 +8,12 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/concepts/manifold.h" +#include "sophus2/common/common.h" +#include "sophus2/concepts/manifold.h" #include -namespace sophus { +namespace sophus2 { // Forward declarations template @@ -76,7 +76,7 @@ class UnitVectorImpl { /// Returns false if the norm of the vector if not close to 1. static auto areParamsValid(Params const& unit_vector) - -> sophus::Expected { + -> sophus2::Expected { static Scalar const kThr = kEpsilon; Scalar const squared_norm = unit_vector.squaredNorm(); using std::abs; @@ -88,7 +88,7 @@ class UnitVectorImpl { squared_norm, kThr); } - return sophus::Expected{}; + return sophus2::Expected{}; } /// oplus operator @@ -155,6 +155,7 @@ class UnitVectorImpl { Tangent tail = params.template tail(); Scalar theta = tail.norm(); + using std::abs; if (abs(theta) < kEpsilon) { return atan2(Scalar(0.0), x) * kUnitX; } @@ -304,4 +305,4 @@ class UnitVector : public linalg::UnitVectorImpl { static_assert(concepts::Manifold>); -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/manifold/unit_vector_test.cpp b/cpp/sophus2/manifold/unit_vector_test.cpp similarity index 95% rename from cpp/sophus/manifold/unit_vector_test.cpp rename to cpp/sophus2/manifold/unit_vector_test.cpp index d879fabb..ad8c6502 100644 --- a/cpp/sophus/manifold/unit_vector_test.cpp +++ b/cpp/sophus2/manifold/unit_vector_test.cpp @@ -6,12 +6,12 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/manifold/unit_vector.h" +#include "sophus2/manifold/unit_vector.h" -#include "sophus/concepts/manifold_prop_tests.h" +#include "sophus2/concepts/manifold_prop_tests.h" #include -namespace sophus::test { +namespace sophus2::test { TEST(unitvec, unit2) { EXPECT_NO_FATAL_FAILURE(UnitVector2::fromUnitVector({1.0, 0.0})); @@ -46,7 +46,7 @@ TEST(unitvec, manifold_prop_tests) { ManifoldPropTestSuite>::runAllTests("UnitVector"); ManifoldPropTestSuite>::runAllTests("UnitVector"); } -} // namespace sophus::test +} // namespace sophus2::test // TEST(unitvec, testRotThroughPoints) { // std::default_random_engine generator(0); diff --git a/cpp/sophus/manifold/vector_manifold.h b/cpp/sophus2/manifold/vector_manifold.h similarity index 93% rename from cpp/sophus/manifold/vector_manifold.h rename to cpp/sophus2/manifold/vector_manifold.h index 262e2c0f..ca13bc41 100644 --- a/cpp/sophus/manifold/vector_manifold.h +++ b/cpp/sophus2/manifold/vector_manifold.h @@ -8,11 +8,11 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/concepts/manifold.h" -#include "sophus/linalg/vector_space.h" +#include "sophus2/common/common.h" +#include "sophus2/concepts/manifold.h" +#include "sophus2/linalg/vector_space.h" -namespace sophus { +namespace sophus2 { /// A n-dimensional vector is a trivial example of a manifold. /// @@ -85,4 +85,4 @@ struct VectorManifold { Params vec; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/manifold/vector_manifold_test.cpp b/cpp/sophus2/manifold/vector_manifold_test.cpp similarity index 75% rename from cpp/sophus/manifold/vector_manifold_test.cpp rename to cpp/sophus2/manifold/vector_manifold_test.cpp index 79390e78..11752586 100644 --- a/cpp/sophus/manifold/vector_manifold_test.cpp +++ b/cpp/sophus2/manifold/vector_manifold_test.cpp @@ -6,8 +6,8 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/manifold/vector_manifold.h" +#include "sophus2/manifold/vector_manifold.h" -#include "sophus/concepts/manifold_prop_tests.h" +#include "sophus2/concepts/manifold_prop_tests.h" #include diff --git a/cpp/sophus2/sensor/CMakeLists.txt b/cpp/sophus2/sensor/CMakeLists.txt new file mode 100644 index 00000000..d02785b6 --- /dev/null +++ b/cpp/sophus2/sensor/CMakeLists.txt @@ -0,0 +1,46 @@ +#[[ +sophus2_sensor + + + +]] + +set(sophus2_sensor_src_prefixes + camera_model + clipping_planes + imu_model + camera_rig + orthographic + ) + +set(sophus2_sensor_cpp) +set(sophus2_sensor_h) +foreach(x ${sophus2_sensor_src_prefixes} ) + list(APPEND sophus2_sensor_cpp ${x}.cpp) + list(APPEND sophus2_sensor_h ${x}.h) +endforeach() + +farm_ng_add_library(sophus2_sensor SHARED +NAMESPACE ${PROJECT_NAME} +INCLUDE_DIR ../../ +HEADERS +${sophus2_sensor_h} +camera_distortion/affine.h +camera_distortion/brown_conrady.h +camera_distortion/kannala_brandt.h +camera_projection/projection_z1.h +camera_projection/projection_ortho.h +SOURCES + ${sophus2_sensor_cpp}) + +target_link_libraries(sophus2_sensor sophus2_lie sophus2_image) + + +if(${BUILD_SOPHUS_TESTS}) + foreach(test_basename ${sophus2_sensor_src_prefixes}) + farm_ng_add_test(${test_basename} + PARENT_LIBRARY sophus2_sensor + LINK_LIBRARIES sophus2_sensor + LABELS small) + endforeach() +endif() diff --git a/cpp/sophus/sensor/camera_distortion/affine.h b/cpp/sophus2/sensor/camera_distortion/affine.h similarity index 98% rename from cpp/sophus/sensor/camera_distortion/affine.h rename to cpp/sophus2/sensor/camera_distortion/affine.h index 0463e049..f03fd13e 100644 --- a/cpp/sophus/sensor/camera_distortion/affine.h +++ b/cpp/sophus2/sensor/camera_distortion/affine.h @@ -10,7 +10,7 @@ #include -namespace sophus { +namespace sophus2 { class AffineTransform { public: @@ -90,4 +90,4 @@ class AffineTransform { } }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/camera_distortion/brown_conrady.h b/cpp/sophus2/sensor/camera_distortion/brown_conrady.h similarity index 96% rename from cpp/sophus/sensor/camera_distortion/brown_conrady.h rename to cpp/sophus2/sensor/camera_distortion/brown_conrady.h index 67a9817f..0ee2836f 100644 --- a/cpp/sophus/sensor/camera_distortion/brown_conrady.h +++ b/cpp/sophus2/sensor/camera_distortion/brown_conrady.h @@ -8,13 +8,13 @@ #pragma once -#include "sophus/ceres/jet_helpers.h" -#include "sophus/common/common.h" -#include "sophus/sensor/camera_distortion/affine.h" +#include "sophus2/ceres/jet_helpers.h" +#include "sophus2/common/common.h" +#include "sophus2/sensor/camera_distortion/affine.h" #include -namespace sophus { +namespace sophus2 { class BrownConradyTransform { public: static int constexpr kNumDistortionParams = 8; @@ -90,6 +90,7 @@ class BrownConradyTransform { PixelImage xy = uv_normalized; for (int i = 0; i < 50; ++i) { + using std::abs; TScalar x = xy[0]; TScalar y = xy[1]; @@ -161,7 +162,7 @@ class BrownConradyTransform { PixelImage step = j_inv * f_xy; if (abs(jet_helpers::GetValue::impl(step.squaredNorm())) < - sophus::kEpsilon * sophus::kEpsilon) { + sophus2::kEpsilon * sophus2::kEpsilon) { break; } xy -= step; @@ -202,8 +203,8 @@ class BrownConradyTransform { template static auto undistort( - Params const& params, PixelImage const& pixel_image) - -> ProjInCameraZ1Plane { + Params const& params, + PixelImage const& pixel_image) -> ProjInCameraZ1Plane { PixelImage proj_point_in_camera_z1_plane = unprojImpl( params.template tail().eval(), AffineTransform::undistort( @@ -277,4 +278,4 @@ class BrownConradyTransform { } }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/camera_distortion/kannala_brandt.h b/cpp/sophus2/sensor/camera_distortion/kannala_brandt.h similarity index 94% rename from cpp/sophus/sensor/camera_distortion/kannala_brandt.h rename to cpp/sophus2/sensor/camera_distortion/kannala_brandt.h index 5da3e183..50feb1cc 100644 --- a/cpp/sophus/sensor/camera_distortion/kannala_brandt.h +++ b/cpp/sophus2/sensor/camera_distortion/kannala_brandt.h @@ -8,14 +8,14 @@ #pragma once -#include "sophus/ceres/jet_helpers.h" -#include "sophus/common/common.h" -#include "sophus/geometry/point_transform.h" -#include "sophus/sensor/camera_distortion/affine.h" +#include "sophus2/ceres/jet_helpers.h" +#include "sophus2/common/common.h" +#include "sophus2/geometry/point_transform.h" +#include "sophus2/sensor/camera_distortion/affine.h" #include -namespace sophus { +namespace sophus2 { // https://github.com/facebookincubator/isometric_pattern_matcher/blob/main/IsometricPatternMatcher/CameraModels.h // @@ -64,7 +64,7 @@ class KannalaBrandtK3Transform { using std::atan2; using std::sqrt; - if (radius_squared > sophus::kEpsilonF64) { + if (radius_squared > sophus2::kEpsilonF64) { auto const radius = sqrt(radius_squared); auto const radius_inverse = 1.0 / radius; auto const theta = atan2(radius, typename TPointTypeT::Scalar(1.0)); @@ -109,7 +109,7 @@ class KannalaBrandtK3Transform { const TScalar vn = (pixel_image(1) - v0) / fv; const TScalar rth2 = un * un + vn * vn; - if (rth2 < sophus::kEpsilon * sophus::kEpsilon) { + if (rth2 < sophus2::kEpsilon * sophus2::kEpsilon) { return Eigen::Matrix(un, vn); } @@ -134,7 +134,7 @@ class KannalaBrandtK3Transform { th -= step; // has converged? if (abs(jet_helpers::GetValue::impl(step)) < - sophus::kEpsilon) { + sophus2::kEpsilon) { break; } } @@ -180,7 +180,7 @@ class KannalaBrandtK3Transform { Eigen::Matrix dx; - if (radius_squared < sophus::kEpsilonSqrtF64) { + if (radius_squared < sophus2::kEpsilonSqrtF64) { // clang-format off dx << // fx, 0, @@ -229,4 +229,4 @@ class KannalaBrandtK3Transform { } }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/camera_model.cpp b/cpp/sophus2/sensor/camera_model.cpp similarity index 98% rename from cpp/sophus/sensor/camera_model.cpp rename to cpp/sophus2/sensor/camera_model.cpp index 3028dc25..1aeebb22 100644 --- a/cpp/sophus/sensor/camera_model.cpp +++ b/cpp/sophus2/sensor/camera_model.cpp @@ -6,9 +6,9 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/sensor/camera_model.h" +#include "sophus2/sensor/camera_model.h" -namespace sophus { +namespace sophus2 { namespace { auto getModelFromType( @@ -238,7 +238,7 @@ auto CameraModel::distortionType() const -> CameraDistortionType { auto CameraModel::createDefaultPinholeModel(ImageSize image_size) -> CameraModel { - return CameraModel(::sophus::createDefaultPinholeModel(image_size)); + return CameraModel(::sophus2::createDefaultPinholeModel(image_size)); } auto CameraModel::scale(ImageSize image_size) const -> CameraModel { @@ -267,4 +267,4 @@ auto createDefaultOrthoModel(ImageSize image_size) -> OrthographicModel { return OrthographicModel(image_size, {sx, sy, cx, cy}); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/camera_model.h b/cpp/sophus2/sensor/camera_model.h similarity index 95% rename from cpp/sophus/sensor/camera_model.h rename to cpp/sophus2/sensor/camera_model.h index e6bcf3a8..f6875468 100644 --- a/cpp/sophus/sensor/camera_model.h +++ b/cpp/sophus2/sensor/camera_model.h @@ -8,25 +8,25 @@ #pragma once -#include "sophus/common/common.h" -#include "sophus/common/enum.h" -#include "sophus/geometry/point_transform.h" -#include "sophus/image/image.h" -#include "sophus/image/image_size.h" -#include "sophus/lie/se3.h" -#include "sophus/linalg/homogeneous.h" -#include "sophus/sensor/camera_distortion/affine.h" -#include "sophus/sensor/camera_distortion/brown_conrady.h" -#include "sophus/sensor/camera_distortion/kannala_brandt.h" -#include "sophus/sensor/camera_projection/projection_ortho.h" -#include "sophus/sensor/camera_projection/projection_z1.h" +#include "sophus2/common/common.h" +#include "sophus2/common/enum.h" +#include "sophus2/geometry/point_transform.h" +#include "sophus2/image/image.h" +#include "sophus2/image/image_size.h" +#include "sophus2/lie/se3.h" +#include "sophus2/linalg/homogeneous.h" +#include "sophus2/sensor/camera_distortion/affine.h" +#include "sophus2/sensor/camera_distortion/brown_conrady.h" +#include "sophus2/sensor/camera_distortion/kannala_brandt.h" +#include "sophus2/sensor/camera_projection/projection_ortho.h" +#include "sophus2/sensor/camera_projection/projection_z1.h" #include #include #include -namespace sophus { +namespace sophus2 { /// Subsamples pixel down, factor of 0.5. /// @@ -158,7 +158,7 @@ class CameraModelT { Params params = this->params_; params[0] = TScalar(0.5) * params[0]; // fx params[1] = TScalar(0.5) * params[1]; // fy - params.template segment<2>(2) = ::sophus::subsampleDown( + params.template segment<2>(2) = ::sophus2::subsampleDown( params.template segment<2>(2).eval()); // cx, cy return CameraModelT(half(image_size_), params); } @@ -172,7 +172,7 @@ class CameraModelT { params[0] = TScalar(2.0) * params[0]; // fx params[1] = TScalar(2.0) * params[1]; // fy params.template segment<2>(2) = - ::sophus::subsampleUp(params.template segment<2>(2).eval()); // cx, cy + ::sophus2::subsampleUp(params.template segment<2>(2).eval()); // cx, cy return CameraModelT( {image_size_.width * 2, image_size_.height * 2}, params); } @@ -189,7 +189,7 @@ class CameraModelT { params[0] = TScalar(0.5) * params[0]; // fx params[1] = TScalar(0.5) * params[1]; // fy params.template segment<2>(2) = - ::sophus::binDown(params.template segment<2>(2).eval()); // cx, cy + ::sophus2::binDown(params.template segment<2>(2).eval()); // cx, cy return CameraModelT(half(image_size_), params); } @@ -202,7 +202,7 @@ class CameraModelT { params[0] = TScalar(2.0) * params[0]; // fx params[1] = TScalar(2.0) * params[1]; // fy params.template segment<2>(2) = - ::sophus::binUp(params.template segment<2>(2).eval()); // cx, cy + ::sophus2::binUp(params.template segment<2>(2).eval()); // cx, cy return CameraModelT( {image_size_.width * 2, image_size_.height * 2}, params); } @@ -510,4 +510,4 @@ auto createDefaultPinholeModel(ImageSize image_size) -> PinholeModel; /// Creates default orthographic model from `image_size`. auto createDefaultOrthoModel(ImageSize image_size) -> OrthographicModel; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/camera_model_test.cpp b/cpp/sophus2/sensor/camera_model_test.cpp similarity index 96% rename from cpp/sophus/sensor/camera_model_test.cpp rename to cpp/sophus2/sensor/camera_model_test.cpp index 1872b50a..5c63c683 100644 --- a/cpp/sophus/sensor/camera_model_test.cpp +++ b/cpp/sophus2/sensor/camera_model_test.cpp @@ -6,16 +6,16 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/sensor/camera_model.h" +#include "sophus2/sensor/camera_model.h" -#include "sophus/calculus/num_diff.h" -#include "sophus/image/interpolation.h" -#include "sophus/lie/se3.h" -#include "sophus/sensor/orthographic.h" +#include "sophus2/calculus/num_diff.h" +#include "sophus2/image/interpolation.h" +#include "sophus2/lie/se3.h" +#include "sophus2/sensor/orthographic.h" #include -using namespace sophus; +using namespace sophus2; double constexpr kEps = 1e-5; @@ -91,10 +91,10 @@ TEST(camera_model, projection_round_trip) { Eigen::Vector zero; zero.setZero(); Eigen::Matrix numeric_dx = - sophus::vectorFieldNumDiff( + sophus2::vectorFieldNumDiff( [&](Eigen::Vector const& x) -> Eigen::Vector2d { return camera_model.camProj( - sophus::SE3d::exp(x) * point_in_camera); + sophus2::SE3d::exp(x) * point_in_camera); }, zero); @@ -125,7 +125,7 @@ TEST(camera_model, projection_round_trip) { Eigen::Matrix2d dx = camera_model.dxDistort(ab_in_z1plane); - Eigen::Matrix2d dx_num = sophus::vectorFieldNumDiff( + Eigen::Matrix2d dx_num = sophus2::vectorFieldNumDiff( [&](Eigen::Vector2d const& x) -> Eigen::Vector2d { return camera_model.distort(x); }, @@ -161,7 +161,7 @@ TEST(camera_model, projection_round_trip) { for (CameraModel const& camera_model : camera_models) { for (Eigen::Vector const& t : tangent_vec) { - sophus::SE3d foo_from_bar = sophus::SE3d::exp(t); + sophus2::SE3d foo_from_bar = sophus2::SE3d::exp(t); PointTransformer trans(foo_from_bar); std::vector pixels_image = { @@ -185,7 +185,7 @@ TEST(camera_model, projection_round_trip) { vectorFieldNumDiff( [&](Eigen::Vector const& vec_a) { return camera_model.distort(proj( - sophus::Isometry3F64::exp(vec_a) * foo_from_bar * + sophus2::Isometry3F64::exp(vec_a) * foo_from_bar * point_in_bar_camera)); }, zero); diff --git a/cpp/sophus/sensor/camera_projection/projection_ortho.h b/cpp/sophus2/sensor/camera_projection/projection_ortho.h similarity index 97% rename from cpp/sophus/sensor/camera_projection/projection_ortho.h rename to cpp/sophus2/sensor/camera_projection/projection_ortho.h index 00b784dd..0ab05d51 100644 --- a/cpp/sophus/sensor/camera_projection/projection_ortho.h +++ b/cpp/sophus2/sensor/camera_projection/projection_ortho.h @@ -10,7 +10,7 @@ #include -namespace sophus { +namespace sophus2 { struct ProjectionOrtho { template @@ -54,4 +54,4 @@ struct ProjectionOrtho { } }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/camera_projection/projection_z1.h b/cpp/sophus2/sensor/camera_projection/projection_z1.h similarity index 99% rename from cpp/sophus/sensor/camera_projection/projection_z1.h rename to cpp/sophus2/sensor/camera_projection/projection_z1.h index ab4d1b3b..cd2acfd1 100644 --- a/cpp/sophus/sensor/camera_projection/projection_z1.h +++ b/cpp/sophus2/sensor/camera_projection/projection_z1.h @@ -10,7 +10,7 @@ #include -namespace sophus { +namespace sophus2 { struct ProjectionZ1 { template diff --git a/cpp/sophus/sensor/camera_rig.cpp b/cpp/sophus2/sensor/camera_rig.cpp similarity index 76% rename from cpp/sophus/sensor/camera_rig.cpp rename to cpp/sophus2/sensor/camera_rig.cpp index f1a67c38..d906efba 100644 --- a/cpp/sophus/sensor/camera_rig.cpp +++ b/cpp/sophus2/sensor/camera_rig.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/sensor/camera_rig.h" +#include "sophus2/sensor/camera_rig.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/sensor/camera_rig.h b/cpp/sophus2/sensor/camera_rig.h similarity index 77% rename from cpp/sophus/sensor/camera_rig.h rename to cpp/sophus2/sensor/camera_rig.h index 7f2f4c55..b261b18b 100644 --- a/cpp/sophus/sensor/camera_rig.h +++ b/cpp/sophus2/sensor/camera_rig.h @@ -8,10 +8,10 @@ #pragma once -#include "sophus/lie/se3.h" -#include "sophus/sensor/camera_model.h" +#include "sophus2/lie/se3.h" +#include "sophus2/sensor/camera_model.h" -namespace sophus { +namespace sophus2 { /// Camera attached to a rigid body. We call the frame of the rigid body "rig". struct RigidCamera { @@ -19,9 +19,9 @@ struct RigidCamera { CameraModel intrinsics; /// Camera extrinsics - sophus::SE3d rig_from_camera; + sophus2::SE3d rig_from_camera; }; using MultiCameraRig = std::vector; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/camera_rig_test.cpp b/cpp/sophus2/sensor/camera_rig_test.cpp similarity index 84% rename from cpp/sophus/sensor/camera_rig_test.cpp rename to cpp/sophus2/sensor/camera_rig_test.cpp index 4f95be90..bd6a635f 100644 --- a/cpp/sophus/sensor/camera_rig_test.cpp +++ b/cpp/sophus2/sensor/camera_rig_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/sensor/camera_rig.h" +#include "sophus2/sensor/camera_rig.h" #include -using namespace sophus; +using namespace sophus2; TEST(camera_rig, smoke) { // TODO: Implement test (Kyle) diff --git a/cpp/sophus/sensor/clipping_planes.cpp b/cpp/sophus2/sensor/clipping_planes.cpp similarity index 75% rename from cpp/sophus/sensor/clipping_planes.cpp rename to cpp/sophus2/sensor/clipping_planes.cpp index 2048e383..f8003637 100644 --- a/cpp/sophus/sensor/clipping_planes.cpp +++ b/cpp/sophus2/sensor/clipping_planes.cpp @@ -6,6 +6,6 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/sensor/clipping_planes.h" +#include "sophus2/sensor/clipping_planes.h" -namespace sophus {} // namespace sophus +namespace sophus2 {} // namespace sophus2 diff --git a/cpp/sophus/sensor/clipping_planes.h b/cpp/sophus2/sensor/clipping_planes.h similarity index 88% rename from cpp/sophus/sensor/clipping_planes.h rename to cpp/sophus2/sensor/clipping_planes.h index 7a818544..28fdb464 100644 --- a/cpp/sophus/sensor/clipping_planes.h +++ b/cpp/sophus2/sensor/clipping_planes.h @@ -8,10 +8,10 @@ #pragma once -namespace sophus { +namespace sophus2 { struct ClippingPlanes { float near = 0.001; float far = 1000; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/clipping_planes_test.cpp b/cpp/sophus2/sensor/clipping_planes_test.cpp similarity index 81% rename from cpp/sophus/sensor/clipping_planes_test.cpp rename to cpp/sophus2/sensor/clipping_planes_test.cpp index c6911db7..f8255119 100644 --- a/cpp/sophus/sensor/clipping_planes_test.cpp +++ b/cpp/sophus2/sensor/clipping_planes_test.cpp @@ -6,8 +6,8 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/sensor/clipping_planes.h" +#include "sophus2/sensor/clipping_planes.h" #include -using namespace sophus; +using namespace sophus2; diff --git a/cpp/sophus/sensor/imu_model.cpp b/cpp/sophus2/sensor/imu_model.cpp similarity index 94% rename from cpp/sophus/sensor/imu_model.cpp rename to cpp/sophus2/sensor/imu_model.cpp index 67abc0de..cfe050b8 100644 --- a/cpp/sophus/sensor/imu_model.cpp +++ b/cpp/sophus2/sensor/imu_model.cpp @@ -6,9 +6,9 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/sensor/imu_model.h" +#include "sophus2/sensor/imu_model.h" -namespace sophus { +namespace sophus2 { auto getModelFromType(GyroModelType model_type, Eigen::VectorXd const& params) -> GyroModelVariant { @@ -60,7 +60,7 @@ auto ImuModel::gyroModelType() const -> GyroModelType { ScalingNonOrthogonalityGyroModel>) { return GyroModelType::scaling_non_orthogonality; } else { - static_assert(::sophus::AlwaysFalse, "non-exhaustive visitor!"); + static_assert(::sophus2::AlwaysFalse, "non-exhaustive visitor!"); } }, this->gyroModel()); @@ -90,4 +90,4 @@ auto ImuModel::acceleroModelType() const -> AcceleroModelType { this->acceleroModel()); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/imu_model.h b/cpp/sophus2/sensor/imu_model.h similarity index 98% rename from cpp/sophus/sensor/imu_model.h rename to cpp/sophus2/sensor/imu_model.h index 2b272e42..5eb5cb11 100644 --- a/cpp/sophus/sensor/imu_model.h +++ b/cpp/sophus2/sensor/imu_model.h @@ -8,14 +8,14 @@ #pragma once -#include "sophus/common/enum.h" -#include "sophus/lie/se3.h" +#include "sophus2/common/enum.h" +#include "sophus2/lie/se3.h" #include #include -namespace sophus { +namespace sophus2 { template auto nonOrthogonalityMatrix(Eigen::Matrix const& non_orthogonality) @@ -176,4 +176,4 @@ class ImuModel { AcceleroModelVariant accelero_model_; }; -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/imu_model_test.cpp b/cpp/sophus2/sensor/imu_model_test.cpp similarity index 96% rename from cpp/sophus/sensor/imu_model_test.cpp rename to cpp/sophus2/sensor/imu_model_test.cpp index cad4490c..fd6945af 100644 --- a/cpp/sophus/sensor/imu_model_test.cpp +++ b/cpp/sophus2/sensor/imu_model_test.cpp @@ -6,11 +6,11 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/sensor/imu_model.h" +#include "sophus2/sensor/imu_model.h" #include -using namespace sophus; +using namespace sophus2; TEST(imu_model, smoke) { Eigen::Matrix imu_vel{1.1, 2.0, -3.5}; diff --git a/cpp/sophus/sensor/orthographic.cpp b/cpp/sophus2/sensor/orthographic.cpp similarity index 100% rename from cpp/sophus/sensor/orthographic.cpp rename to cpp/sophus2/sensor/orthographic.cpp diff --git a/cpp/sophus/sensor/orthographic.h b/cpp/sophus2/sensor/orthographic.h similarity index 92% rename from cpp/sophus/sensor/orthographic.h rename to cpp/sophus2/sensor/orthographic.h index 4bf0a11d..019a9faf 100644 --- a/cpp/sophus/sensor/orthographic.h +++ b/cpp/sophus2/sensor/orthographic.h @@ -8,11 +8,11 @@ #pragma once -#include "sophus/calculus/region.h" -#include "sophus/image/image_size.h" -#include "sophus/sensor/camera_model.h" +#include "sophus2/calculus/region.h" +#include "sophus2/image/image_size.h" +#include "sophus2/sensor/camera_model.h" -namespace sophus { +namespace sophus2 { template using OrthographicModelT = @@ -63,4 +63,4 @@ auto boundingBoxFromOrthoCam(OrthographicModelT const& ortho_cam) ortho_cam.focalLength().array()); } -} // namespace sophus +} // namespace sophus2 diff --git a/cpp/sophus/sensor/orthographic_test.cpp b/cpp/sophus2/sensor/orthographic_test.cpp similarity index 81% rename from cpp/sophus/sensor/orthographic_test.cpp rename to cpp/sophus2/sensor/orthographic_test.cpp index 5aece78b..ec81599d 100644 --- a/cpp/sophus/sensor/orthographic_test.cpp +++ b/cpp/sophus2/sensor/orthographic_test.cpp @@ -6,8 +6,8 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/sensor/orthographic.h" +#include "sophus2/sensor/orthographic.h" #include -// using namespace sophus; +// using namespace sophus2; diff --git a/cpp/sophus/sympy/.gitignore b/cpp/sophus2/sympy/.gitignore similarity index 100% rename from cpp/sophus/sympy/.gitignore rename to cpp/sophus2/sympy/.gitignore diff --git a/cpp/sophus/sympy/cpp_gencode/Se2_Dx_exp_x.cpp b/cpp/sophus2/sympy/cpp_gencode/Se2_Dx_exp_x.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/Se2_Dx_exp_x.cpp rename to cpp/sophus2/sympy/cpp_gencode/Se2_Dx_exp_x.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/Se2_Dx_log_this.cpp b/cpp/sophus2/sympy/cpp_gencode/Se2_Dx_log_this.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/Se2_Dx_log_this.cpp rename to cpp/sophus2/sympy/cpp_gencode/Se2_Dx_log_this.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp b/cpp/sophus2/sympy/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp rename to cpp/sophus2/sympy/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/Se3_Dx_exp_x.cpp b/cpp/sophus2/sympy/cpp_gencode/Se3_Dx_exp_x.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/Se3_Dx_exp_x.cpp rename to cpp/sophus2/sympy/cpp_gencode/Se3_Dx_exp_x.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/Se3_Dx_log_this.cpp b/cpp/sophus2/sympy/cpp_gencode/Se3_Dx_log_this.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/Se3_Dx_log_this.cpp rename to cpp/sophus2/sympy/cpp_gencode/Se3_Dx_log_this.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp b/cpp/sophus2/sympy/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp rename to cpp/sophus2/sympy/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/So2_Dx_exp_x.cpp b/cpp/sophus2/sympy/cpp_gencode/So2_Dx_exp_x.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/So2_Dx_exp_x.cpp rename to cpp/sophus2/sympy/cpp_gencode/So2_Dx_exp_x.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/So2_Dx_log_exp_x_times_this_at_0.cpp b/cpp/sophus2/sympy/cpp_gencode/So2_Dx_log_exp_x_times_this_at_0.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/So2_Dx_log_exp_x_times_this_at_0.cpp rename to cpp/sophus2/sympy/cpp_gencode/So2_Dx_log_exp_x_times_this_at_0.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/So2_Dx_log_this.cpp b/cpp/sophus2/sympy/cpp_gencode/So2_Dx_log_this.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/So2_Dx_log_this.cpp rename to cpp/sophus2/sympy/cpp_gencode/So2_Dx_log_this.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp b/cpp/sophus2/sympy/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp rename to cpp/sophus2/sympy/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/So3_Dx_exp_x.cpp b/cpp/sophus2/sympy/cpp_gencode/So3_Dx_exp_x.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/So3_Dx_exp_x.cpp rename to cpp/sophus2/sympy/cpp_gencode/So3_Dx_exp_x.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/So3_Dx_log_exp_x_times_this_at_0.cpp b/cpp/sophus2/sympy/cpp_gencode/So3_Dx_log_exp_x_times_this_at_0.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/So3_Dx_log_exp_x_times_this_at_0.cpp rename to cpp/sophus2/sympy/cpp_gencode/So3_Dx_log_exp_x_times_this_at_0.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/So3_Dx_log_this.cpp b/cpp/sophus2/sympy/cpp_gencode/So3_Dx_log_this.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/So3_Dx_log_this.cpp rename to cpp/sophus2/sympy/cpp_gencode/So3_Dx_log_this.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp b/cpp/sophus2/sympy/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp rename to cpp/sophus2/sympy/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/affine/dx_pixel_from_z1_plane_x.cpp b/cpp/sophus2/sympy/cpp_gencode/affine/dx_pixel_from_z1_plane_x.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/affine/dx_pixel_from_z1_plane_x.cpp rename to cpp/sophus2/sympy/cpp_gencode/affine/dx_pixel_from_z1_plane_x.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/brown_conrady/dx_normalized_from_z1_plane_x.cpp b/cpp/sophus2/sympy/cpp_gencode/brown_conrady/dx_normalized_from_z1_plane_x.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/brown_conrady/dx_normalized_from_z1_plane_x.cpp rename to cpp/sophus2/sympy/cpp_gencode/brown_conrady/dx_normalized_from_z1_plane_x.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/brown_conrady/dx_pixel_from_z1_plane_x.cpp b/cpp/sophus2/sympy/cpp_gencode/brown_conrady/dx_pixel_from_z1_plane_x.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/brown_conrady/dx_pixel_from_z1_plane_x.cpp rename to cpp/sophus2/sympy/cpp_gencode/brown_conrady/dx_pixel_from_z1_plane_x.cpp diff --git a/cpp/sophus/sympy/cpp_gencode/kannala_brandt/dx_pixel_from_z1_plane_x.cpp b/cpp/sophus2/sympy/cpp_gencode/kannala_brandt/dx_pixel_from_z1_plane_x.cpp similarity index 100% rename from cpp/sophus/sympy/cpp_gencode/kannala_brandt/dx_pixel_from_z1_plane_x.cpp rename to cpp/sophus2/sympy/cpp_gencode/kannala_brandt/dx_pixel_from_z1_plane_x.cpp diff --git a/cpp/sophus/sympy/run_tests.sh b/cpp/sophus2/sympy/run_tests.sh similarity index 100% rename from cpp/sophus/sympy/run_tests.sh rename to cpp/sophus2/sympy/run_tests.sh diff --git a/cpp/sophus/sympy/sophus/__init__.py b/cpp/sophus2/sympy/sophus/__init__.py similarity index 100% rename from cpp/sophus/sympy/sophus/__init__.py rename to cpp/sophus2/sympy/sophus/__init__.py diff --git a/cpp/sophus/sympy/sophus/affine_camera.py b/cpp/sophus2/sympy/sophus/affine_camera.py similarity index 100% rename from cpp/sophus/sympy/sophus/affine_camera.py rename to cpp/sophus2/sympy/sophus/affine_camera.py diff --git a/cpp/sophus/sympy/sophus/brown_conrady_camera.py b/cpp/sophus2/sympy/sophus/brown_conrady_camera.py similarity index 100% rename from cpp/sophus/sympy/sophus/brown_conrady_camera.py rename to cpp/sophus2/sympy/sophus/brown_conrady_camera.py diff --git a/cpp/sophus/sympy/sophus/complex.py b/cpp/sophus2/sympy/sophus/complex.py similarity index 100% rename from cpp/sophus/sympy/sophus/complex.py rename to cpp/sophus2/sympy/sophus/complex.py diff --git a/cpp/sophus/sympy/sophus/cse_codegen.py b/cpp/sophus2/sympy/sophus/cse_codegen.py similarity index 100% rename from cpp/sophus/sympy/sophus/cse_codegen.py rename to cpp/sophus2/sympy/sophus/cse_codegen.py diff --git a/cpp/sophus/sympy/sophus/dual_quaternion.py b/cpp/sophus2/sympy/sophus/dual_quaternion.py similarity index 100% rename from cpp/sophus/sympy/sophus/dual_quaternion.py rename to cpp/sophus2/sympy/sophus/dual_quaternion.py diff --git a/cpp/sophus/sympy/sophus/inverse_depth.py b/cpp/sophus2/sympy/sophus/inverse_depth.py similarity index 100% rename from cpp/sophus/sympy/sophus/inverse_depth.py rename to cpp/sophus2/sympy/sophus/inverse_depth.py diff --git a/cpp/sophus/sympy/sophus/kannala_brandt_camera.py b/cpp/sophus2/sympy/sophus/kannala_brandt_camera.py similarity index 100% rename from cpp/sophus/sympy/sophus/kannala_brandt_camera.py rename to cpp/sophus2/sympy/sophus/kannala_brandt_camera.py diff --git a/cpp/sophus/sympy/sophus/matrix.py b/cpp/sophus2/sympy/sophus/matrix.py similarity index 100% rename from cpp/sophus/sympy/sophus/matrix.py rename to cpp/sophus2/sympy/sophus/matrix.py diff --git a/cpp/sophus/sympy/sophus/quaternion.py b/cpp/sophus2/sympy/sophus/quaternion.py similarity index 100% rename from cpp/sophus/sympy/sophus/quaternion.py rename to cpp/sophus2/sympy/sophus/quaternion.py diff --git a/cpp/sophus/sympy/sophus/se2.py b/cpp/sophus2/sympy/sophus/se2.py similarity index 100% rename from cpp/sophus/sympy/sophus/se2.py rename to cpp/sophus2/sympy/sophus/se2.py diff --git a/cpp/sophus/sympy/sophus/se3.py b/cpp/sophus2/sympy/sophus/se3.py similarity index 100% rename from cpp/sophus/sympy/sophus/se3.py rename to cpp/sophus2/sympy/sophus/se3.py diff --git a/cpp/sophus/sympy/sophus/so2.py b/cpp/sophus2/sympy/sophus/so2.py similarity index 100% rename from cpp/sophus/sympy/sophus/so2.py rename to cpp/sophus2/sympy/sophus/so2.py diff --git a/cpp/sophus/sympy/sophus/so3.py b/cpp/sophus2/sympy/sophus/so3.py similarity index 100% rename from cpp/sophus/sympy/sophus/so3.py rename to cpp/sophus2/sympy/sophus/so3.py diff --git a/docs/api_reference/doxyfile b/docs/api_reference/doxyfile index f5f17f94..8f41ea98 100644 --- a/docs/api_reference/doxyfile +++ b/docs/api_reference/doxyfile @@ -24,8 +24,8 @@ MARKDOWN_SUPPORT = YES INLINE_INHERITED_MEMB = NO EXCLUDE_SYMBOLS = ceres \ Eigen \ - sophus::details \ - sophus::interp_details \ + sophus2::details \ + sophus2::interp_details \ farm_ng::details \ farm_ng::proto_conv_details GENERATE_LATEX = NO diff --git a/docs/api_reference/doxyfile_cpp_farm_ng_warn_as_error b/docs/api_reference/doxyfile_cpp_farm_ng_warn_as_error index b320a44e..0ed3a15c 100644 --- a/docs/api_reference/doxyfile_cpp_farm_ng_warn_as_error +++ b/docs/api_reference/doxyfile_cpp_farm_ng_warn_as_error @@ -24,8 +24,8 @@ MARKDOWN_SUPPORT = YES INLINE_INHERITED_MEMB = NO EXCLUDE_SYMBOLS = ceres \ Eigen \ - sophus::details \ - sophus::interp_details \ + sophus2::details \ + sophus2::interp_details \ farm_ng::details \ farm_ng::proto_conv_details GENERATE_LATEX = NO diff --git a/examples/cpp-vcpkg/.gitignore b/examples/cpp-vcpkg/.gitignore new file mode 100644 index 00000000..7ab71862 --- /dev/null +++ b/examples/cpp-vcpkg/.gitignore @@ -0,0 +1,2 @@ +overlay-ports +out diff --git a/examples/cpp-vcpkg/CMakeLists.txt b/examples/cpp-vcpkg/CMakeLists.txt new file mode 100644 index 00000000..dd54e9ed --- /dev/null +++ b/examples/cpp-vcpkg/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.24) + +project(farm_ng_core_downstream VERSION 0.2.0) + +find_package(farm_ng_core REQUIRED) + +add_executable(HelloWorld helloworld.cpp) + +target_link_libraries(HelloWorld PRIVATE farm_ng_core::farm_ng_core_logging) diff --git a/examples/cpp-vcpkg/CMakePresets.json b/examples/cpp-vcpkg/CMakePresets.json new file mode 100644 index 00000000..d3c5f373 --- /dev/null +++ b/examples/cpp-vcpkg/CMakePresets.json @@ -0,0 +1,135 @@ +{ + "version": 3, + "cmakeMinimumRequired": { + "major": 3, + "minor": 16, + "patch": 0 + }, + "configurePresets": [ + { + "name": "common", + "hidden": true, + "description": "Common farm-ng configuration", + "generator": "Ninja", + "binaryDir": "${sourceDir}/out/build/${presetName}", + "installDir": "${sourceDir}/out/install/${presetName}", + "cacheVariables": { + "CMAKE_TOOLCHAIN_FILE": "${sourceDir}/vcpkg/scripts/buildsystems/vcpkg.cmake", + "VCPKG_TARGET_TRIPLET": "x64-linux-dynamic", + "CMAKE_CXX_FLAGS": "-Werror -Wall -Wextra -Wpedantic", + "CMAKE_CXX_EXTENSIONS": "OFF", + "CMAKE_CXX_STANDARD": "17", + "CMAKE_CXX_STANDARD_REQUIRED": "ON", + "CMAKE_CXX_FLAGS_RELWITHDEBINFO": "-O2 -g", + "CMAKE_FIND_USE_PACKAGE_REGISTRY": "OFF" + }, + "condition": { + "type": "inList", + "string": "${hostSystemName}", + "list": ["Linux", "Darwin"] + }, + "warnings": { + "dev": false, + "deprecated": true, + "uninitialized": true, + "unusedCli": true, + "systemVars": false + }, + "errors": { + "dev": false, + "deprecated": true + } + }, + { + "name": "gcc-debug", + "displayName": "GCC Debug", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "gcc", + "CMAKE_CXX_COMPILER": "g++", + "CMAKE_BUILD_TYPE": "Debug" + } + }, + { + "name": "gcc-relwithdebinfo", + "displayName": "GCC RelWithDebInfo", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "gcc", + "CMAKE_CXX_COMPILER": "g++", + "CMAKE_BUILD_TYPE": "RelWithDebInfo" + } + }, + { + "name": "clang-debug", + "displayName": "Clang Debug", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "clang", + "CMAKE_CXX_COMPILER": "clang++", + "CMAKE_BUILD_TYPE": "Debug" + } + }, + { + "name": "clang-relwithdebinfo", + "displayName": "Clang RelWithDebInfo", + "inherits": "common", + "cacheVariables": { + "CMAKE_C_COMPILER": "clang", + "CMAKE_CXX_COMPILER": "clang++", + "CMAKE_BUILD_TYPE": "RelWithDebInfo" + } + } + ], + "buildPresets": [ + { + "name": "gcc-debug", + "configurePreset": "gcc-debug" + }, + { + "name": "gcc-relwithdebinfo", + "configurePreset": "gcc-relwithdebinfo" + }, + { + "name": "clang-debug", + "configurePreset": "clang-debug" + }, + { + "name": "clang-relwithdebinfo", + "configurePreset": "clang-relwithdebinfo" + } + ], + "testPresets": [ + { + "name": "test-common", + "description": "Test CMake settings that apply to all configurations", + "hidden": true, + "output": { + "outputOnFailure": true + }, + "execution": { + "noTestsAction": "error" + } + }, + { + "name": "gcc-debug", + "inherits": "test-common", + "configurePreset": "gcc-debug" + }, + { + "name": "gcc-relwithdebinfo", + "inherits": "test-common", + "configurePreset": "gcc-relwithdebinfo" + }, + { + "name": "clang-debug", + "inherits": "test-common", + "configurePreset": "clang-debug" + }, + { + "name": "clang-relwithdebinfo", + "inherits": "test-common", + "configurePreset": "clang-relwithdebinfo" + } + ] +} diff --git a/examples/cpp-vcpkg/README.md b/examples/cpp-vcpkg/README.md new file mode 100644 index 00000000..ad086f02 --- /dev/null +++ b/examples/cpp-vcpkg/README.md @@ -0,0 +1,3 @@ +An example of a downstream project that consumes farm-ng-core with the [vcpkg](https://vcpkg.io/en/) package manager. + +See `build.sh` for build instructions. diff --git a/examples/cpp-vcpkg/build.sh b/examples/cpp-vcpkg/build.sh new file mode 100755 index 00000000..1c6714b8 --- /dev/null +++ b/examples/cpp-vcpkg/build.sh @@ -0,0 +1,28 @@ +#!/bin/bash +set -e + +# Execute commands from this script's directory +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" +cd $DIR + +# Bootstrap vcpkg +git submodule update --init --recursive +./vcpkg/bootstrap-vcpkg.sh -disableMetrics + +# Use latest farm-ng-core vcpkg ports as a local overlay +# https://learn.microsoft.com/en-us/vcpkg/concepts/overlay-ports +rsync -a --delete $DIR/../../cmake/vcpkg/ports/ ./overlay-ports/ + +# In the overlay, use the current git sha as the farm-ng-core version +portfile=$DIR/overlay-ports/farm-ng-core/portfile.cmake +git_sha=`git rev-parse HEAD` +git_sha512=`curl -sL https://github.com/farm-ng/farm-ng-core/archive/$git_sha.tar.gz | shasum -a 512 | head -c 128` +sed -i "s/ REF.*$/ REF $git_sha # Auto-generated/" $portfile +sed -i "s/ SHA512.*$/ SHA512 $git_sha512 # Auto-generated/" $portfile + +# Build +cmake --preset clang-debug +cmake --build --preset clang-debug + +# Run +./out/build/clang-debug/HelloWorld diff --git a/examples/cpp-vcpkg/helloworld.cpp b/examples/cpp-vcpkg/helloworld.cpp new file mode 100644 index 00000000..979fd36a --- /dev/null +++ b/examples/cpp-vcpkg/helloworld.cpp @@ -0,0 +1,20 @@ +// Copyright 2022, farm-ng inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +int main() { + FARM_INFO("Hello, world"); + return 0; +} diff --git a/examples/cpp-vcpkg/vcpkg b/examples/cpp-vcpkg/vcpkg new file mode 160000 index 00000000..83972272 --- /dev/null +++ b/examples/cpp-vcpkg/vcpkg @@ -0,0 +1 @@ +Subproject commit 83972272512ce4ede5fc3b2ba98f6468b179f192 diff --git a/examples/cpp-vcpkg/vcpkg-configuration.json b/examples/cpp-vcpkg/vcpkg-configuration.json new file mode 100644 index 00000000..f7a7c244 --- /dev/null +++ b/examples/cpp-vcpkg/vcpkg-configuration.json @@ -0,0 +1,15 @@ +{ + "default-registry": { + "kind": "git", + "baseline": "6c937c32233bdf295ab2140dbce97fd00084a5f3", + "repository": "https://github.com/microsoft/vcpkg" + }, + "registries": [ + { + "kind": "artifact", + "location": "https://github.com/microsoft/vcpkg-ce-catalog/archive/refs/heads/main.zip", + "name": "microsoft" + } + ], + "overlay-ports": ["./overlay-ports"] +} diff --git a/examples/cpp-vcpkg/vcpkg.json b/examples/cpp-vcpkg/vcpkg.json new file mode 100644 index 00000000..e5596b18 --- /dev/null +++ b/examples/cpp-vcpkg/vcpkg.json @@ -0,0 +1,3 @@ +{ + "dependencies": ["farm-ng-core"] +} diff --git a/protos/farm_ng/core/examples.proto b/protos/farm_ng/core/examples.proto index 75fd1c96..8f2714d5 100644 --- a/protos/farm_ng/core/examples.proto +++ b/protos/farm_ng/core/examples.proto @@ -25,3 +25,7 @@ message StructExample2 { string name = 1; StructExample1 ex1 = 2; } + +message EnumExample1 { + string variant_name = 1; +} diff --git a/protos/farm_ng/core/lie.proto b/protos/farm_ng/core/lie.proto index 2121a0a0..ee0e4cb2 100644 --- a/protos/farm_ng/core/lie.proto +++ b/protos/farm_ng/core/lie.proto @@ -15,6 +15,7 @@ syntax = "proto3"; import "farm_ng/core/linalg.proto"; +import "farm_ng/core/std.proto"; package farm_ng.core.proto; @@ -32,6 +33,15 @@ message Isometry2F64 { Vec2F64 translation = 2; } +message OptionalG0Isometry3F64 { + Isometry3F64 value = 1; + bool has_message = 2; +} + +message RepeatedG0Isometry3F64 { + repeated Isometry3F64 value = 1; +} + message Rotation3F64 { // Quaternion which is required to be unit length. QuaternionF64 unit_quaternion = 1; diff --git a/protos/farm_ng/core/sensor.proto b/protos/farm_ng/core/sensor.proto index 0480d601..82541d80 100644 --- a/protos/farm_ng/core/sensor.proto +++ b/protos/farm_ng/core/sensor.proto @@ -68,6 +68,10 @@ message RigidCamera { Isometry3F64 rig_from_camera = 2; } +message MultiCameraRig { + repeated RigidCamera cameras = 1; +} + message GyroModel { string model_type = 1; repeated double params = 2; diff --git a/protos/farm_ng/core/std.proto b/protos/farm_ng/core/std.proto index 169adc11..1dddff31 100644 --- a/protos/farm_ng/core/std.proto +++ b/protos/farm_ng/core/std.proto @@ -22,6 +22,23 @@ package farm_ng.core.proto; // MesageNameG0ParameterName - e.g. OptionalG0Double which is // std::optional in c++ +// Field naming pattern for primitives: +// +// ``` +// PRIMITIVE value = 1; +// bool has_value = 2; +// ```` + +// Field naming pattern for messages: +// +// ``` +// MESSAGE value = 1; +// bool has_message = 2; +// ``` +// +// ("has_value" is already auto-generated for "value" in cpp +// proto, so we can't use that field name.) + message OptionalG0Float { float value = 1; bool has_value = 2; @@ -52,5 +69,13 @@ message OptionalG0String { bool has_value = 2; } +message RepeatedG0Float { + repeated float value = 1; +} + +message RepeatedG0UInt32{ + repeated uint32 value = 1; +} + message FileSystemPath { string path_string = 1; } diff --git a/py/farm_ng/core/events_file_reader.py b/py/farm_ng/core/events_file_reader.py index a2a659ed..275b0f7c 100644 --- a/py/farm_ng/core/events_file_reader.py +++ b/py/farm_ng/core/events_file_reader.py @@ -129,6 +129,10 @@ def read_message(self) -> Message: """Read the message from the event.""" return self.reader.read_message(self) + def read_payload(self) -> bytes: + """Read the message payload from the event.""" + return self.reader.read_payload(self) + class EventsFileReader: """EventsFileReader reads events from a file.""" @@ -291,6 +295,15 @@ def get_index(self) -> list[EventLogPosition]: def read_message(self, event_log: EventLogPosition) -> Message: """Read the message from the event. + Args: + event_log (EventLogPosition): the event log + """ + payload = self.read_payload(event_log) + return payload_to_protobuf(event_log.event, payload) + + def read_payload(self, event_log: EventLogPosition) -> bytes: + """Read the payload from the event. + Args: event_log (EventLogPosition): the event log """ @@ -299,7 +312,7 @@ def read_message(self, event_log: EventLogPosition) -> Message: payload: bytes = file_stream.read(event_log.event.payload_length) if len(payload) != event_log.event.payload_length: raise EOFError - return payload_to_protobuf(event_log.event, payload) + return payload def read(self) -> tuple[Event, Message]: """Read the next event and message and return them. diff --git a/py/pybind/lie_pybind.cpp b/py/pybind/lie_pybind.cpp index 03169eb8..c41fa982 100644 --- a/py/pybind/lie_pybind.cpp +++ b/py/pybind/lie_pybind.cpp @@ -19,15 +19,15 @@ #include // Required for the operator overloads #include #include -#include -#include -#include +#include +#include +#include #include namespace py = pybind11; using namespace pybind11::literals; // to bring in the `_a` literal -using sophus::Pose3F64; +using sophus2::Pose3F64; // to guarantee that the array is contiguous, we need to use the buffer protocol using py_array = py::array_t; @@ -73,7 +73,7 @@ void bind_lie(py::module_& m) { py::module_::import("farm_ng.core.lie_pb2").attr("Isometry3F64Tangent"); py::object PbPose = py::module_::import("farm_ng.core.pose_pb2").attr("Pose"); auto rotation3F64ToProto = [PbQuaternionF64, PbRotation3F64, PbVec3F64]( - sophus::Rotation3F64 const& self) { + sophus2::Rotation3F64 const& self) { auto quat = self.unitQuaternion(); return PbRotation3F64( "unit_quaternion"_a = PbQuaternionF64( @@ -88,7 +88,7 @@ void bind_lie(py::module_& m) { py::object pb_quat = proto.attr("unit_quaternion"); py::object imag = pb_quat.attr("imag"); - sophus::QuaternionF64 quat; + sophus2::QuaternionF64 quat; quat.real() = py::cast(pb_quat.attr("real")); quat.imag() = Eigen::Vector3d( py::cast(imag.attr("x")), @@ -101,11 +101,11 @@ void bind_lie(py::module_& m) { quat.squaredNorm(), quat.params().transpose())); } - return sophus::Rotation3F64::fromUnitQuaternion(quat); + return sophus2::Rotation3F64::fromUnitQuaternion(quat); }; auto isometry3F64ToProto = [rotation3F64ToProto, PbVec3F64, PbIsometry3F64]( - sophus::Isometry3F64 const& self) { + sophus2::Isometry3F64 const& self) { Eigen::Vector3d p = self.translation(); return PbIsometry3F64( "rotation"_a = rotation3F64ToProto(self.rotation()), @@ -114,7 +114,7 @@ void bind_lie(py::module_& m) { auto isometry3F64FromProto = [rotation3F64FromProto](py::object proto) { auto translation = proto.attr("translation"); - return sophus::Isometry3F64( + return sophus2::Isometry3F64( Eigen::Vector3d( py::cast(translation.attr("x")), py::cast(translation.attr("y")), @@ -167,100 +167,100 @@ void bind_lie(py::module_& m) { tangent_of_b_in_a); }; - bind_liegroup(m, "Rotation2F64") + bind_liegroup(m, "Rotation2F64") .def_property( "rotation_matrix", - &sophus::Rotation2F64::rotationMatrix, - [](sophus::Rotation2F64& self, Eigen::Matrix2d const& mat) { - self = sophus::Rotation2F64::fromRotationMatrix(mat); + &sophus2::Rotation2F64::rotationMatrix, + [](sophus2::Rotation2F64& self, Eigen::Matrix2d const& mat) { + self = sophus2::Rotation2F64::fromRotationMatrix(mat); }) .def( "to_proto", - [PbRotation2F64](sophus::Rotation2F64& self) { + [PbRotation2F64](sophus2::Rotation2F64& self) { return PbRotation2F64("theta"_a = self.log()[0]); }) .def_static("from_proto", [](py::object proto) { - return sophus::Rotation2F64::fromAngle( + return sophus2::Rotation2F64::fromAngle( py::cast(proto.attr("theta"))); }); - bind_liegroup(m, "Rotation3F64") + bind_liegroup(m, "Rotation3F64") .def_property( "rotation_matrix", - &sophus::Rotation3F64::rotationMatrix, - [](sophus::Rotation3F64& self, Eigen::Matrix3d const& mat) { - self = sophus::Rotation3F64::fromRotationMatrix(mat); + &sophus2::Rotation3F64::rotationMatrix, + [](sophus2::Rotation3F64& self, Eigen::Matrix3d const& mat) { + self = sophus2::Rotation3F64::fromRotationMatrix(mat); }) .def("to_proto", rotation3F64ToProto) .def_static("from_proto", rotation3F64FromProto) - .def_static("Rx", sophus::Rotation3F64::fromRx) - .def_static("Ry", sophus::Rotation3F64::fromRy) - .def_static("Rz", sophus::Rotation3F64::fromRz); + .def_static("Rx", sophus2::Rotation3F64::fromRx) + .def_static("Ry", sophus2::Rotation3F64::fromRy) + .def_static("Rz", sophus2::Rotation3F64::fromRz); - bind_liegroup(m, "Isometry3F64") + bind_liegroup(m, "Isometry3F64") .def( py::init([](Eigen::Vector3d const& translation, - sophus::Rotation3F64 const& rotation) { - return sophus::Isometry3F64(translation, rotation); + sophus2::Rotation3F64 const& rotation) { + return sophus2::Isometry3F64(translation, rotation); }), py::arg("translation") = Eigen::Vector3d::Zero(), - py::arg("rotation") = sophus::Rotation3F64()) + py::arg("rotation") = sophus2::Rotation3F64()) .def_property( "rotation_matrix", - &sophus::Isometry3F64::rotationMatrix, - [](sophus::Isometry3F64& self, Eigen::Matrix3d const& mat) { - self.setRotation(sophus::Rotation3F64::fromRotationMatrix(mat)); + &sophus2::Isometry3F64::rotationMatrix, + [](sophus2::Isometry3F64& self, Eigen::Matrix3d const& mat) { + self.setRotation(sophus2::Rotation3F64::fromRotationMatrix(mat)); }) .def_property( "rotation", - [](sophus::Isometry3F64& self) { return self.rotation(); }, - [](sophus::Isometry3F64& self, sophus::Rotation3F64 const& x) { + [](sophus2::Isometry3F64& self) { return self.rotation(); }, + [](sophus2::Isometry3F64& self, sophus2::Rotation3F64 const& x) { return self.setRotation(x); }) .def_property( "translation", - [](sophus::Isometry3F64& self) { return self.translation(); }, - [](sophus::Isometry3F64& self, Eigen::Vector3d const& x) { + [](sophus2::Isometry3F64& self) { return self.translation(); }, + [](sophus2::Isometry3F64& self, Eigen::Vector3d const& x) { return self.translation() = x; }) .def("to_proto", isometry3F64ToProto) .def_static("from_proto", isometry3F64FromProto) - .def_static("Rx", sophus::Isometry3F64::fromRx) - .def_static("Ry", sophus::Isometry3F64::fromRy) - .def_static("Rz", sophus::Isometry3F64::fromRz); + .def_static("Rx", sophus2::Isometry3F64::fromRx) + .def_static("Ry", sophus2::Isometry3F64::fromRy) + .def_static("Rz", sophus2::Isometry3F64::fromRz); ; - bind_liegroup(m, "Isometry2F64") + bind_liegroup(m, "Isometry2F64") .def(py::init([](Eigen::Vector2d const& translation, - sophus::Rotation2F64 const& rotation) { - return sophus::Isometry2F64(translation, rotation); + sophus2::Rotation2F64 const& rotation) { + return sophus2::Isometry2F64(translation, rotation); })) .def(py::init([](Eigen::Vector2d const& translation, double angle) { - return sophus::Isometry2F64( - translation, sophus::Rotation2F64::fromAngle(angle)); + return sophus2::Isometry2F64( + translation, sophus2::Rotation2F64::fromAngle(angle)); })) .def_property( "rotation_matrix", - &sophus::Isometry2F64::rotationMatrix, - [](sophus::Isometry2F64& self, Eigen::Matrix2d const& mat) { - self.setRotation(sophus::Rotation2F64::fromRotationMatrix(mat)); + &sophus2::Isometry2F64::rotationMatrix, + [](sophus2::Isometry2F64& self, Eigen::Matrix2d const& mat) { + self.setRotation(sophus2::Rotation2F64::fromRotationMatrix(mat)); }) .def_property( "rotation", - [](sophus::Isometry2F64& self) { return self.rotation(); }, - [](sophus::Isometry2F64& self, sophus::Rotation2F64 const& x) { + [](sophus2::Isometry2F64& self) { return self.rotation(); }, + [](sophus2::Isometry2F64& self, sophus2::Rotation2F64 const& x) { return self.setRotation(x); }) .def_property( "theta", - [](sophus::Isometry2F64& self) { return self.angle(); }, - [](sophus::Isometry2F64& self, double theta) { - return self.setRotation(sophus::Rotation2F64::fromAngle(theta)); + [](sophus2::Isometry2F64& self) { return self.angle(); }, + [](sophus2::Isometry2F64& self, double theta) { + return self.setRotation(sophus2::Rotation2F64::fromAngle(theta)); }) .def_property( "translation", - [](sophus::Isometry2F64& self) { return self.translation(); }, - [](sophus::Isometry2F64& self, Eigen::Vector2d const& x) { + [](sophus2::Isometry2F64& self) { return self.translation(); }, + [](sophus2::Isometry2F64& self, Eigen::Vector2d const& x) { return self.translation() = x; }); @@ -304,7 +304,7 @@ void bind_lie(py::module_& m) { .def_property( "rotation", [](Pose3F64 const& self) { return self.rotation(); }, - [](Pose3F64& self, sophus::Rotation3F64 const& x) { + [](Pose3F64& self, sophus2::Rotation3F64 const& x) { return self.setRotation(x); }) .def_property( diff --git a/setup.py b/setup.py index 3cdc0c29..1c5c877d 100644 --- a/setup.py +++ b/setup.py @@ -109,7 +109,8 @@ ] setup( - ext_modules=ext_modules, + # DISABLED DURING VCPKG REFACTOR + # ext_modules=ext_modules, cmdclass={ "install": BuildProtosInstall, "develop": BuildProtosDevelop, diff --git a/vcpkg b/vcpkg new file mode 160000 index 00000000..0e47c198 --- /dev/null +++ b/vcpkg @@ -0,0 +1 @@ +Subproject commit 0e47c1985273129e4d0ee52ff73bed9125555de8 diff --git a/vcpkg-configuration.json b/vcpkg-configuration.json new file mode 100644 index 00000000..87dde1bf --- /dev/null +++ b/vcpkg-configuration.json @@ -0,0 +1,14 @@ +{ + "default-registry": { + "kind": "git", + "baseline": "0e47c1985273129e4d0ee52ff73bed9125555de8", + "repository": "https://github.com/microsoft/vcpkg" + }, + "registries": [ + { + "kind": "artifact", + "location": "https://github.com/microsoft/vcpkg-ce-catalog/archive/refs/heads/main.zip", + "name": "microsoft" + } + ] +} diff --git a/vcpkg.json b/vcpkg.json new file mode 100644 index 00000000..03ad700e --- /dev/null +++ b/vcpkg.json @@ -0,0 +1,33 @@ +{ + "name": "farm-ng-core", + "version": "0.0.0", + "description": "Foundational library for robotics and machine sensing applications", + "homepage": "https://github.com/farm-ng/farm-ng-core", + "license": "Apache-2.0", + "dependencies": [ + "boost-asio", + "boost-signals2", + { + "name": "ceres", + "features": ["eigensparse"] + }, + "cli11", + "eigen3", + "fmt", + { + "name": "grpc", + "features": ["codegen"] + }, + "gtest", + "protobuf", + "tl-expected", + { + "name": "vcpkg-cmake", + "host": true + }, + { + "name": "vcpkg-cmake-config", + "host": true + } + ] +}