diff --git a/.github/scripts/boost.sh b/.github/scripts/boost.sh index 647a84628..3c7e01274 100644 --- a/.github/scripts/boost.sh +++ b/.github/scripts/boost.sh @@ -9,10 +9,10 @@ tar -zxf ${BOOST_FOLDER}.tar.gz # Bootstrap cd ${BOOST_FOLDER}/ -./bootstrap.sh +./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex # Build and install -sudo ./b2 install +sudo ./b2 -j$(nproc) install # Rebuild ld cache sudo ldconfig diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 22098ec08..3f5701281 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -85,4 +85,4 @@ make -j2 install cd $GITHUB_WORKSPACE/build/python $PYTHON setup.py install --user --prefix= cd $GITHUB_WORKSPACE/python/gtsam/tests -$PYTHON -m unittest discover +$PYTHON -m unittest discover -v diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 55a8ac372..9689d346c 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -68,6 +68,8 @@ function configure() -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ + -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \ + -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DBOOST_ROOT=$BOOST_ROOT \ -DBoost_NO_SYSTEM_PATHS=ON \ @@ -92,7 +94,11 @@ function build () configure - make -j2 + if [ "$(uname)" == "Linux" ]; then + make -j$(nproc) + elif [ "$(uname)" == "Darwin" ]; then + make -j$(sysctl -n hw.physicalcpu) + fi finish } @@ -105,8 +111,12 @@ function test () configure - # Actual build: - make -j2 check + # Actual testing + if [ "$(uname)" == "Linux" ]; then + make -j$(nproc) check + elif [ "$(uname)" == "Darwin" ]; then + make -j$(sysctl -n hw.physicalcpu) check + fi finish } diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 5483c32cf..f52e5eec3 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -47,8 +47,7 @@ jobs: - name: Checkout uses: actions/checkout@v2 - - name: Install (Linux) - if: runner.os == 'Linux' + - name: Install Dependencies run: | # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then @@ -63,7 +62,7 @@ jobs: fi sudo apt-get -y update - sudo apt-get install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev + sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib @@ -79,7 +78,5 @@ jobs: run: | bash .github/scripts/boost.sh - - name: Build and Test (Linux) - if: runner.os == 'Linux' - run: | - bash .github/scripts/unix.sh -t + - name: Build and Test + run: bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index ed8f8563b..462723222 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -34,8 +34,7 @@ jobs: - name: Checkout uses: actions/checkout@v2 - - name: Install (macOS) - if: runner.os == 'macOS' + - name: Install Dependencies run: | brew install cmake ninja brew install boost @@ -48,7 +47,5 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi - - name: Build and Test (macOS) - if: runner.os == 'macOS' - run: | - bash .github/scripts/unix.sh -t + - name: Build and Test + run: bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index addde8c64..3fc2d662f 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -81,7 +81,7 @@ jobs: fi sudo apt-get -y update - sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev + sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 6427e13bc..647b9c0f1 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -55,6 +55,12 @@ jobs: version: "9" flag: cayley + - name: ubuntu-system-libs + os: ubuntu-18.04 + compiler: gcc + version: "9" + flag: system-libs + steps: - name: Checkout uses: actions/checkout@v2 @@ -70,7 +76,7 @@ jobs: fi sudo apt-get -y update - sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev + sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib @@ -126,6 +132,12 @@ jobs: echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV echo "GTSAM Uses Cayley map for Rot3" + - name: Use system versions of 3rd party libraries + if: matrix.flag == 'system' + run: | + echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV + echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV + - name: Build & Test run: | bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index a564dade9..5dfdcd013 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -12,42 +12,46 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} + BOOST_VERSION: 1.72.0 + BOOST_EXE: boost_1_72_0-msvc-14.2 + strategy: fail-fast: false matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - #TODO This build keeps timing out, need to understand why. - # windows-2016-cl, - windows-2019-cl, - ] + #TODO This build fails, need to understand why. + # windows-2016-cl, + windows-2019-cl, + ] build_type: [Debug, Release] build_unstable: [ON] include: - - #TODO This build keeps timing out, need to understand why. + #TODO This build fails, need to understand why. # - name: windows-2016-cl # os: windows-2016 # compiler: cl + # platform: 32 - name: windows-2019-cl os: windows-2019 compiler: cl + platform: 64 steps: - - name: Checkout - uses: actions/checkout@v2 - - name: Install (Windows) - if: runner.os == 'Windows' + - name: Install Dependencies shell: powershell run: | Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh') + scoop install cmake --global # So we don't get issues with CMP0074 policy scoop install ninja --global + if ("${{ matrix.compiler }}".StartsWith("clang")) { scoop install llvm --global } + if ("${{ matrix.compiler }}" -eq "gcc") { # Chocolatey GCC is broken on the windows-2019 image. # See: https://github.com/DaanDeMeyer/doctest/runs/231595515 @@ -55,27 +59,38 @@ jobs: scoop install gcc --global echo "CC=gcc" >> $GITHUB_ENV echo "CXX=g++" >> $GITHUB_ENV + } elseif ("${{ matrix.compiler }}" -eq "clang") { echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV + } else { - echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV - echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV + echo "CC=${{ matrix.compiler }}" >> $env:GITHUB_ENV + echo "CXX=${{ matrix.compiler }}" >> $env:GITHUB_ENV + } + # Scoop modifies the PATH so we make the modified PATH global. - echo "$env:PATH" >> $GITHUB_PATH + echo "$env:PATH" >> $env:GITHUB_PATH - - name: Download and install Boost - uses: MarkusJx/install-boost@v1.0.1 - id: install-boost - with: - boost_version: 1.72.0 - toolset: msvc14.2 + - name: Install Boost + shell: powershell + run: | + # Snippet from: https://github.com/actions/virtual-environments/issues/2667 + $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64" - - name: Build (Windows) - if: runner.os == 'Windows' - env: - BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }} + # Use the prebuilt binary for Windows + $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe" + (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") + Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH" + + # Set the BOOST_ROOT variable + echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV + + - name: Checkout + uses: actions/checkout@v2 + + - name: Build run: | cmake -E remove_directory build cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" diff --git a/CMakeLists.txt b/CMakeLists.txt index 2fdadc68a..d2559705d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,11 +38,14 @@ if(${GTSAM_SOURCE_DIR} STREQUAL ${GTSAM_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() +include(cmake/HandleGeneralOptions.cmake) # CMake build options + +# Libraries: include(cmake/HandleBoost.cmake) # Boost include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 -include(cmake/HandleGeneralOptions.cmake) # CMake build options +include(cmake/HandleMetis.cmake) # metis include(cmake/HandleMKL.cmake) # MKL include(cmake/HandleOpenMP.cmake) # OpenMP include(cmake/HandlePerfTools.cmake) # Google perftools diff --git a/DEVELOP.md b/DEVELOP.md index 133f3ea11..8604afe0f 100644 --- a/DEVELOP.md +++ b/DEVELOP.md @@ -17,3 +17,5 @@ class GTSAM_EXPORT MyClass { ... }; GTSAM_EXPORT myFunction(); ``` + +More details [here](Using-GTSAM-EXPORT.md). diff --git a/INSTALL.md b/INSTALL.md index 64857774a..965246304 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -13,7 +13,8 @@ $ make install ## Important Installation Notes 1. GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.58 or greater (install through Linux repositories or MacPorts) + - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes). + - Cmake version 3.0 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher @@ -66,11 +67,15 @@ execute commands as follows for an out-of-source build: This will build the library and unit tests, run all of the unit tests, and then install the library itself. +## Boost Notes + +Versions of Boost prior to 1.65 have a known bug that prevents proper "deep" serialization of objects, which means that objects encapsulated inside other objects don't get serialized. +This is particularly seen when using `clang` as the C++ compiler. + +For this reason we require Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager. + ## Known Issues -- When using `GTSAM_BUILD_WITH_MARCH_NATIVE=ON`, you may encounter issues in running tests which we are still investigating: - - Use of a version of GCC < 7.5 results in an "Indeterminant Linear System" error for `testSmartProjectionFactor`. - - Use of Boost version < 1.67 with clang will give a segfault for mulitple test cases. - MSVC 2013 is not yet supported because it cannot build the serialization module of Boost 1.55 (or earlier). # Windows Installation diff --git a/README.md b/README.md index 60eff197a..046132301 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ $ make install Prerequisites: -- [Boost](http://www.boost.org/users/download/) >= 1.58 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [Boost](http://www.boost.org/users/download/) >= 1.65 (Ubuntu: `sudo apt-get install libboost-all-dev`) - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`) - A modern compiler, i.e., at least gcc 4.7.3 on Linux. @@ -55,9 +55,9 @@ Optional prerequisites - used automatically if findable by CMake: GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. It also introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect. -GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 to use the deprecated methods. +GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4` to use the deprecated methods. -GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag GTSAM_ALLOW_DEPRECATED_SINCE_V41 for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile. +GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V41` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile. ## Wrappers @@ -68,16 +68,16 @@ We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) GTSAM includes a state of the art IMU handling scheme based on -- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505) +- Todd Lupton and Salah Sukkarieh, _"Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions"_, TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505) Our implementation improves on this using integration on the manifold, as detailed in -- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483) +- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, _"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors"_, Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483) - Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf) If you are using the factor in academic work, please cite the publications above. -In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF. +In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag `GTSAM_TANGENT_PREINTEGRATION` to OFF. ## Additional Information diff --git a/Using-GTSAM-EXPORT.md b/Using-GTSAM-EXPORT.md index 41eccc178..cae1d499c 100644 --- a/Using-GTSAM-EXPORT.md +++ b/Using-GTSAM-EXPORT.md @@ -10,7 +10,7 @@ To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and need 3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.) ## When is GTSAM_EXPORT being used incorrectly -Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library. For example, an error in gtsam/base will often show up when compiling the check_base_program or the MATLAB wrapper, but not when compiling/linking gtsam itself. The most common errors will say something like: +Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library. For example, an error in `gtsam/base` will often show up when compiling the `check_base_program` or the MATLAB wrapper, but not when compiling/linking gtsam itself. The most common errors will say something like: ``` Error LNK2019 unresolved external symbol "public: void __cdecl gtsam::SO3::print(class std::basic_string,class std::allocator > const &)const " (?print@SO3@gtsam@@QEBAXAEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) referenced in function "public: static void __cdecl gtsam::Testable::Print(class gtsam::SO3 const &,class std::basic_string,class std::allocator > const &)" (?Print@?$Testable@VSO3@gtsam@@@gtsam@@SAXAEBVSO3@2@AEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) check_geometry_program C:\AFIT\lib\gtsam\build\gtsam\geometry\tests\testSO3.obj diff --git a/cmake/FindTBB.cmake b/cmake/FindTBB.cmake index e2b1df6e3..0ecd4ca0e 100644 --- a/cmake/FindTBB.cmake +++ b/cmake/FindTBB.cmake @@ -144,7 +144,8 @@ if(NOT TBB_FOUND) elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin") # OS X - set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb") + set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb" + "/usr/local/opt/tbb") # TODO: Check to see which C++ library is being used by the compiler. if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0) @@ -181,7 +182,18 @@ if(NOT TBB_FOUND) ################################## if(TBB_INCLUDE_DIRS) - file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file) + set(_tbb_version_file_prior_to_tbb_2021_1 "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h") + set(_tbb_version_file_after_tbb_2021_1 "${TBB_INCLUDE_DIRS}/oneapi/tbb/version.h") + + if (EXISTS "${_tbb_version_file_prior_to_tbb_2021_1}") + file(READ "${_tbb_version_file_prior_to_tbb_2021_1}" _tbb_version_file ) + elseif (EXISTS "${_tbb_version_file_after_tbb_2021_1}") + file(READ "${_tbb_version_file_after_tbb_2021_1}" _tbb_version_file ) + else() + message(FATAL_ERROR "Found TBB installation: ${TBB_INCLUDE_DIRS} " + "missing version header.") + endif() + string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1" TBB_VERSION_MAJOR "${_tbb_version_file}") string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1" diff --git a/cmake/HandleBoost.cmake b/cmake/HandleBoost.cmake index e73c2237d..6c742cfe5 100644 --- a/cmake/HandleBoost.cmake +++ b/cmake/HandleBoost.cmake @@ -22,7 +22,7 @@ endif() # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. -set(BOOST_FIND_MINIMUM_VERSION 1.58) +set(BOOST_FIND_MINIMUM_VERSION 1.65) set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) @@ -30,7 +30,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) - message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.") + message(FATAL_ERROR "Missing required Boost components >= v1.65, please install/upgrade Boost or configure your search paths.") endif() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) diff --git a/cmake/HandleCPack.cmake b/cmake/HandleCPack.cmake index 1c32433a4..0f8d1680c 100644 --- a/cmake/HandleCPack.cmake +++ b/cmake/HandleCPack.cmake @@ -25,4 +25,4 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION # Deb-package specific cpack set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.65)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index ee86066a2..64c239f39 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -14,20 +14,21 @@ if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) endif() -option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) -option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) -option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) -option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) -option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) -option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) -option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) -option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) -option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) -option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) -option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) -option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) +option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) +option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) +option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) +option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) +option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) +option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) +option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) +option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) +option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) +option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) if(NOT MSVC AND NOT XCODE_VERSION) option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) endif() diff --git a/cmake/HandleMetis.cmake b/cmake/HandleMetis.cmake new file mode 100644 index 000000000..9c29e5776 --- /dev/null +++ b/cmake/HandleMetis.cmake @@ -0,0 +1,44 @@ +############################################################################### +# Metis library + +# For both system or bundle version, a cmake target "metis-gtsam-if" is defined (interface library) + +# Dont try to use metis if GTSAM_SUPPORT_NESTED_DISSECTION is disabled: +if (NOT GTSAM_SUPPORT_NESTED_DISSECTION) + return() +endif() + +option(GTSAM_USE_SYSTEM_METIS "Find and use system-installed libmetis. If 'off', use the one bundled with GTSAM" OFF) + +if(GTSAM_USE_SYSTEM_METIS) + # Debian package: libmetis-dev + + find_path(METIS_INCLUDE_DIR metis.h REQUIRED) + find_library(METIS_LIBRARY metis REQUIRED) + + if(METIS_INCLUDE_DIR AND METIS_LIBRARY) + mark_as_advanced(METIS_INCLUDE_DIR) + mark_as_advanced(METIS_LIBRARY) + + add_library(metis-gtsam-if INTERFACE) + target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR}) + target_link_libraries(metis-gtsam-if INTERFACE ${METIS_LIBRARY}) + endif() +else() + # Bundled version: + option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) + add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis) + + target_include_directories(metis-gtsam BEFORE PUBLIC + $ + $ + $ + $ + ) + + add_library(metis-gtsam-if INTERFACE) + target_link_libraries(metis-gtsam-if INTERFACE metis-gtsam) +endif() + +list(APPEND GTSAM_EXPORTED_TARGETS metis-gtsam-if) +install(TARGETS metis-gtsam-if EXPORT GTSAM-exports ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index 4ffd00e54..ad6ac5c5c 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -32,6 +32,7 @@ endif() print_build_options_for_target(gtsam) print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") +print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}") if(GTSAM_USE_TBB) print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})") diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake index cedee55ea..118dc4dac 100644 --- a/cmake/HandleTBB.cmake +++ b/cmake/HandleTBB.cmake @@ -1,24 +1,32 @@ ############################################################################### -# Find TBB -find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) +if (GTSAM_WITH_TBB) + # Find TBB + find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) -# Set up variables if we're using TBB -if(TBB_FOUND AND GTSAM_WITH_TBB) - set(GTSAM_USE_TBB 1) # This will go into config.h - if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) - set(TBB_GREATER_EQUAL_2020 1) + # Set up variables if we're using TBB + if(TBB_FOUND) + set(GTSAM_USE_TBB 1) # This will go into config.h + + if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1")) + message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.") + endif() + + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() + # all definitions and link requisites will go via imported targets: + # tbb & tbbmalloc + list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) else() - set(TBB_GREATER_EQUAL_2020 0) + set(GTSAM_USE_TBB 0) # This will go into config.h + endif() + + ############################################################################### + # Prohibit Timing build mode in combination with TBB + if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") endif() - # all definitions and link requisites will go via imported targets: - # tbb & tbbmalloc - list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) -else() - set(GTSAM_USE_TBB 0) # This will go into config.h -endif() -############################################################################### -# Prohibit Timing build mode in combination with TBB -if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) - message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") endif() diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 7c43a8989..2218addcf 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -22,18 +22,19 @@ if (GTSAM_BUILD_DOCS) # GTSAM core subfolders set(gtsam_doc_subdirs - gtsam/base - gtsam/discrete - gtsam/geometry - gtsam/inference - gtsam/linear - gtsam/navigation - gtsam/nonlinear - gtsam/sam - gtsam/sfm - gtsam/slam - gtsam/smart - gtsam/symbolic + gtsam/base + gtsam/basis + gtsam/discrete + gtsam/geometry + gtsam/inference + gtsam/linear + gtsam/navigation + gtsam/nonlinear + gtsam/sam + gtsam/sfm + gtsam/slam + gtsam/smart + gtsam/symbolic gtsam ) diff --git a/docker/README.md b/docker/README.md index 0c136f94c..37c47a27f 100644 --- a/docker/README.md +++ b/docker/README.md @@ -1,6 +1,57 @@ # Instructions -Build all docker images, in order: +# Images on Docker Hub + +There are 4 images available on https://hub.docker.com/orgs/borglab/repositories: + +- `borglab/ubuntu-boost-tbb`: 18.06 Linux (nicknamed `bionic`) base image, with Boost and TBB installed. +- `borglab/ubuntu-gtsam`: GTSAM Release version installed in `/usr/local`. +- `borglab/ubuntu-gtsam-python`: installed GTSAM with python wrapper. +- `borglab/ubuntu-gtsam-python-vnc`: image with GTSAM+python wrapper that will run a VNC server to connect to. + +# Using the images + +## Just GTSAM + +To start the Docker image, execute +```bash +docker run -it borglab/ubuntu-gtsam:bionic +``` +after you will find yourself in a bash shell, in the directory `/usr/src/gtsam/build`. +## GTSAM with Python wrapper + +To use GTSAM via the python wrapper, similarly execute +```bash +docker run -it borglab/ubuntu-gtsam-python:bionic +``` +and then launch `python3`: +```bash +python3 +>>> import gtsam +>>> gtsam.Pose2(1,2,3) +(1, 2, 3) +``` + +## GTSAM with Python wrapper and VNC + +First, start the docker image, which will run a VNC server on port 5900: +```bash +docker run -p 5900:5900 borglab/ubuntu-gtsam-python-vnc:bionic +``` + +Then open a remote VNC X client, for example: + +### Linux +```bash +sudo apt-get install tigervnc-viewer +xtigervncviewer :5900 +``` +### Mac +The Finder's "Connect to Server..." with `vnc://127.0.0.1` does not work, for some reason. Using the free [VNC Viewer](https://www.realvnc.com/en/connect/download/viewer/), enter `0.0.0.0:5900` as the server. + +# Re-building the images locally + +To build all docker images, in order: ```bash (cd ubuntu-boost-tbb && ./build.sh) @@ -9,13 +60,4 @@ Build all docker images, in order: (cd ubuntu-gtsam-python-vnc && ./build.sh) ``` -Then launch with: - - docker run -p 5900:5900 dellaert/ubuntu-gtsam-python-vnc:bionic - -Then open a remote VNC X client, for example: - - sudo apt-get install tigervnc-viewer - xtigervncviewer :5900 - - +Note: building GTSAM can take a lot of memory because of the heavy templating. It is advisable to give Docker enough resources, e.g., 8GB, to avoid OOM errors while compiling. \ No newline at end of file diff --git a/docker/ubuntu-boost-tbb/build.sh b/docker/ubuntu-boost-tbb/build.sh index 2dac4c3db..35b349c6a 100755 --- a/docker/ubuntu-boost-tbb/build.sh +++ b/docker/ubuntu-boost-tbb/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image # TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t dellaert/ubuntu-boost-tbb:bionic . +docker build --no-cache -t borglab/ubuntu-boost-tbb:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/Dockerfile b/docker/ubuntu-gtsam-python-vnc/Dockerfile index 61ecd9b9a..8039698c3 100644 --- a/docker/ubuntu-gtsam-python-vnc/Dockerfile +++ b/docker/ubuntu-gtsam-python-vnc/Dockerfile @@ -1,7 +1,7 @@ # This GTSAM image connects to the host X-server via VNC to provide a Graphical User Interface for interaction. # Get the base Ubuntu/GTSAM image from Docker Hub -FROM dellaert/ubuntu-gtsam-python:bionic +FROM borglab/ubuntu-gtsam-python:bionic # Things needed to get a python GUI ENV DEBIAN_FRONTEND noninteractive diff --git a/docker/ubuntu-gtsam-python-vnc/build.sh b/docker/ubuntu-gtsam-python-vnc/build.sh index 8d280252f..a0bbb6a96 100755 --- a/docker/ubuntu-gtsam-python-vnc/build.sh +++ b/docker/ubuntu-gtsam-python-vnc/build.sh @@ -1,4 +1,4 @@ # Build command for Docker image # TODO(dellaert): use docker compose and/or cmake # Needs to be run in docker/ubuntu-gtsam-python-vnc directory -docker build -t dellaert/ubuntu-gtsam-python-vnc:bionic . +docker build -t borglab/ubuntu-gtsam-python-vnc:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/vnc.sh b/docker/ubuntu-gtsam-python-vnc/vnc.sh index c0ab692c6..b749093af 100755 --- a/docker/ubuntu-gtsam-python-vnc/vnc.sh +++ b/docker/ubuntu-gtsam-python-vnc/vnc.sh @@ -2,4 +2,4 @@ docker run -it \ --workdir="/usr/src/gtsam" \ -p 5900:5900 \ - dellaert/ubuntu-gtsam-python-vnc:bionic \ No newline at end of file + borglab/ubuntu-gtsam-python-vnc:bionic \ No newline at end of file diff --git a/docker/ubuntu-gtsam-python/Dockerfile b/docker/ubuntu-gtsam-python/Dockerfile index ce5d8fdca..85eed4d4e 100644 --- a/docker/ubuntu-gtsam-python/Dockerfile +++ b/docker/ubuntu-gtsam-python/Dockerfile @@ -1,7 +1,7 @@ # GTSAM Ubuntu image with Python wrapper support. # Get the base Ubuntu/GTSAM image from Docker Hub -FROM dellaert/ubuntu-gtsam:bionic +FROM borglab/ubuntu-gtsam:bionic # Install pip RUN apt-get install -y python3-pip python3-dev @@ -22,7 +22,9 @@ RUN cmake \ .. # Build again, as ubuntu-gtsam image cleaned -RUN make -j4 install && make clean +RUN make -j4 install +RUN make python-install +RUN make clean # Needed to run python wrapper: RUN echo 'export PYTHONPATH=/usr/local/python/:$PYTHONPATH' >> /root/.bashrc diff --git a/docker/ubuntu-gtsam-python/build.sh b/docker/ubuntu-gtsam-python/build.sh index 1696f6c61..68827074d 100755 --- a/docker/ubuntu-gtsam-python/build.sh +++ b/docker/ubuntu-gtsam-python/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image # TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t dellaert/ubuntu-gtsam-python:bionic . +docker build --no-cache -t borglab/ubuntu-gtsam-python:bionic . diff --git a/docker/ubuntu-gtsam/Dockerfile b/docker/ubuntu-gtsam/Dockerfile index f2b476f15..ce6927fe8 100644 --- a/docker/ubuntu-gtsam/Dockerfile +++ b/docker/ubuntu-gtsam/Dockerfile @@ -1,7 +1,7 @@ # Ubuntu image with GTSAM installed. Configured with Boost and TBB support. # Get the base Ubuntu image from Docker Hub -FROM dellaert/ubuntu-boost-tbb:bionic +FROM borglab/ubuntu-boost-tbb:bionic # Install git RUN apt-get update && \ diff --git a/docker/ubuntu-gtsam/build.sh b/docker/ubuntu-gtsam/build.sh index bf545e9c2..790ee1575 100755 --- a/docker/ubuntu-gtsam/build.sh +++ b/docker/ubuntu-gtsam/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image # TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t dellaert/ubuntu-gtsam:bionic . +docker build --no-cache -t borglab/ubuntu-gtsam:bionic . diff --git a/examples/README.md b/examples/README.md index 9d58b5200..5a72736e0 100644 --- a/examples/README.md +++ b/examples/README.md @@ -51,13 +51,13 @@ The directory **vSLAMexample** includes 2 simple examples using GTSAM: See the separate README file there. -##Undirected Graphical Models (UGM) +## Undirected Graphical Models (UGM) The best representation for a Markov Random Field is a factor graph :-) This is illustrated with some discrete examples from the UGM MATLAB toolbox, which can be found at -##Building and Running -To build, cd into the directory and do: +## Building and Running +To build, cd into the top-level gtsam directory and do: ``` mkdir build diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 8b356393b..a1e917bbe 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -49,10 +49,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() -option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) -if(GTSAM_SUPPORT_NESTED_DISSECTION) - add_subdirectory(metis) -endif() +# metis: already handled in ROOT/cmake/HandleMetis.cmake add_subdirectory(ceres) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 71daf0653..535d60eb1 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,6 +5,7 @@ project(gtsam LANGUAGES CXX) # The following variable is the master list of subdirs to add set (gtsam_subdirs base + basis geometry inference symbolic @@ -88,7 +89,8 @@ list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/d install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) if(GTSAM_SUPPORT_NESTED_DISSECTION) - list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam) + # target metis-gtsam-if is defined in both cases: embedded metis or system version: + list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam-if) endif() # Versions @@ -154,16 +156,6 @@ target_include_directories(gtsam SYSTEM BEFORE PUBLIC $ $ ) -if(GTSAM_SUPPORT_NESTED_DISSECTION) - target_include_directories(gtsam BEFORE PUBLIC - $ - $ - $ - $ - ) -endif() - - if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library if (NOT BUILD_SHARED_LIBS) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fe730c934..ac7c2a9a5 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -17,6 +17,7 @@ * @author Frank Dellaert * @author Mike Bosse * @author Duy Nguyen Ta + * @author Yotam Stern */ @@ -319,12 +320,28 @@ T expm(const Vector& x, int K=7) { } /** - * Linear interpolation between X and Y by coefficient t in [0, 1]. + * Linear interpolation between X and Y by coefficient t. Typically t \in [0,1], + * but can also be used to extrapolate before pose X or after pose Y. */ template -T interpolate(const T& X, const T& Y, double t) { - assert(t >= 0 && t <= 1); - return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); +T interpolate(const T& X, const T& Y, double t, + typename MakeOptionalJacobian::type Hx = boost::none, + typename MakeOptionalJacobian::type Hy = boost::none) { + if (Hx || Hy) { + typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; + const T between = + traits::Between(X, Y, between_H_x); // between_H_y = identity + typename traits::TangentVector delta = traits::Logmap(between, log_H); + const T Delta = traits::Expmap(t * delta, exp_H); + const T result = traits::Compose( + X, Delta, compose_H_x); // compose_H_xinv_y = identity + + if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; + if (Hy) *Hy = t * exp_H * log_H; + return result; + } + return traits::Compose( + X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } /** diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 4b580f82e..07801df7a 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -89,6 +89,13 @@ public: usurp(dynamic.data()); } + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd* dynamic) : + map_(nullptr) { + dynamic->resize(Rows, Cols); // no malloc if correct size + usurp(dynamic->data()); + } + #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty diff --git a/gtsam/base/base.i b/gtsam/base/base.i index c24b04088..d9c51fbe8 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -33,13 +33,13 @@ class IndexPair { size_t j() const; }; -// template -// class DSFMap { -// DSFMap(); -// KEY find(const KEY& key) const; -// void merge(const KEY& x, const KEY& y); -// std::map sets(); -// }; +template +class DSFMap { + DSFMap(); + KEY find(const KEY& key) const; + void merge(const KEY& x, const KEY& y); + std::map sets(); +}; class IndexPairSet { IndexPairSet(); @@ -81,13 +81,6 @@ class IndexPairSetMap { gtsam::IndexPairSet at(gtsam::IndexPair& key); }; -class DSFMapIndexPair { - DSFMapIndexPair(); - gtsam::IndexPair find(const gtsam::IndexPair& key) const; - void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); - gtsam::IndexPairSetMap sets(); -}; - #include bool linear_independent(Matrix A, Matrix B, double tol); diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index 128576107..ae91642f4 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -24,40 +24,33 @@ using namespace std; using namespace gtsam; //****************************************************************************** +#define TEST_CONSTRUCTOR(DIM1, DIM2, X, TRUTHY) \ + { \ + OptionalJacobian H(X); \ + EXPECT(H == TRUTHY); \ + } TEST( OptionalJacobian, Constructors ) { Matrix23 fixed; - - OptionalJacobian<2, 3> H1; - EXPECT(!H1); - - OptionalJacobian<2, 3> H2(fixed); - EXPECT(H2); - - OptionalJacobian<2, 3> H3(&fixed); - EXPECT(H3); - Matrix dynamic; - OptionalJacobian<2, 3> H4(dynamic); - EXPECT(H4); - - OptionalJacobian<2, 3> H5(boost::none); - EXPECT(!H5); - boost::optional optional(dynamic); - OptionalJacobian<2, 3> H6(optional); - EXPECT(H6); + OptionalJacobian<2, 3> H; + EXPECT(!H); + + TEST_CONSTRUCTOR(2, 3, fixed, true); + TEST_CONSTRUCTOR(2, 3, &fixed, true); + TEST_CONSTRUCTOR(2, 3, dynamic, true); + TEST_CONSTRUCTOR(2, 3, &dynamic, true); + TEST_CONSTRUCTOR(2, 3, boost::none, false); + TEST_CONSTRUCTOR(2, 3, optional, true); + + // Test dynamic OptionalJacobian<-1, -1> H7; EXPECT(!H7); - OptionalJacobian<-1, -1> H8(dynamic); - EXPECT(H8); - - OptionalJacobian<-1, -1> H9(boost::none); - EXPECT(!H9); - - OptionalJacobian<-1, -1> H10(optional); - EXPECT(H10); + TEST_CONSTRUCTOR(-1, -1, dynamic, true); + TEST_CONSTRUCTOR(-1, -1, boost::none, false); + TEST_CONSTRUCTOR(-1, -1, optional, true); } //****************************************************************************** @@ -101,6 +94,25 @@ TEST( OptionalJacobian, Fixed) { dynamic2.setOnes(); test(dynamic2); EXPECT(assert_equal(kTestMatrix, dynamic2)); + + { // Dynamic pointer + // Passing in an empty matrix means we want it resized + Matrix dynamic0; + test(&dynamic0); + EXPECT(assert_equal(kTestMatrix, dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3, 5); + dynamic1.setOnes(); + test(&dynamic1); + EXPECT(assert_equal(kTestMatrix, dynamic1)); + + // Dynamic right size + Matrix dynamic2(2, 5); + dynamic2.setOnes(); + test(&dynamic2); + EXPECT(assert_equal(kTestMatrix, dynamic2)); + } } //****************************************************************************** diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h new file mode 100644 index 000000000..d8bd28c1a --- /dev/null +++ b/gtsam/basis/Basis.h @@ -0,0 +1,507 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Basis.h + * @brief Compute an interpolating basis + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +/** + * This file supports creating continuous functions `f(x;p)` as a linear + * combination of `basis functions` such as the Fourier basis on SO(2) or a set + * of Chebyshev polynomials on [-1,1]. + * + * In the expression `f(x;p)` the variable `x` is + * the continuous argument at which the function is evaluated, and `p` are + * the parameters which are coefficients of the different basis functions, + * e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial. + * However, different parameterizations are also possible. + + * The `Basis` class below defines a number of functors that can be used to + * evaluate `f(x;p)` at a given `x`, and these functors also calculate + * the Jacobian of `f(x;p)` with respect to the parameters `p`. + * This is actually the most important calculation, as it will allow GTSAM + * to optimize over the parameters `p`. + + * This functionality is implemented using the `CRTP` or "Curiously recurring + * template pattern" C++ idiom, which is a meta-programming technique in which + * the derived class is passed as a template argument to `Basis`. + * The DERIVED class is assumed to satisfy a C++ concept, + * i.e., we expect it to define the following types and methods: + + - type `Parameters`: the parameters `p` in f(x;p) + - `CalculateWeights(size_t N, double x, double a=default, double b=default)` + - `DerivativeWeights(size_t N, double x, double a=default, double b=default)` + + where `Weights` is an N*1 row vector which defines the basis values for the + polynomial at the specified point `x`. + + E.g. A Fourier series would give the following: + - `CalculateWeights` -> For N=5, the values for the bases: + [1, cos(x), sin(x), cos(2x), sin(2x)] + - `DerivativeWeights` -> For N=5, these are: + [0, -sin(x), cos(x), -2sin(2x), 2cos(x)] + + Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are + instead the values for the Barycentric interpolation formula, since the values + at the polynomial points (e.g. Chebyshev points) define the bases. + */ + +namespace gtsam { + +using Weights = Eigen::Matrix; /* 1xN vector */ + +/** + * @brief Function for computing the kronecker product of the 1*N Weight vector + * `w` with the MxM identity matrix `I` efficiently. + * The main reason for this is so we don't need to use Eigen's Unsupported + * library. + * + * @tparam M Size of the identity matrix. + * @param w The weights of the polynomial. + * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I] + */ +template +Matrix kroneckerProductIdentity(const Weights& w) { + Matrix result(M, w.cols() * M); + result.setZero(); + + for (int i = 0; i < w.cols(); i++) { + result.block(0, i * M, M, M).diagonal().array() = w(i); + } + return result; +} + +/// CRTP Base class for function bases +template +class GTSAM_EXPORT Basis { + public: + /** + * Calculate weights for all x in vector X. + * Returns M*N matrix where M is the size of the vector X, + * and N is the number of basis functions. + */ + static Matrix WeightMatrix(size_t N, const Vector& X) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i)); + return W; + } + + /** + * @brief Calculate weights for all x in vector X, with interval [a,b]. + * + * @param N The number of basis functions. + * @param X The vector for which to compute the weights. + * @param a The lower bound for the interval range. + * @param b The upper bound for the interval range. + * @return Returns M*N matrix where M is the size of the vector X. + */ + static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); + return W; + } + + /** + * An instance of an EvaluationFunctor calculates f(x;p) at a given `x`, + * applied to Parameters `p`. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific N*1 vector of Parameters, returns the scalar + * value of the function at x, possibly with Jacobians wrpt the parameters. + */ + class EvaluationFunctor { + protected: + Weights weights_; + + public: + /// For serialization + EvaluationFunctor() {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x) + : weights_(DERIVED::CalculateWeights(N, x)) {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x, double a, double b) + : weights_(DERIVED::CalculateWeights(N, x, a, b)) {} + + /// Regular 1D evaluation + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + if (H) *H = weights_; + return (weights_ * p)(0); + } + + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + return apply(p, H); // might call apply in derived + } + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + */ + template + class VectorEvaluationFunctor : protected EvaluationFunctor { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorEvaluationFunctor() {} + + /// Default Constructor + VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { + calculateJacobian(); + } + + /// Constructor, with interval [a,b] + VectorEvaluationFunctor(size_t N, double x, double a, double b) + : EvaluationFunctor(N, x, a, b) { + calculateJacobian(); + } + + /// M-dimensional evaluation + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + + /// c++ sugar + VectorM operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * VectorComponentFunctor computes the N-vector value for a specific row + * component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class VectorComponentFunctor : public EvaluationFunctor { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorEvaluationFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) + H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); + } + + public: + /// For serialization + VectorComponentFunctor() {} + + /// Construct with row index + VectorComponentFunctor(size_t N, size_t i, double x) + : EvaluationFunctor(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) + : EvaluationFunctor(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + + /// Calculate component of component rowIndex_ of P + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); + } + + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + * + * The difference with the VectorEvaluationFunctor is that after computing the + * M*1 vector xi=F(x;P), with x a scalar and P the M*N parameter vector, we + * also retract xi back to the T manifold. + * For example, if T==Rot3, then we first compute a 3-vector xi using x and P, + * and then map that 3-vector xi back to the Rot3 manifold, yielding a valid + * 3D rotation. + */ + template + class ManifoldEvaluationFunctor + : public VectorEvaluationFunctor::dimension> { + enum { M = traits::dimension }; + using Base = VectorEvaluationFunctor; + + public: + /// For serialization + ManifoldEvaluationFunctor() {} + + /// Default Constructor + ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} + + /// Constructor, with interval [a,b] + ManifoldEvaluationFunctor(size_t N, double x, double a, double b) + : Base(N, x, a, b) {} + + /// Manifold evaluation + T apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + // Interpolate the M-dimensional vector to yield a vector in tangent space + Eigen::Matrix xi = Base::operator()(P, H); + + // Now call retract with this M-vector, possibly with derivatives + Eigen::Matrix D_result_xi; + T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0); + + // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N) + // derivative of interpolation and D_result_xi is MxM derivative of + // retract. + if (H) *H = D_result_xi * (*H); + + // and return a T + return result; + } + + /// c++ sugar + T operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); // might call apply in derived + } + }; + + /// Base class for functors below that calculate derivative weights + class DerivativeFunctorBase { + protected: + Weights weights_; + + public: + /// For serialization + DerivativeFunctorBase() {} + + DerivativeFunctorBase(size_t N, double x) + : weights_(DERIVED::DerivativeWeights(N, x)) {} + + DerivativeFunctorBase(size_t N, double x, double a, double b) + : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {} + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * An instance of a DerivativeFunctor calculates f'(x;p) at a given `x`, + * applied to Parameters `p`. + * When given a scalar value x and a specific N*1 vector of Parameters, + * this functor returns the scalar derivative value of the function at x, + * possibly with Jacobians wrpt the parameters. + */ + class DerivativeFunctor : protected DerivativeFunctorBase { + public: + /// For serialization + DerivativeFunctor() {} + + DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {} + + DerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) {} + + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + if (H) *H = this->weights_; + return (this->weights_ * p)(0); + } + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + return apply(p, H); // might call apply in derived + } + }; + + /** + * VectorDerivativeFunctor at a given x, applied to ParameterMatrix. + * + * This functor is used to evaluate the derivatives of a parameterized + * function at a given scalar value x. When given a specific M*N parameters, + * returns an M-vector the M corresponding function derivatives at x, possibly + * with Jacobians wrpt the parameters. + */ + template + class VectorDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorDerivativeFunctor() {} + + /// Default Constructor + VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { + calculateJacobian(); + } + + /// Constructor, with optional interval [a,b] + VectorDerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) { + calculateJacobian(); + } + + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + /// c++ sugar + VectorM operator()( + const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * ComponentDerivativeFunctor computes the N-vector derivative for a specific + * row component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class ComponentDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorDerivativeFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < this->weights_.size(); j++) + H_(0, rowIndex_ + j * M) = this->weights_(j); + } + + public: + /// For serialization + ComponentDerivativeFunctor() {} + + /// Construct with row index + ComponentDerivativeFunctor(size_t N, size_t i, double x) + : DerivativeFunctorBase(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + /// Calculate derivative of component rowIndex_ of F + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * this->weights_.transpose(); + } + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + // Vector version for MATLAB :-( + static double Derivative(double x, const Vector& p, // + OptionalJacobian H = boost::none) { + return DerivativeFunctor(x)(p.transpose(), H); + } +}; + +} // namespace gtsam diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h new file mode 100644 index 000000000..0b3d4c1a0 --- /dev/null +++ b/gtsam/basis/BasisFactors.h @@ -0,0 +1,424 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file BasisFactors.h + * @brief Factor definitions for various Basis functors. + * @author Varun Agrawal + * @date July 4, 2020 + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Factor for enforcing the scalar value of the polynomial BASIS + * representation at `x` is the same as the measurement `z` when using a + * pseudo-spectral parameterization. + * + * @tparam BASIS The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + EvaluationFactor() {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} + + virtual ~EvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (M, N) is equal to a vector-valued measurement at the same point, + when + * using a pseudo-spectral parameterization. + * + * This factors tries to enforce the basis function evaluation `f(x;p)` to take + * on the value `z` at location `x`, providing a gradient on the parameters p. + * In a probabilistic estimation context, `z` is known as a measurement, and the + * parameterized basis function can be seen as a + * measurement prediction function. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector. + */ +template +class GTSAM_EXPORT VectorEvaluationFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorEvaluationFactor() {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} + + virtual ~VectorEvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (P, N) is equal to specified measurement at the same point, when + * using a pseudo-spectral parameterization. + * + * This factor is similar to `VectorEvaluationFactor` with the key difference + * being that it only enforces the constraint for a single scalar in the vector, + * indexed by `i`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the fixed-size vector. + * + * Example: + * VectorComponentFactor controlPrior(key, measured, model, + * N, i, t, a, b); + * where N is the degree and i is the component index. + */ +template +class GTSAM_EXPORT VectorComponentFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorComponentFactor() {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x) + : Base(key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x)) {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate 0the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { + } + + virtual ~VectorComponentFactor() {} +}; + +/** + * For a measurement value of type T i.e. `T z = g(x)`, this unary + * factor enforces that the polynomial basis, when evaluated at `x`, gives us + * the same `z`, i.e. `T z = g(x) = f(x;p)`. + * + * This is done via computations on the tangent space of the + * manifold of T. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param T: Object type which is synthesized by the provided functor. + * + * Example: + * ManifoldEvaluationFactor rotationFactor(key, measurement, + * model, N, x, a, b); + * + * where `x` is the value (e.g. timestep) at which the rotation was evaluated. + */ +template +class GTSAM_EXPORT ManifoldEvaluationFactor + : public FunctorizedFactor::dimension>> { + private: + using Base = FunctorizedFactor::dimension>>; + + public: + ManifoldEvaluationFactor() {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x, a, b)) { + } + + virtual ~ManifoldEvaluationFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point`x` is equal to the scalar measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT DerivativeFactor + : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + DerivativeFactor() {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {} + + virtual ~DerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point `x` is equal to the vector value `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector derivative. + */ +template +class GTSAM_EXPORT VectorDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template VectorDerivativeFunctor; + + public: + VectorDerivativeFactor() {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, Func(N, x)) {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, Func(N, x, a, b)) {} + + virtual ~VectorDerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial is equal to the scalar value at a specific index `i` of a + * vector-valued measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the control component derivative. + */ +template +class GTSAM_EXPORT ComponentDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template ComponentDerivativeFunctor

; + + public: + ComponentDerivativeFactor() {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x) + : Base(key, z, model, Func(N, i, x)) {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x, double a, double b) + : Base(key, z, model, Func(N, i, x, a, b)) {} + + virtual ~ComponentDerivativeFactor() {} +}; + +} // namespace gtsam diff --git a/gtsam/basis/CMakeLists.txt b/gtsam/basis/CMakeLists.txt new file mode 100644 index 000000000..203fd96a2 --- /dev/null +++ b/gtsam/basis/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB basis_headers "*.h") +install(FILES ${basis_headers} DESTINATION include/gtsam/basis) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/basis/Chebyshev.cpp b/gtsam/basis/Chebyshev.cpp new file mode 100644 index 000000000..3b5825fc3 --- /dev/null +++ b/gtsam/basis/Chebyshev.cpp @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev.cpp + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +/** + * @brief Scale x from [a, b] to [t1, t2] + * + * ((b'-a') * (x - a) / (b - a)) + a' + * + * @param x Value to scale to new range. + * @param a Original lower limit. + * @param b Original upper limit. + * @param t1 New lower limit. + * @param t2 New upper limit. + * @return double + */ +static double scale(double x, double a, double b, double t1, double t2) { + return ((t2 - t1) * (x - a) / (b - a)) + t1; +} + +Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Tx(1, N); + + x = scale(x, a, b, -1, 1); + + Tx(0) = 1; + Tx(1) = x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); + } + return Tx; +} + +Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + Weights weights = Weights::Zero(N); + for (size_t n = 1; n < N; n++) { + weights(n) = n * Ux(n - 1); + } + return weights; +} + +Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Ux(N); + + x = scale(x, a, b, -1, 1); + + Ux(0) = 1; + Ux(1) = 2 * x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2); + } + return Ux; +} + +Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Tx = Chebyshev1Basis::CalculateWeights(N + 1, x, a, b); + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + + Weights weights(N); + + x = scale(x, a, b, -1, 1); + if (x == -1 || x == 1) { + throw std::runtime_error( + "Derivative of Chebyshev2 Basis does not exist at range limits."); + } + + for (size_t n = 0; n < N; n++) { + weights(n) = ((n + 1) * Tx(n + 1) - x * Ux(n)) / (x * x - 1); + } + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h new file mode 100644 index 000000000..d16ccfaac --- /dev/null +++ b/gtsam/basis/Chebyshev.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev.h + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * Basis of Chebyshev polynomials of the first kind + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind + * These are typically denoted with the symbol T_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + */ +struct Chebyshev1Basis : Basis { + using Parameters = Eigen::Matrix; + + Parameters parameters_; + + /** + * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * The derivative weights are pre-multiplied to the polynomial Parameters. + * + * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) + * I.e. the derivative fo a first kind cheb is just a second kind cheb + * So, we define a second kind basis here of order N-1 + * Note that it has one less weight. + * + * The Parameters pertain to 1st kind chebs up to order N-1 + * But of course the first one (order 0) is constant, so omit that weight. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev1Basis + +/** + * Basis of Chebyshev polynomials of the second kind. + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind + * These are typically denoted with the symbol U_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + * In contrast to the templates in Chebyshev2, the classes below specify + * basis functions, weighted combinations of which are used to approximate + * functions. In this sense, they are like the sines and cosines of the Fourier + * basis. + */ +struct Chebyshev2Basis : Basis { + using Parameters = Eigen::Matrix; + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev2Basis + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp new file mode 100644 index 000000000..44876b6e9 --- /dev/null +++ b/gtsam/basis/Chebyshev2.cpp @@ -0,0 +1,205 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev2.cpp + * @brief Chebyshev parameterizations on Chebyshev points of second kind + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weights(N); + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weights.setZero(); + weights(j) = 1; + return weights; + } + distances(j) = dj; + } + + // Beginning of interval, j = 0, x(0) = a + weights(0) = 0.5 / distances(0); + + // All intermediate points j=1:N-2 + double d = weights(0), s = -1; // changes sign s at every iteration + for (size_t j = 1; j < N - 1; j++, s = -s) { + weights(j) = s / distances(j); + d += weights(j); + } + + // End of interval, j = N-1, x(N-1) = b + weights(N - 1) = 0.5 * s / distances(N - 1); + d += weights(N - 1); + + // normalize + return weights / d; +} + +Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weightDerivatives(N); + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weightDerivatives.setZero(); + // compute the jth row of the differentiation matrix for this point + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + for (size_t k = 0; k < N; k++) { + if (j == 0 && k == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (j == N - 1 && k == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (k == j) { + double xj = Point(N, j); + double xj2 = xj * xj; + weightDerivatives(k) = -0.5 * xj / (1 - xj2); + } else { + double xj = Point(N, j); + double xk = Point(N, k); + double ck = (k == 0 || k == N - 1) ? 2. : 1.; + t = ((j + k) % 2) == 0 ? 1 : -1; + weightDerivatives(k) = (cj / ck) * t / (xj - xk); + } + } + return 2 * weightDerivatives / (b - a); + } + distances(j) = dj; + } + + // This section of code computes the derivative of + // the Barycentric Interpolation weights formula by applying + // the chain rule on the original formula. + + // g and k are multiplier terms which represent the derivatives of + // the numerator and denominator + double g = 0, k = 0; + double w = 1; + + for (size_t j = 0; j < N; j++) { + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + w = 1.0; + } + + t = (j % 2 == 0) ? 1 : -1; + + double c = t / distances(j); + g += w * c; + k += (w * c / distances(j)); + } + + double s = 1; // changes sign s at every iteration + double g2 = g * g; + + for (size_t j = 0; j < N; j++, s = -s) { + // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1, + // x0 = 1.0 + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + // All intermediate points j=1:N-2 + w = 1.0; + } + weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) - + (w * -s * k / (g2 * distances(j))); + } + + return weightDerivatives; +} + +Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, + double b) { + DiffMatrix D(N, N); + if (N == 1) { + D(0, 0) = 1; + return D; + } + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + for (size_t i = 0; i < N; i++) { + double xi = Point(N, i); + double ci = (i == 0 || i == N - 1) ? 2. : 1.; + for (size_t j = 0; j < N; j++) { + if (i == 0 && j == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == N - 1 && j == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == j) { + double xi2 = xi * xi; + D(i, j) = -xi / (2 * (1 - xi2)); + } else { + double xj = Point(N, j); + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + t = ((i + j) % 2) == 0 ? 1 : -1; + D(i, j) = (ci / cj) * t / (xi - xj); + } + } + } + // scale the matrix to the range + return D / ((b - a) / 2.0); +} + +Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) { + // Allocate space for weights + Weights weights(N); + size_t K = N - 1, // number of intervals between N points + K2 = K * K; + weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1); + weights(N - 1) = weights(0); + + size_t last_k = K / 2 + K % 2 - 1; + + for (size_t i = 1; i <= N - 2; ++i) { + double theta = i * M_PI / K; + weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; + + for (size_t k = 1; k <= last_k; ++k) + weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1); + weights(i) *= (b - a) / K; + } + + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h new file mode 100644 index 000000000..28590961d --- /dev/null +++ b/gtsam/basis/Chebyshev2.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Chebyshev2.h + * @brief Pseudo-spectral parameterization for Chebyshev polynomials of the + * second kind. + * + * In a pseudo-spectral case, rather than the parameters acting as + * weights for the bases polynomials (as in Chebyshev2Basis), here the + * parameters are the *values* at a specific set of points in the interval, the + * "Chebyshev points". These values uniquely determine the polynomial that + * interpolates them at the Chebyshev points. + * + * This is different from Chebyshev.h since it leverage ideas from + * pseudo-spectral optimization, i.e. we don't decompose into basis functions, + * rather estimate function parameters that enforce function nodes at Chebyshev + * points. + * + * Please refer to Agrawal21icra for more details. + * + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Chebyshev Interpolation on Chebyshev points of the second kind + * Note that N here, the number of points, is one less than N from + * 'Approximation Theory and Approximation Practice by L. N. Trefethen (pg.42)'. + */ +class GTSAM_EXPORT Chebyshev2 : public Basis { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using Base = Basis; + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /// Specific Chebyshev point + static double Point(size_t N, int j) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(- M_PI_2 + dtheta*j); also works + return cos(-M_PI + dtheta * j); + } + + /// Specific Chebyshev point, within [a,b] interval + static double Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N - 1); + // We add -PI so that we get values ordered from -1 to +1 + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; + } + + /// All Chebyshev points + static Vector Points(size_t N) { + Vector points(N); + for (size_t j = 0; j < N; j++) points(j) = Point(N, j); + return points; + } + + /// All Chebyshev points, within [a,b] interval + static Vector Points(size_t N, double a, double b) { + Vector points = Points(N); + const double T1 = (a + b) / 2, T2 = (b - a) / 2; + points = T1 + (T2 * points).array(); + return points; + } + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + * These weights implement barycentric interpolation at a specific x. + * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the + * values of the function f at the Chebyshev points. As such, for a given x we + * obtain a linear map from parameter vectors f to interpolated values f(x). + * Optional [a,b] interval can be specified as well. + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * Evaluate derivative of barycentric weights. + * This is easy and efficient via the DifferentiationMatrix. + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); + + /// compute D = differentiation matrix, Trefethen00book p.53 + /// when given a parameter vector f of function values at the Chebyshev + /// points, D*f are the values of f'. + /// https://people.maths.ox.ac.uk/trefethen/8all.pdf Theorem 8.4 + static DiffMatrix DifferentiationMatrix(size_t N, double a = -1, + double b = 1); + + /** + * Evaluate Clenshaw-Curtis integration weights. + * Trefethen00book, pg 128, clencurt.m + * Note that N in clencurt.m is 1 less than our N + * K = N-1; + theta = pi*(0:K)'/K; + w = zeros(1,N); ii = 2:K; v = ones(K-1, 1); + if mod(K,2) == 0 + w(1) = 1/(K^2-1); w(N) = w(1); + for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + v = v - cos(K*theta(ii))/(K^2-1); + else + w(1) = 1/K^2; w(N) = w(1); + for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + end + w(ii) = 2*v/K; + + */ + static Weights IntegrationWeights(size_t N, double a = -1, double b = 1); + + /** + * Create matrix of values at Chebyshev points given vector-valued function. + */ + template + static Matrix matrix(boost::function(double)> f, + size_t N, double a = -1, double b = 1) { + Matrix Xmat(M, N); + for (size_t j = 0; j < N; j++) { + Xmat.col(j) = f(Point(N, j, a, b)); + } + return Xmat; + } +}; // \ Chebyshev2 + +} // namespace gtsam diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h new file mode 100644 index 000000000..f5cb99bd7 --- /dev/null +++ b/gtsam/basis/FitBasis.h @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FitBasis.h + * @date July 4, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Fit a Basis using least-squares + */ + +/* + * Concept needed for LS. Parameters = Coefficients | Values + * - Parameters, Jacobian + * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H) + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Our sequence representation is a map of {x: y} values where y = f(x) +using Sequence = std::map; +/// A sample is a key-value pair from a sequence. +using Sample = std::pair; + +/** + * Class that does regression via least squares + * Example usage: + * size_t N = 3; + * auto fit = FitBasis(data_points, noise_model, N); + * Vector coefficients = fit.parameters(); + * + * where `data_points` are a map from `x` to `y` values indicating a function + * mapping at specific points, `noise_model` is the gaussian noise model, and + * `N` is the degree of the polynomial basis used to fit the function. + */ +template +class FitBasis { + public: + using Parameters = typename Basis::Parameters; + + private: + Parameters parameters_; + + public: + /// Create nonlinear FG from Sequence + static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, + const SharedNoiseModel& model, + size_t N) { + NonlinearFactorGraph graph; + for (const Sample sample : sequence) { + graph.emplace_shared>(0, sample.second, model, N, + sample.first); + } + return graph; + } + + /// Create linear FG from Sequence + static GaussianFactorGraph::shared_ptr LinearGraph( + const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N); + Values values; + values.insert(0, Parameters::Zero(N)); + GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + return gfg; + } + + /** + * @brief Construct a new FitBasis object. + * + * @param sequence map of x->y values for a function, a.k.a. y = f(x). + * @param model The noise model to use. + * @param N The degree of the polynomial to fit. + */ + FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N); + VectorValues solution = gfg->optimize(); + parameters_ = solution.at(0); + } + + /// Return Fourier coefficients + Parameters parameters() const { return parameters_; } +}; + +} // namespace gtsam diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h new file mode 100644 index 000000000..d264e182d --- /dev/null +++ b/gtsam/basis/Fourier.h @@ -0,0 +1,112 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Fourier.h + * @brief Fourier decomposition, see e.g. + * http://mathworld.wolfram.com/FourierSeries.html + * @author Varun Agrawal, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include + +namespace gtsam { + +/// Fourier basis +class GTSAM_EXPORT FourierBasis : public Basis { + public: + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x) { + Weights b(N); + b[0] = 1; + for (size_t i = 1, n = 1; i < N; i++) { + if (i % 2 == 1) { + b[i] = cos(n * x); + } else { + b[i] = sin(n * x); + n++; + } + } + return b; + } + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x, double a, double b) { + // TODO(Varun) How do we enforce an interval for Fourier series? + return CalculateWeights(N, x); + } + + /** + * Compute D = differentiation matrix. + * Given coefficients c of a Fourier series c, D*c are the values of c'. + */ + static DiffMatrix DifferentiationMatrix(size_t N) { + DiffMatrix D = DiffMatrix::Zero(N, N); + double k = 1; + for (size_t i = 1; i < N; i += 2) { + D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x) + D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x) + k += 1; + } + + return D; + } + + /** + * @brief Get weights at a given x that calculate the derivative. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x) { + return CalculateWeights(N, x) * DifferentiationMatrix(N); + } + + /** + * @brief Get derivative weights at a given x that calculate the derivative, + in the interval [a, b]. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a, double b) { + return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N); + } + +}; // FourierBasis + +} // namespace gtsam diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h new file mode 100644 index 000000000..df2d9f62e --- /dev/null +++ b/gtsam/basis/ParameterMatrix.h @@ -0,0 +1,215 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ParamaterMatrix.h + * @brief Define ParameterMatrix class which is used to store values at + * interpolation points. + * @author Varun Agrawal, Frank Dellaert + * @date September 21, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * A matrix abstraction of MxN values at the Basis points. + * This class serves as a wrapper over an Eigen matrix. + * @tparam M: The dimension of the type you wish to evaluate. + * @param N: the number of Basis points (e.g. Chebyshev points of the second + * kind). + */ +template +class ParameterMatrix { + using MatrixType = Eigen::Matrix; + + private: + MatrixType matrix_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum { dimension = Eigen::Dynamic }; + + /** + * Create ParameterMatrix using the number of basis points. + * @param N: The number of basis points (the columns). + */ + ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); } + + /** + * Create ParameterMatrix from an MxN Eigen Matrix. + * @param matrix: An Eigen matrix used to initialze the ParameterMatrix. + */ + ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {} + + /// Get the number of rows. + size_t rows() const { return matrix_.rows(); } + + /// Get the number of columns. + size_t cols() const { return matrix_.cols(); } + + /// Get the underlying matrix. + MatrixType matrix() const { return matrix_; } + + /// Return the tranpose of the underlying matrix. + Eigen::Matrix transpose() const { return matrix_.transpose(); } + + /** + * Get the matrix row specified by `index`. + * @param index: The row index to retrieve. + */ + Eigen::Matrix row(size_t index) const { + return matrix_.row(index); + } + + /** + * Set the matrix row specified by `index`. + * @param index: The row index to set. + */ + auto row(size_t index) -> Eigen::Block { + return matrix_.row(index); + } + + /** + * Get the matrix column specified by `index`. + * @param index: The column index to retrieve. + */ + Eigen::Matrix col(size_t index) const { + return matrix_.col(index); + } + + /** + * Set the matrix column specified by `index`. + * @param index: The column index to set. + */ + auto col(size_t index) -> Eigen::Block { + return matrix_.col(index); + } + + /** + * Set all matrix coefficients to zero. + */ + void setZero() { matrix_.setZero(); } + + /** + * Add a ParameterMatrix to another. + * @param other: ParameterMatrix to add. + */ + ParameterMatrix operator+(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ + other.matrix()); + } + + /** + * Add a MxN-sized vector to the ParameterMatrix. + * @param other: Vector which is reshaped and added. + */ + ParameterMatrix operator+( + const Eigen::Matrix& other) const { + // This form avoids a deep copy and instead typecasts `other`. + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ + other_); + } + + /** + * Subtract a ParameterMatrix from another. + * @param other: ParameterMatrix to subtract. + */ + ParameterMatrix operator-(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ - other.matrix()); + } + + /** + * Subtract a MxN-sized vector from the ParameterMatrix. + * @param other: Vector which is reshaped and subracted. + */ + ParameterMatrix operator-( + const Eigen::Matrix& other) const { + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ - other_); + } + + /** + * Multiply ParameterMatrix with an Eigen matrix. + * @param other: Eigen matrix which should be multiplication compatible with + * the ParameterMatrix. + */ + MatrixType operator*(const Eigen::Matrix& other) const { + return matrix_ * other; + } + + /// @name Vector Space requirements, following LieMatrix + /// @{ + + /** + * Print the ParameterMatrix. + * @param s: The prepend string to add more contextual info. + */ + void print(const std::string& s = "") const { + std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl; + } + + /** + * Check for equality up to absolute tolerance. + * @param other: The ParameterMatrix to check equality with. + * @param tol: The absolute tolerance threshold. + */ + bool equals(const ParameterMatrix& other, double tol = 1e-8) const { + return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol); + } + + /// Returns dimensionality of the tangent space + inline size_t dim() const { return matrix_.size(); } + + /// Convert to vector form, is done row-wise + inline Vector vector() const { + using RowMajor = Eigen::Matrix; + Vector result(matrix_.size()); + Eigen::Map(&result(0), rows(), cols()) = matrix_; + return result; + } + + /** Identity function to satisfy VectorSpace traits. + * + * NOTE: The size at compile time is unknown so this identity is zero + * length and thus not valid. + */ + inline static ParameterMatrix identity() { + // throw std::runtime_error( + // "ParameterMatrix::identity(): Don't use this function"); + return ParameterMatrix(0); + } + + /// @} +}; + +// traits for ParameterMatrix +template +struct traits> + : public internal::VectorSpace> {}; + +/* ************************************************************************* */ +// Stream operator that takes a ParameterMatrix. Used for printing. +template +inline std::ostream& operator<<(std::ostream& os, + const ParameterMatrix& parameterMatrix) { + os << parameterMatrix.matrix(); + return os; +} + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i new file mode 100644 index 000000000..8f06fd2e1 --- /dev/null +++ b/gtsam/basis/basis.i @@ -0,0 +1,146 @@ +//************************************************************************* +// basis +//************************************************************************* + +namespace gtsam { + +// TODO(gerry): add all the Functors to the Basis interfaces, e.g. +// `EvaluationFunctor` + +#include + +class FourierBasis { + static Vector CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); + + static Matrix DifferentiationMatrix(size_t N); + static Vector DerivativeWeights(size_t N, double x); +}; + +#include + +class Chebyshev1Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector X); +}; + +class Chebyshev2Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); +}; + +#include +class Chebyshev2 { + static double Point(size_t N, int j); + static double Point(size_t N, int j, double a, double b); + + static Vector Points(size_t N); + static Vector Points(size_t N, double a, double b); + + static Matrix WeightMatrix(size_t N, Vector X); + static Matrix WeightMatrix(size_t N, Vector X, double a, double b); + + static Matrix CalculateWeights(size_t N, double x, double a, double b); + static Matrix DerivativeWeights(size_t N, double x, double a, double b); + static Matrix IntegrationWeights(size_t N, double a, double b); + static Matrix DifferentiationMatrix(size_t N, double a, double b); + + // TODO Needs OptionalJacobian + // static double Derivative(double x, Vector f); +}; + +#include + +template +class ParameterMatrix { + ParameterMatrix(const size_t N); + ParameterMatrix(const Matrix& matrix); + + Matrix matrix() const; + + void print(const string& s = "") const; +}; + +#include + +template +virtual class EvaluationFactor : gtsam::NoiseModelFactor { + EvaluationFactor(); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +template +virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { + VectorEvaluationFactor(); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(Varun) Better way to support arbitrary dimensions? +// Especially if users mainly do `pip install gtsam` for the Python wrapper. +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D3; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D4; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D12; + +template +virtual class VectorComponentFactor : gtsam::NoiseModelFactor { + VectorComponentFactor(); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x, double a, double b); +}; + +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D3; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D4; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D12; + +template +virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { + ManifoldEvaluationFactor(); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and +// `ComponentDerivativeFactor` + +#include +template +class FitBasis { + FitBasis(const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + + static gtsam::NonlinearFactorGraph NonlinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + Parameters parameters() const; +}; + +} // namespace gtsam diff --git a/gtsam/basis/tests/CMakeLists.txt b/gtsam/basis/tests/CMakeLists.txt new file mode 100644 index 000000000..63cad0be6 --- /dev/null +++ b/gtsam/basis/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(basis "test*.cpp" "" "gtsam") diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp new file mode 100644 index 000000000..64c925886 --- /dev/null +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -0,0 +1,236 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +const size_t N = 3; + +//****************************************************************************** +TEST(Chebyshev, Chebyshev1) { + using Synth = Chebyshev1Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Chebyshev2) { + using Synth = Chebyshev2Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Evaluation) { + Chebyshev1Basis::EvaluationFunctor fx(N, 0.5); + Vector c(N); + c << 3, 5, -12; + EXPECT_DOUBLES_EQUAL(11.5, fx(c), 1e-9); +} + +//****************************************************************************** +#include +#include +TEST(Chebyshev, Expression) { + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + + // Let's pretend we have 6 GPS measurements (we just do x coordinate) + // at times + const size_t m = 6; + Vector t(m); + t << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + Vector x(m); + x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + + for (size_t i = 0; i < m; i++) { + graph.emplace_shared>(key, x(i), model, N, + t(i)); + } + + // Solve + Values initial; + initial.insert(key, Vector::Zero(N)); // initial does not matter + + // ... and optimize + GaussNewtonParams parameters; + GaussNewtonOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check + Vector expected(N); + expected << 0, 1, 0; + Vector actual_c = result.at(key); + EXPECT(assert_equal(expected, actual_c)); + + // Calculate and print covariances + Marginals marginals(graph, result); + Matrix3 cov = marginals.marginalCovariance(key); + EXPECT_DOUBLES_EQUAL(0.626, cov(1, 1), 1e-3); + + // Predict x at time 1.0 + Chebyshev1Basis::EvaluationFunctor f(N, 1.0); + Matrix H; + double actual = f(actual_c, H); + EXPECT_DOUBLES_EQUAL(1.0, actual, 1e-9); + + // Calculate predictive variance on prediction + double actual_variance_on_prediction = (H * cov * H.transpose())(0); + EXPECT_DOUBLES_EQUAL(1.1494, actual_variance_on_prediction, 1e-4); +} + +//****************************************************************************** +TEST(Chebyshev, Decomposition) { + const size_t M = 16; + + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < M; i++) { + double x = ((double)i / M); // - 0.99; + double y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, N); + + // Check + Vector expected = Vector::Zero(N); + expected(1) = 1; + EXPECT(assert_equal(expected, (Vector)actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev1, Derivative) { + Vector c(N); + c << 12, 3, 2; + + Weights D; + + double x = -1.0; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-5, (D * c)(0), 1e-9); + + x = -0.5; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-1, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(5.4, (D * c)(0), 1e-9); +} + +//****************************************************************************** +Vector3 f(-6, 1, 0.5); + +double proxy1(double x, size_t N) { + return Chebyshev1Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev1, Derivative2) { + const double x = 0.5; + auto D = Chebyshev1Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy1, x, N); + // regression + EXPECT_DOUBLES_EQUAL(2, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(2, (D * f)(0), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev2, Derivative) { + Vector c(N); + c << 12, 6, 2; + + Weights D; + + double x = -1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + x = 1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + + x = -0.5; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(4, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(16.8, (D * c)(0), 1e-9); + + x = 0.75; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(24, (D * c)(0), 1e-9); + + x = 10; + D = Chebyshev2Basis::DerivativeWeights(N, x, 0, 20); + // regression + EXPECT_DOUBLES_EQUAL(12, (D * c)(0), 1e-9); +} + +//****************************************************************************** +double proxy2(double x, size_t N) { + return Chebyshev2Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev2, Derivative2) { + const double x = 0.5; + auto D = Chebyshev2Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy2, x, N); + // regression + EXPECT_DOUBLES_EQUAL(4, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(4, (D * f)(0), 1e-9); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp new file mode 100644 index 000000000..4cee70daf --- /dev/null +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -0,0 +1,435 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral + * methods + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::placeholders; + +noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); + +const size_t N = 32; + +//****************************************************************************** +TEST(Chebyshev2, Point) { + static const int N = 5; + auto points = Chebyshev2::Points(N); + Vector expected(N); + expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // Check symmetry + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 0), -Chebyshev2::Point(N, 4), tol); + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 1), -Chebyshev2::Point(N, 3), tol); +} + +//****************************************************************************** +TEST(Chebyshev2, PointInInterval) { + static const int N = 5; + auto points = Chebyshev2::Points(N, 0, 20); + Vector expected(N); + expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.; + expected *= 10.0; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // all at once + Vector actual = Chebyshev2::Points(N, 0, 20); + CHECK(assert_equal(expected, actual)); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5] +TEST(Chebyshev2, Interpolate2) { + size_t N = 3; + Chebyshev2::EvaluationFunctor fx(N, 0.5); + Vector f(N); + f << 4, 2, 6; + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5] +TEST(Chebyshev2, Interpolate2_Interval) { + Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2); + Vector3 f(4, 2, 6); + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1, +// 3}}, 0.5] +TEST(Chebyshev2, Interpolate5) { + Chebyshev2::EvaluationFunctor fx(5, 0.5); + Vector f(5); + f << 4, 2, 6, 3, 3; + EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5); +} + +//****************************************************************************** +// Interpolating vectors +TEST(Chebyshev2, InterpolateVector) { + double t = 30, a = 0, b = 100; + const size_t N = 3; + // Create 2x3 matrix with Vectors at Chebyshev points + ParameterMatrix<2> X(N); + X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp + + // Check value + Vector expected(2); + expected << t, 0; + Eigen::Matrix actualH(2, 2 * N); + + Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); + EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); + + // Check derivative + boost::function)> f = boost::bind( + &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); + Matrix numericalH = + numericalDerivative11, 2 * N>(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +//****************************************************************************** +TEST(Chebyshev2, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = (double)i / 16. - 0.99, y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, 3); + + // Check + Vector expected(3); + expected << -1, 0, 1; + EXPECT(assert_equal(expected, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DifferentiationMatrix3) { + // Trefethen00book, p.55 + const size_t N = 3; + Matrix expected(N, N); + // Differentiation matrix computed from Chebfun + expected << 1.5000, -2.0000, 0.5000, // + 0.5000, -0.0000, -0.5000, // + -0.5000, 2.0000, -1.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DerivativeMatrix6) { + // Trefethen00book, p.55 + const size_t N = 6; + Matrix expected(N, N); + expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000, // + 2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764, // + -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820, // + 0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236, // + -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // + 0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal((Matrix)expected, actual, 1e-4)); +} + +// test function for CalculateWeights and DerivativeWeights +double f(double x) { + // return 3*(x**3) - 2*(x**2) + 5*x - 11 + return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11; +} + +// its derivative +double fprime(double x) { + // return 9*(x**2) - 4*(x) + 5 + return 9.0 * pow(x, 2) - 4.0 * x + 5.0; +} + +//****************************************************************************** +TEST(Chebyshev2, CalculateWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376; + Weights weights1 = Chebyshev2::CalculateWeights(N, x1); + Weights weights2 = Chebyshev2::CalculateWeights(N, x2); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + EXPECT_DOUBLES_EQUAL(f(x2), weights2 * fvals, 1e-8); +} + +TEST(Chebyshev2, CalculateWeights2) { + double a = 0, b = 10, x1 = 7, x2 = 4.12; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + + Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b); + double expected2 = f(x2); // 185.454784 + double actual2 = weights2 * fvals; + EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8); +} + +TEST(Chebyshev2, DerivativeWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376, x3 = 0.0; + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9); + + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9); + + // test if derivative calculation and cheb point is correct + double x4 = Chebyshev2::Point(N, 3); + Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4); + EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9); +} + +TEST(Chebyshev2, DerivativeWeights2) { + double x1 = 5, x2 = 4.12, a = 0, b = 10; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); + + // test if derivative calculation and cheb point is correct + double x3 = Chebyshev2::Point(N, 3, a, b); + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) { + const size_t N6 = 6; + double x1 = 0.311; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6; + Weights actual = Chebyshev2::DerivativeWeights(N6, x1); + EXPECT(assert_equal(expected, actual, 1e-12)); + + double a = -3, b = 8, x2 = 5.05; + Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b); + Weights expected1 = Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2; + Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b); + EXPECT(assert_equal(expected1, actual1, 1e-12)); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights6) { + const size_t N6 = 6; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Chebyshev2::Parameters x = Chebyshev2::Points(N6); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x))); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights7) { + const size_t N7 = 7; + Matrix D7 = Chebyshev2::DifferentiationMatrix(N7); + Chebyshev2::Parameters x = Chebyshev2::Points(N7); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x))); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished(); +double proxy3(double x) { + return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6) { + // Check Derivative evaluation at point x=0.2 + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy3, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Assert that derivative also works in non-standard interval [0,3] +double proxy4(double x) { + return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6_03) { + // Check Derivative evaluation at point x=0.2, in interval [0,3] + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy4, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor +TEST(Chebyshev2, VectorDerivativeFunctor) { + const size_t N = 3, M = 2; + const double x = 0.2; + using VecD = Chebyshev2::VectorDerivativeFunctor; + VecD fx(N, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(M, M * N); + EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); + + // Test Jacobian + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor with polynomial function +TEST(Chebyshev2, VectorDerivativeFunctor2) { + const size_t N = 64, M = 1, T = 15; + using VecD = Chebyshev2::VectorDerivativeFunctor; + + const Vector points = Chebyshev2::Points(N, 0, T); + + // Assign the parameter matrix + Vector values(N); + for (size_t i = 0; i < N; ++i) { + values(i) = f(points(i)); + } + ParameterMatrix X(values); + + // Evaluate the derivative at the chebyshev points using + // VectorDerivativeFunctor. + for (size_t i = 0; i < N; ++i) { + VecD d(N, points(i), 0, T); + Vector1 Dx = d(X); + EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); + } + + // Test Jacobian at the first chebyshev point. + Matrix actualH(M, M * N); + VecD vecd(N, points(0), 0, T); + vecd(X, actualH); + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), vecd, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-6)); +} + +//****************************************************************************** +// Test ComponentDerivativeFunctor +TEST(Chebyshev2, ComponentDerivativeFunctor) { + const size_t N = 6, M = 2; + const double x = 0.2; + using CompFunc = Chebyshev2::ComponentDerivativeFunctor; + size_t row = 1; + CompFunc fx(N, row, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(1, M * N); + EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); + + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +TEST(Chebyshev2, IntegralWeights) { + const size_t N7 = 7; + Vector actual = Chebyshev2::IntegrationWeights(N7); + Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254, + 0.457142857142857, 0.520634920634921, 0.457142857142857, + 0.253968253968254, 0.0285714285714286) + .finished(); + EXPECT(assert_equal(expected, actual)); + + const size_t N8 = 8; + Vector actual2 = Chebyshev2::IntegrationWeights(N8); + Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, + 0.352242423718159, 0.437208405798326, 0.437208405798326, + 0.352242423718159, 0.190141007218208, 0.0204081632653061) + .finished(); + EXPECT(assert_equal(expected2, actual2)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp new file mode 100644 index 000000000..7a53cfcc9 --- /dev/null +++ b/gtsam/basis/tests/testFourier.cpp @@ -0,0 +1,254 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testFourier.cpp + * @date July 4, 2020 + * @author Frank Dellaert, Varun Agrawal + * @brief Unit tests for Fourier Basis Decompositions with Expressions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +// Coefficients for testing, respectively 3 and 7 parameter Fourier basis. +// They correspond to best approximation of TestFunction(x) +const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished(); +const Vector7 k7Coefficients = + (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943) + .finished(); + +// The test-function used below +static double TestFunction(double x) { return exp(sin(x) + cos(x)); } + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctor) { + // fx(0) takes coefficients c to calculate the value f(c;x==0) + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients), 1e-9); +} + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctorDerivative) { + // If we give the H argument, we get the derivative of fx(0) wrpt coefficients + // Needs to be Matrix so it can be used by OptionalJacobian. + Matrix H(1, 3); + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients, H), 1e-9); + + Matrix13 expectedH(1, 1, 0); + EXPECT(assert_equal(expectedH, H)); +} + +//****************************************************************************** +TEST(Basis, Manual) { + const size_t N = 3; + + // We will create a linear factor graph + GaussianFactorGraph graph; + + // We create an unknown Vector expression for the coefficients + Key key(1); + + // We will need values below to test the Jacobians + Values values; + values.insert(key, Vector::Zero(N)); // value does not really matter + + // At 16 different samples points x, check Predict_ expression + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, N); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A, b); + graph.add(linearFactor); + + // Create factor to predict value at x + EvaluationFactor predictFactor(key, desiredValue, model, N, + x); + + // Check expression Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9); + + auto linearizedFactor = predictFactor.linearize(values); + auto linearizedJacobianFactor = + boost::dynamic_pointer_cast(linearizedFactor); + CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor + EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9)); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, EvaluationFactor) { + // Check fitting a function with a 7-parameter Fourier basis + + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, desiredValue = TestFunction(x); + graph.emplace_shared>(key, desiredValue, + model, 7, x); + } + + // Solve FourierFactorGraph + Values values; + values.insert(key, Vector::Zero(7)); + GaussianFactorGraph::shared_ptr lfg = graph.linearize(values); + VectorValues actual = lfg->optimize(); + + EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, WeightMatrix) { + // The WeightMatrix creates an m*n matrix, where m is the number of sample + // points, and n is the number of parameters. + Matrix expected(2, 3); + expected.row(0) << 1, cos(1), sin(1); + expected.row(1) << 1, cos(2), sin(2); + Vector2 X(1, 2); + Matrix actual = FourierBasis::WeightMatrix(3, X); + EXPECT(assert_equal(expected, actual, 1e-9)); +} + +//****************************************************************************** +TEST(Basis, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, y = TestFunction(x); + sequence[x] = y; + } + + // Do Fourier Decomposition + FitBasis actual(sequence, model, 3); + + // Check + EXPECT(assert_equal((Vector)k3Coefficients, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +double proxy(double x) { + return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients); +} + +TEST(Basis, Derivative7) { + // Check Derivative evaluation at point x=0.2 + + // Calculate expected values by numerical derivative of proxy. + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy, x); + + // Calculate derivatives at Chebyshev points using D7, interpolate + Matrix D7 = FourierBasis::DifferentiationMatrix(7); + Vector derivativeCoefficients = D7 * k7Coefficients; + FourierBasis::EvaluationFunctor fx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8); + + // Do directly + FourierBasis::DerivativeFunctor dfdx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8); +} + +//****************************************************************************** +TEST(Basis, VecDerivativeFunctor) { + using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>; + const size_t N = 3; + + // MATLAB example, Dec 27 2019, commit 014eded5 + double h = 2 * M_PI / 16; + Vector2 dotShape(0.5556, -0.8315); // at h/2 + DotShape dotShapeFunction(N, h / 2); + Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) + .finished() + .transpose(); + ParameterMatrix<2> theta(theta_mat); + EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); +} + +//****************************************************************************** +// Suppose we want to parameterize a periodic function with function values at +// specific times, rather than coefficients. Can we do it? This would be a +// generalization of the Fourier transform, and constitute a "pseudo-spectral" +// parameterization. One way to do this is to establish hard constraints that +// express the relationship between the new parameters and the coefficients. +// For example, below I'd like the parameters to be the function values at +// X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients. +// Because the values f(X) = at these points can be written as f(X) = W(X)*c, +// we can simply express the coefficents c as c=inv(W(X))*f, which is a +// generalized Fourier transform. That also means we can create factors with the +// unknown f-values, as done manually below. +TEST(Basis, PseudoSpectral) { + // We will create a linear factor graph + GaussianFactorGraph graph; + + const size_t N = 3; + const Key key(1); + + // The correct values at X = {0.1,0.2,0.3} are simply W*c + const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); + const Matrix W = FourierBasis::WeightMatrix(N, X); + const Vector expected = W * k3Coefficients; + + // Check those values are indeed correct values of Fourier approximation + using Eval = FourierBasis::EvaluationFunctor; + EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); + + // Calculate "inverse Fourier transform" matrix + const Matrix invW = W.inverse(); + + // At 16 different samples points x, add a factor on fExpr + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, 3); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A * invW, b); + graph.add(linearFactor); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp new file mode 100644 index 000000000..ec62e8eea --- /dev/null +++ b/gtsam/basis/tests/testParameterMatrix.cpp @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testParameterMatrix.cpp + * @date Sep 22, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Unit tests for ParameterMatrix.h + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +using Parameters = Chebyshev2::Parameters; + +const size_t M = 2, N = 5; + +//****************************************************************************** +TEST(ParameterMatrix, Constructor) { + ParameterMatrix<2> actual1(3); + ParameterMatrix<2> expected1(Matrix::Zero(2, 3)); + EXPECT(assert_equal(expected1, actual1)); + + ParameterMatrix<2> actual2(Matrix::Ones(2, 3)); + ParameterMatrix<2> expected2(Matrix::Ones(2, 3)); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal(expected2.matrix(), actual2.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Dimensions) { + ParameterMatrix params(N); + EXPECT_LONGS_EQUAL(params.rows(), M); + EXPECT_LONGS_EQUAL(params.cols(), N); + EXPECT_LONGS_EQUAL(params.dim(), M * N); +} + +//****************************************************************************** +TEST(ParameterMatrix, Getters) { + ParameterMatrix params(N); + + Matrix expectedMatrix = Matrix::Zero(2, 5); + EXPECT(assert_equal(expectedMatrix, params.matrix())); + + Matrix expectedMatrixTranspose = Matrix::Zero(5, 2); + EXPECT(assert_equal(expectedMatrixTranspose, params.transpose())); + + ParameterMatrix p2(Matrix::Ones(M, N)); + Vector expectedRowVector = Vector::Ones(N); + for (size_t r = 0; r < M; ++r) { + EXPECT(assert_equal(p2.row(r), expectedRowVector)); + } + + ParameterMatrix p3(2 * Matrix::Ones(M, N)); + Vector expectedColVector = 2 * Vector::Ones(M); + for (size_t c = 0; c < M; ++c) { + EXPECT(assert_equal(p3.col(c), expectedColVector)); + } +} + +//****************************************************************************** +TEST(ParameterMatrix, Setters) { + ParameterMatrix params(Matrix::Zero(M, N)); + Matrix expected = Matrix::Zero(M, N); + + // row + params.row(0) = Vector::Ones(N); + expected.row(0) = Vector::Ones(N); + EXPECT(assert_equal(expected, params.matrix())); + + // col + params.col(2) = Vector::Ones(M); + expected.col(2) = Vector::Ones(M); + + EXPECT(assert_equal(expected, params.matrix())); + + // setZero + params.setZero(); + expected.setZero(); + EXPECT(assert_equal(expected, params.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Addition) { + ParameterMatrix params(Matrix::Ones(M, N)); + ParameterMatrix expected(2 * Matrix::Ones(M, N)); + + // Add vector + EXPECT(assert_equal(expected, params + Vector::Ones(M * N))); + // Add another ParameterMatrix + ParameterMatrix actual = params + ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Subtraction) { + ParameterMatrix params(2 * Matrix::Ones(M, N)); + ParameterMatrix expected(Matrix::Ones(M, N)); + + // Subtract vector + EXPECT(assert_equal(expected, params - Vector::Ones(M * N))); + // Subtract another ParameterMatrix + ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Multiplication) { + ParameterMatrix params(Matrix::Ones(M, N)); + Matrix multiplier = 2 * Matrix::Ones(N, 2); + Matrix expected = 2 * N * Matrix::Ones(M, 2); + EXPECT(assert_equal(expected, params * multiplier)); +} + +//****************************************************************************** +TEST(ParameterMatrix, VectorSpace) { + ParameterMatrix params(Matrix::Ones(M, N)); + // vector + EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); + // identity + EXPECT(assert_equal(ParameterMatrix::identity(), + ParameterMatrix(Matrix::Zero(M, 0)))); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 9d1bd4ebd..e7623c52b 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -77,3 +77,9 @@ // Support Metis-based nested dissection #cmakedefine GTSAM_TANGENT_PREINTEGRATION + +// Whether to use the system installed Metis instead of the provided one +#cmakedefine GTSAM_USE_SYSTEM_METIS + +// Toggle switch for BetweenFactor jacobian computation +#cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 7d2f818fa..143d4bc3c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -147,51 +147,149 @@ public: * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * Fixed size version - */ - template // N = 2 or 3, ND is the camera dimension - static SymmetricBlockMatrix SchurComplement( - const std::vector< Eigen::Matrix, Eigen::aligned_allocator< Eigen::Matrix > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + */ + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix SchurComplement( + const std::vector< + Eigen::Matrix, + Eigen::aligned_allocator>>& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + // a single point is observed in m cameras + size_t m = Fs.size(); - // a single point is observed in m cameras - size_t m = Fs.size(); + // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) + size_t M1 = ND * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, ND); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); - // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) - size_t M1 = ND * m + 1; - std::vector dims(m + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, ND); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera - // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera + const Eigen::Matrix& Fi = Fs[i]; + const auto FiT = Fi.transpose(); + const Eigen::Matrix Ei_P = // + E.block(ZDim * i, 0, ZDim, N) * P; - const Eigen::Matrix& Fi = Fs[i]; - const auto FiT = Fi.transpose(); - const Eigen::Matrix Ei_P = // - E.block(ZDim * i, 0, ZDim, N) * P; + // D = (Dx2) * ZDim + augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * i) // F' * b + - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) - // D = (Dx2) * ZDim - augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * i) // F' * b - - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian.setDiagonalBlock(i, FiT + * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian.setDiagonalBlock(i, FiT - * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const Eigen::Matrix& Fj = Fs[j]; - // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera - const Eigen::Matrix& Fj = Fs[j]; + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian.setOffDiagonalBlock(i, j, -FiT + * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); + } + } // end of for over cameras - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian.setOffDiagonalBlock(i, j, -FiT - * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); + augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); + return augmentedHessian; + } + + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * G = F' * F - F' * E * P * E' * F + * g = F' * (b - E * P * E' * b) + * In this version, we allow for the case where the keys in the Jacobian are + * organized differently from the keys in the output SymmetricBlockMatrix In + * particular: each diagonal block of the Jacobian F captures 2 poses (useful + * for rolling shutter and extrinsic calibration) such that F keeps the block + * structure that makes the Schur complement trick fast. + * + * N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is + * the Hessian block dimension + */ + template + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( + const std::vector< + Eigen::Matrix, + Eigen::aligned_allocator>>& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b, + const KeyVector& jacobianKeys, const KeyVector& hessianKeys) { + size_t nrNonuniqueKeys = jacobianKeys.size(); + size_t nrUniqueKeys = hessianKeys.size(); + + // Marginalize point: note - we reuse the standard SchurComplement function. + SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs, E, P, b); + + // Pack into an Hessian factor, allow space for b term. + std::vector dims(nrUniqueKeys + 1); + std::fill(dims.begin(), dims.end() - 1, NDD); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessianUniqueKeys; + + // Deal with the fact that some blocks may share the same keys. + if (nrUniqueKeys == nrNonuniqueKeys) { + // Case when there is 1 calibration key per camera: + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix(augmentedHessian.selfadjointView())); + } else { + // When multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix. + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // includes b + std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); + nonuniqueDims.back() = 1; + augmentedHessian = SymmetricBlockMatrix( + nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); + + // Get map from key to location in the new augmented Hessian matrix (the + // one including only unique keys). + std::map keyToSlotMap; + for (size_t k = 0; k < nrUniqueKeys; k++) { + keyToSlotMap[hessianKeys[k]] = k; + } + + // Initialize matrix to zero. + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1)); + + // Add contributions for each key: note this loops over the hessian with + // nonUnique keys (augmentedHessian) and populates an Hessian that only + // includes the unique keys (that is what we want to return). + for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows + Key key_i = jacobianKeys.at(i); + + // Update information vector. + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], nrUniqueKeys, + augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); + + // Update blocks. + for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols + Key key_j = jacobianKeys.at(j); + if (i == j) { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); + } else { // (i < j) + if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], keyToSlotMap[key_j], + augmentedHessian.aboveDiagonalBlock(i, j)); + } else { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + } + } } - } // end of for over cameras + } - augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); - return augmentedHessian; + // Update bottom right element of the matrix. + augmentedHessianUniqueKeys.updateDiagonalBlock( + nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); } + return augmentedHessianUniqueKeys; + } /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix @@ -206,7 +304,7 @@ public: } /// Computes Point Covariance P, with lambda parameter - template // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void ComputePointCovariance(Eigen::Matrix& P, const Matrix& E, double lambda, bool diagonalDamping = false) { @@ -258,7 +356,7 @@ public: * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. */ - template // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b, const KeyVector& allKeys, const KeyVector& keys, diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 8ac67a5c3..c1f0b6b3f 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -30,7 +30,7 @@ namespace gtsam { * \nosubgrouping */ template -class PinholeCamera: public PinholeBaseK { +class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { public: diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 949caaa27..cc6435a58 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -31,7 +31,7 @@ namespace gtsam { * \nosubgrouping */ template -class PinholeBaseK: public PinholeBase { +class GTSAM_EXPORT PinholeBaseK: public PinholeBase { private: diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 17f87f656..cdb9f4480 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -25,6 +25,12 @@ namespace gtsam { /// As of GTSAM 4, in order to make GTSAM more lean, /// it is now possible to just typedef Point2 to Vector2 typedef Vector2 Point2; + +// Convenience typedef +using Point2Pair = std::pair; +GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); + +using Point2Pairs = std::vector; /// Distance of the point from the origin, with Jacobian GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); @@ -34,10 +40,6 @@ GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q, OptionalJacobian<1, 2> H1 = boost::none, OptionalJacobian<1, 2> H2 = boost::none); -// Convenience typedef -typedef std::pair Point2Pair; -GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); - // For MATLAB wrapper typedef std::vector > Point2Vector; diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 80c0171ad..2585c37be 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,25 +261,59 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (tr + 1.0 < 1e-10) { - if (std::abs(R33 + 1.0) > 1e-5) - omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); - else if (std::abs(R22 + 1.0) > 1e-5) - omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); - else - // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit - omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); + if (tr + 1.0 < 1e-3) { + if (R33 > R22 && R33 > R11) { + // R33 is the largest diagonal, a=3, b=1, c=2 + const double W = R21 - R12; + const double Q1 = 2.0 + 2.0 * R33; + const double Q2 = R31 + R13; + const double Q3 = R23 + R32; + const double r = sqrt(Q1); + const double one_over_r = 1 / r; + const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); + const double sgn_w = W < 0 ? -1.0 : 1.0; + const double mag = M_PI - (2 * sgn_w * W) / norm; + const double scale = 0.5 * one_over_r * mag; + omega = sgn_w * scale * Vector3(Q2, Q3, Q1); + } else if (R22 > R11) { + // R22 is the largest diagonal, a=2, b=3, c=1 + const double W = R13 - R31; + const double Q1 = 2.0 + 2.0 * R22; + const double Q2 = R23 + R32; + const double Q3 = R12 + R21; + const double r = sqrt(Q1); + const double one_over_r = 1 / r; + const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); + const double sgn_w = W < 0 ? -1.0 : 1.0; + const double mag = M_PI - (2 * sgn_w * W) / norm; + const double scale = 0.5 * one_over_r * mag; + omega = sgn_w * scale * Vector3(Q3, Q1, Q2); + } else { + // R11 is the largest diagonal, a=1, b=2, c=3 + const double W = R32 - R23; + const double Q1 = 2.0 + 2.0 * R11; + const double Q2 = R12 + R21; + const double Q3 = R31 + R13; + const double r = sqrt(Q1); + const double one_over_r = 1 / r; + const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); + const double sgn_w = W < 0 ? -1.0 : 1.0; + const double mag = M_PI - (2 * sgn_w * W) / norm; + const double scale = 0.5 * one_over_r * mag; + omega = sgn_w * scale * Vector3(Q1, Q2, Q3); + } } else { double magnitude; - const double tr_3 = tr - 3.0; // always negative - if (tr_3 < -1e-7) { + const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal + if (tr_3 < -1e-6) { + // this is the normal case -1 < trace < 3 double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); } else { // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) // see https://github.com/borglab/gtsam/issues/746 for details - magnitude = 0.5 - tr_3 / 12.0; + magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0; } omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); } diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0e303cbcd..9baa49e8e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -31,6 +31,14 @@ class Point2 { // enable pickling in python void pickle() const; }; + +class Point2Pairs { + Point2Pairs(); + size_t size() const; + bool empty() const; + gtsam::Point2Pair at(size_t n) const; + void push_back(const gtsam::Point2Pair& point_pair); +}; // std::vector class Point2Vector { @@ -429,6 +437,8 @@ class Pose2 { // enable pickling in python void pickle() const; }; + +boost::optional align(const gtsam::Point2Pairs& pairs); #include class Pose3 { diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 761ef3a8c..144f28314 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -125,6 +126,89 @@ TEST(CameraSet, Pinhole) { EXPECT(assert_equal(actualE, E)); } +/* ************************************************************************* */ +TEST(CameraSet, SchurComplementAndRearrangeBlocks) { + typedef PinholePose Camera; + typedef CameraSet Set; + + // this is the (block) Jacobian with respect to the nonuniqueKeys + std::vector, + Eigen::aligned_allocator > > Fs; + Fs.push_back(1 * Matrix::Ones(2, 12)); // corresponding to key pair (0,1) + Fs.push_back(2 * Matrix::Ones(2, 12)); // corresponding to key pair (1,2) + Fs.push_back(3 * Matrix::Ones(2, 12)); // corresponding to key pair (2,0) + Matrix E = 4 * Matrix::Identity(6, 3) + Matrix::Ones(6, 3); + E(0, 0) = 3; + E(1, 1) = 2; + E(2, 2) = 5; + Matrix Et = E.transpose(); + Matrix P = (Et * E).inverse(); + Vector b = 5 * Vector::Ones(6); + + { // SchurComplement + // Actual + SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3, 12>(Fs, E, + P, b); + Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); + + // Expected + Matrix F = Matrix::Zero(6, 3 * 12); + F.block<2, 12>(0, 0) = 1 * Matrix::Ones(2, 12); + F.block<2, 12>(2, 12) = 2 * Matrix::Ones(2, 12); + F.block<2, 12>(4, 24) = 3 * Matrix::Ones(2, 12); + + Matrix Ft = F.transpose(); + Matrix H = Ft * F - Ft * E * P * Et * F; + Vector v = Ft * (b - E * P * Et * b); + Matrix expectedAugmentedHessian = Matrix::Zero(3 * 12 + 1, 3 * 12 + 1); + expectedAugmentedHessian.block<36, 36>(0, 0) = H; + expectedAugmentedHessian.block<36, 1>(0, 36) = v; + expectedAugmentedHessian.block<1, 36>(36, 0) = v.transpose(); + expectedAugmentedHessian(36, 36) = b.squaredNorm(); + + EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); + } + + { // SchurComplementAndRearrangeBlocks + KeyVector nonuniqueKeys; + nonuniqueKeys.push_back(0); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(0); + + KeyVector uniqueKeys; + uniqueKeys.push_back(0); + uniqueKeys.push_back(1); + uniqueKeys.push_back(2); + + // Actual + SymmetricBlockMatrix augmentedHessianBM = + Set::SchurComplementAndRearrangeBlocks<3, 12, 6>( + Fs, E, P, b, nonuniqueKeys, uniqueKeys); + Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); + + // Expected + // we first need to build the Jacobian F according to unique keys + Matrix F = Matrix::Zero(6, 18); + F.block<2, 6>(0, 0) = Fs[0].block<2, 6>(0, 0); + F.block<2, 6>(0, 6) = Fs[0].block<2, 6>(0, 6); + F.block<2, 6>(2, 6) = Fs[1].block<2, 6>(0, 0); + F.block<2, 6>(2, 12) = Fs[1].block<2, 6>(0, 6); + F.block<2, 6>(4, 12) = Fs[2].block<2, 6>(0, 0); + F.block<2, 6>(4, 0) = Fs[2].block<2, 6>(0, 6); + + Matrix Ft = F.transpose(); + Vector v = Ft * (b - E * P * Et * b); + Matrix H = Ft * F - Ft * E * P * Et * F; + Matrix expectedAugmentedHessian(19, 19); + expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm(); + + EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); + } +} + /* ************************************************************************* */ #include TEST(CameraSet, Stereo) { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 11b8791d4..7c1fa81e6 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1046,6 +1046,68 @@ TEST(Pose3, interpolate) { EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t))); } +/* ************************************************************************* */ +Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); } + +TEST(Pose3, interpolateJacobians) { + { + Pose3 X = Pose3::identity(); + Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0)); + double t = 0.5; + Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation + Matrix actualJacobianX, actualJacobianY; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } + { + Pose3 X = Pose3::identity(); + Pose3 Y(Rot3::identity(), Point3(1, 0, 0)); + double t = 0.3; + Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0)); + Matrix actualJacobianX, actualJacobianY; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } + { + Pose3 X = Pose3::identity(); + Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0)); + double t = 0.5; + Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); + Matrix actualJacobianX, actualJacobianY; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } + { + Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2)); + Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1)); + double t = 0.3; + Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); + Matrix actualJacobianX, actualJacobianY; + interpolate(X, Y, t, actualJacobianX, actualJacobianY); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } +} + /* ************************************************************************* */ TEST(Pose3, Create) { Matrix63 actualH1, actualH2; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 34f90c8cc..dc4b888b3 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -122,6 +122,21 @@ TEST( Rot3, AxisAngle) CHECK(assert_equal(expected,actual3,1e-5)); } +/* ************************************************************************* */ +TEST( Rot3, AxisAngle2) +{ + // constructor from a rotation matrix, as doubles in *row-major* order. + Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781); + + Unit3 actualAxis; + double actualAngle; + // convert Rot3 to quaternion using GTSAM + std::tie(actualAxis, actualAngle) = R1.axisAngle(); + + double expectedAngle = 3.1396582; + CHECK(assert_equal(expectedAngle, actualAngle, 1e-5)); +} + /* ************************************************************************* */ TEST( Rot3, Rodrigues) { @@ -181,13 +196,13 @@ TEST( Rot3, retract) } /* ************************************************************************* */ -TEST(Rot3, log) { +TEST( Rot3, log) { static const double PI = boost::math::constants::pi(); Vector w; Rot3 R; #define CHECK_OMEGA(X, Y, Z) \ - w = (Vector(3) << X, Y, Z).finished(); \ + w = (Vector(3) << (X), (Y), (Z)).finished(); \ R = Rot3::Rodrigues(w); \ EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12)); @@ -219,17 +234,17 @@ TEST(Rot3, log) { CHECK_OMEGA(0, 0, PI) // Windows and Linux have flipped sign in quaternion mode -#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) +//#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) w = (Vector(3) << x * PI, y * PI, z * PI).finished(); R = Rot3::Rodrigues(w); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12)); -#else - CHECK_OMEGA(x * PI, y * PI, z * PI) -#endif +//#else +// CHECK_OMEGA(x * PI, y * PI, z * PI) +//#endif // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X, Y, Z) \ - w = (Vector(3) << X, Y, Z).finished(); \ + w = (Vector(3) << (X), (Y), (Z)).finished(); \ R = Rot3::Rodrigues(w); \ EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R))); @@ -247,15 +262,15 @@ TEST(Rot3, log) { // Rot3's Logmap returns different, but equivalent compacted // axis-angle vectors depending on whether Rot3 is implemented // by Quaternions or SO3. - #if defined(GTSAM_USE_QUATERNIONS) - // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees - EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), +#if defined(GTSAM_USE_QUATERNIONS) + // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees + EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), + (Vector)Rot3::Logmap(Rlund), 1e-8)); +#else + // SO3 will be approximate because of the non-orthogonality + EXPECT(assert_equal(Vector3(0.264452, -0.742197708, -3.04098184), (Vector)Rot3::Logmap(Rlund), 1e-8)); - #else - // SO3 does not bound angle resulting in ~180.1 degrees - EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314), - (Vector)Rot3::Logmap(Rlund), 1e-8)); - #endif +#endif } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testUtilities.cpp b/gtsam/geometry/tests/testUtilities.cpp new file mode 100644 index 000000000..25ac3acc8 --- /dev/null +++ b/gtsam/geometry/tests/testUtilities.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testUtilities.cpp + * @date Aug 19, 2021 + * @author Varun Agrawal + * @brief Tests for the utilities. + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using gtsam::symbol_shorthand::L; +using gtsam::symbol_shorthand::R; +using gtsam::symbol_shorthand::X; + +/* ************************************************************************* */ +TEST(Utilities, ExtractPoint2) { + Point2 p0(0, 0), p1(1, 0); + Values values; + values.insert(L(0), p0); + values.insert(L(1), p1); + values.insert(R(0), Rot3()); + values.insert(X(0), Pose3()); + + Matrix all_points = utilities::extractPoint2(values); + EXPECT_LONGS_EQUAL(2, all_points.rows()); +} + +/* ************************************************************************* */ +TEST(Utilities, ExtractPoint3) { + Point3 p0(0, 0, 0), p1(1, 0, 0); + Values values; + values.insert(L(0), p0); + values.insert(L(1), p1); + values.insert(R(0), Rot3()); + values.insert(X(0), Pose3()); + + Matrix all_points = utilities::extractPoint3(values); + EXPECT_LONGS_EQUAL(2, all_points.rows()); +} + +/* ************************************************************************* */ +int main() { + srand(time(nullptr)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index a55581e50..67c3278a3 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -165,22 +165,17 @@ gtsam::Values allPose2s(gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); gtsam::Values allPose3s(gtsam::Values& values); Matrix extractPose3(const gtsam::Values& values); -void perturbPoint2(gtsam::Values& values, double sigma, int seed); +void perturbPoint2(gtsam::Values& values, double sigma, int seed = 42u); void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR, - int seed); -void perturbPoint3(gtsam::Values& values, double sigma, int seed); + int seed = 42u); +void perturbPoint3(gtsam::Values& values, double sigma, int seed = 42u); void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCamera& c, Vector J, Matrix Z, double depth); -void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, - Vector J, Matrix Z, - const gtsam::noiseModel::Base* model, - const gtsam::Cal3_S2* K); -void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, - Vector J, Matrix Z, - const gtsam::noiseModel::Base* model, - const gtsam::Cal3_S2* K, - const gtsam::Pose3& body_P_sensor); +void insertProjectionFactors( + gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, + const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, + const gtsam::Pose3& body_P_sensor = gtsam::Pose3()); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); gtsam::Values localToWorld(const gtsam::Values& local, diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 7b79ccf68..c9bb6db94 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -70,16 +70,23 @@ namespace gtsam { /// @name Standard Constructors /// @{ - /** Default constructor */ + /// Default constructor BayesTreeCliqueBase() : problemSize_(1) {} - /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {} + /// Construct from a conditional, leaving parent and child pointers + /// uninitialized. + BayesTreeCliqueBase(const sharedConditional& conditional) + : conditional_(conditional), problemSize_(1) {} - /** Shallow copy constructor */ - BayesTreeCliqueBase(const BayesTreeCliqueBase& c) : conditional_(c.conditional_), parent_(c.parent_), children(c.children), problemSize_(c.problemSize_), is_root(c.is_root) {} + /// Shallow copy constructor. + BayesTreeCliqueBase(const BayesTreeCliqueBase& c) + : conditional_(c.conditional_), + parent_(c.parent_), + children(c.children), + problemSize_(c.problemSize_), + is_root(c.is_root) {} - /** Shallow copy assignment constructor */ + /// Shallow copy assignment constructor BayesTreeCliqueBase& operator=(const BayesTreeCliqueBase& c) { conditional_ = c.conditional_; parent_ = c.parent_; @@ -89,6 +96,9 @@ namespace gtsam { return *this; } + // Virtual destructor. + virtual ~BayesTreeCliqueBase() {} + /// @} /// This stores the Cached separator marginal P(S) @@ -119,7 +129,9 @@ namespace gtsam { bool equals(const DERIVED& other, double tol = 1e-9) const; /** print this node */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Standard Interface diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index 2cc19d07a..31428a50e 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -32,7 +32,7 @@ namespace gtsam { /// Typedef for a function to format a key, i.e. to convert it to a string -typedef std::function KeyFormatter; +using KeyFormatter = std::function; // Helper function for DefaultKeyFormatter GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); @@ -83,28 +83,32 @@ class key_formatter { }; /// Define collection type once and for all - also used in wrappers -typedef FastVector KeyVector; +using KeyVector = FastVector; // TODO(frank): Nothing fast about these :-( -typedef FastList KeyList; -typedef FastSet KeySet; -typedef FastMap KeyGroupMap; +using KeyList = FastList; +using KeySet = FastSet; +using KeyGroupMap = FastMap; /// Utility function to print one key with optional prefix -GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKey( + Key key, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix -GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKeyList( + const KeyList &keys, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix -GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s = - "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKeyVector( + const KeyVector &keys, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix -GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKeySet( + const KeySet &keys, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); // Define Key to be Testable by specializing gtsam::traits template struct traits; diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 266c54ca6..440d2b828 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -25,8 +25,12 @@ #include #ifdef GTSAM_SUPPORT_NESTED_DISSECTION +#ifdef GTSAM_USE_SYSTEM_METIS +#include +#else #include #endif +#endif using namespace std; diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 63047bf4f..8635c55f8 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -16,9 +16,9 @@ virtual class Base { }; virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* Information(Matrix R); - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + static gtsam::noiseModel::Gaussian* Information(Matrix R, bool smart = true); + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R, bool smart = true); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R, bool smart = true); bool equals(gtsam::noiseModel::Base& expected, double tol); @@ -37,9 +37,9 @@ virtual class Gaussian : gtsam::noiseModel::Base { }; virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas, bool smart = true); + static gtsam::noiseModel::Diagonal* Variances(Vector variances, bool smart = true); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions, bool smart = true); Matrix R() const; // access to noise model @@ -69,9 +69,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal { }; virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma, bool smart = true); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace, bool smart = true); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision, bool smart = true); // access to noise model double sigma() const; @@ -289,6 +289,13 @@ virtual class JacobianFactor : gtsam::GaussianFactor { JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, Vector b, const gtsam::noiseModel::Diagonal* model); JacobianFactor(const gtsam::GaussianFactorGraph& graph); + JacobianFactor(const gtsam::GaussianFactorGraph& graph, + const gtsam::VariableSlots& p_variableSlots); + JacobianFactor(const gtsam::GaussianFactorGraph& graph, + const gtsam::Ordering& ordering); + JacobianFactor(const gtsam::GaussianFactorGraph& graph, + const gtsam::Ordering& ordering, + const gtsam::VariableSlots& p_variableSlots); //Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 801d87895..585da38b1 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -819,7 +819,6 @@ struct ImuFactorMergeTest { loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) { // arbitrary noise values p_->gyroscopeCovariance = I_3x3 * 0.01; - p_->accelerometerCovariance = I_3x3 * 0.02; p_->accelerometerCovariance = I_3x3 * 0.03; } diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 691ab8ac8..e1f8ece8d 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -110,7 +110,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { bool equals(const NonlinearFactor &other, double tol = 1e-9) const override { const FunctorizedFactor *e = dynamic_cast *>(&other); - return e && Base::equals(other, tol) && + return e != nullptr && Base::equals(other, tol) && traits::Equals(this->measured_, e->measured_, tol); } /// @} diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index eb353c53f..3ddaf4820 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -42,7 +42,7 @@ static double Chi2inv(const double alpha, const size_t dofs) { /* ************************************************************************* */ template -class GncOptimizer { +class GTSAM_EXPORT GncOptimizer { public: /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename GncParameters::OptimizerType BaseOptimizer; diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index c1bf7a035..086f08acc 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -39,7 +39,7 @@ enum GncLossType { }; template -class GncParams { +class GTSAM_EXPORT GncParams { public: /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename BaseOptimizerParameters::OptimizerType OptimizerType; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 6b9972156..47083d5d7 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -66,12 +66,9 @@ private: public: - /** - * Function that compares two values - */ - typedef std::function CompareFunction; + /// Function that compares two values. + using CompareFunction = std::function; CompareFunction compare_; -// bool (*compare_)(const T& a, const T& b); /// Default constructor - only for serialization NonlinearEquality() { @@ -198,9 +195,8 @@ private: }; // \class NonlinearEquality -template -struct traits > : Testable > { -}; +template +struct traits> : Testable> {}; /* ************************************************************************* */ /** @@ -285,33 +281,28 @@ private: }; // \NonlinearEquality1 -template -struct traits > : Testable > { -}; +template +struct traits > + : Testable > {}; /* ************************************************************************* */ /** * Simple binary equality constraint - this constraint forces two variables to * be the same. */ -template -class NonlinearEquality2: public NoiseModelFactor2 { -public: - typedef VALUE X; +template +class NonlinearEquality2 : public NoiseModelFactor2 { + protected: + using Base = NoiseModelFactor2; + using This = NonlinearEquality2; -protected: - typedef NoiseModelFactor2 Base; - typedef NonlinearEquality2 This; - - GTSAM_CONCEPT_MANIFOLD_TYPE(X) + GTSAM_CONCEPT_MANIFOLD_TYPE(T) /// Default constructor to allow for serialization - NonlinearEquality2() { - } + NonlinearEquality2() {} -public: - - typedef boost::shared_ptr > shared_ptr; + public: + typedef boost::shared_ptr> shared_ptr; /** * Constructor @@ -319,11 +310,10 @@ public: * @param key2 the key for the second unknown variable to be constrained * @param mu a parameter which really turns this into a strong prior */ - NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) : - Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), key1, key2) { - } - ~NonlinearEquality2() override { - } + NonlinearEquality2(Key key1, Key key2, double mu = 1e4) + : Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), + key1, key2) {} + ~NonlinearEquality2() override {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -332,32 +322,30 @@ public: } /// g(x) with optional derivative2 - Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const override { - static const size_t p = traits::dimension; - if (H1) *H1 = -Matrix::Identity(p,p); - if (H2) *H2 = Matrix::Identity(p,p); - return traits::Local(x1,x2); + Vector evaluateError( + const T& x1, const T& x2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + static const size_t p = traits::dimension; + if (H1) *H1 = -Matrix::Identity(p, p); + if (H2) *H2 = Matrix::Identity(p, p); + return traits::Local(x1, x2); } GTSAM_MAKE_ALIGNED_OPERATOR_NEW -private: - + private: /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar - & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor2", boost::serialization::base_object(*this)); } }; // \NonlinearEquality2 -template -struct traits > : Testable > { +template +struct traits> : Testable> { }; - }// namespace gtsam diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 8071e8bc7..d068bd7ee 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -75,12 +75,15 @@ size_t Z(size_t j); } // namespace symbol_shorthand // Default keyformatter -void PrintKeyList(const gtsam::KeyList& keys); -void PrintKeyList(const gtsam::KeyList& keys, string s); -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); -void PrintKeySet(const gtsam::KeySet& keys); -void PrintKeySet(const gtsam::KeySet& keys, string s); +void PrintKeyList( + const gtsam::KeyList& keys, const string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +void PrintKeyVector( + const gtsam::KeyVector& keys, const string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +void PrintKeySet( + const gtsam::KeySet& keys, const string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); #include class LabeledSymbol { @@ -522,6 +525,24 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { void setVerbosityDL(string verbosityDL) const; }; +#include +template +virtual class GncParams { + GncParams(const PARAMS& baseOptimizerParams); + GncParams(); + void setVerbosityGNC(const This::Verbosity value); + void print(const string& str) const; + + enum Verbosity { + SILENT, + SUMMARY, + VALUES + }; +}; + +typedef gtsam::GncParams GncGaussNewtonParams; +typedef gtsam::GncParams GncLMParams; + #include virtual class NonlinearOptimizer { gtsam::Values optimize(); @@ -551,6 +572,18 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { const gtsam::DoglegParams& params); double getDelta() const; }; + +#include +template +virtual class GncOptimizer { + GncOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const PARAMS& params); + gtsam::Values optimize(); +}; + +typedef gtsam::GncOptimizer> GncGaussNewtonOptimizer; +typedef gtsam::GncOptimizer> GncLMOptimizer; #include virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { @@ -801,4 +834,17 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor { void serialize() const; }; +template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> +virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { + NonlinearEquality2(Key key1, Key key2, double mu = 1e4); + gtsam::Vector evaluateError(const T& x1, const T& x2); +}; + } // namespace gtsam diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index b0ec5e722..14a14fc19 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -20,8 +20,12 @@ #include #include #include +#include +#include +#include #include #include +#include #include using namespace std; @@ -60,7 +64,7 @@ class ProjectionFunctor { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; } @@ -255,18 +259,148 @@ TEST(FunctorizedFactor, Lambda2) { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; }; // FunctorizedFactor factor(key, measurement, model, lambda); - auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, model2, lambda); + auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, + model2, lambda); Vector error = factor.evaluateError(A, x); EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); } +const size_t N = 2; + +//****************************************************************************** +TEST(FunctorizedFactor, Print2) { + const size_t M = 1; + + Vector measured = Vector::Ones(M) * 42; + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, priorFactor)); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorEvaluationFactor) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(priorFactor); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorComponentFactor) { + const int P = 4; + const size_t i = 2; + const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + VectorComponentFactor controlPrior(key, measured, model, N, i, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(controlPrior); + + ParameterMatrix

stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VecDerivativePrior) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(vecDPrior); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, ComponentDerivativeFactor) { + const size_t M = 4; + + double measured = 0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + ComponentDerivativeFactor controlDPrior(key, measured, model, + N, 0, 0); + + NonlinearFactorGraph graph; + graph.add(controlDPrior); + + Values initial; + ParameterMatrix stateMatrix(N); + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 4e79e20ff..fdc1da2c4 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file matlab.h + * @file utilities.h * @brief Contains *generic* global functions designed particularly for the matlab interface * @author Stephen Williams */ @@ -89,21 +89,41 @@ KeySet createKeySet(std::string s, const Vector& I) { /// Extract all Point2 values into a single matrix [x y] Matrix extractPoint2(const Values& values) { + Values::ConstFiltered points = values.filter(); + // Point2 is aliased as a gtsam::Vector in the wrapper + Values::ConstFiltered points2 = values.filter(); + + Matrix result(points.size() + points2.size(), 2); + size_t j = 0; - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(), 2); - for(const auto& key_value: points) + for (const auto& key_value : points) { result.row(j++) = key_value.value; + } + for (const auto& key_value : points2) { + if (key_value.value.rows() == 2) { + result.row(j++) = key_value.value; + } + } return result; } /// Extract all Point3 values into a single matrix [x y z] Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(), 3); + Values::ConstFiltered points = values.filter(); + // Point3 is aliased as a gtsam::Vector in the wrapper + Values::ConstFiltered points2 = values.filter(); + + Matrix result(points.size() + points2.size(), 3); + size_t j = 0; - for(const auto& key_value: points) + for (const auto& key_value : points) { result.row(j++) = key_value.value; + } + for (const auto& key_value : points2) { + if (key_value.value.rows() == 3) { + result.row(j++) = key_value.value; + } + } return result; } @@ -144,11 +164,18 @@ Matrix extractPose3(const Values& values) { /// Perturb all Point2 values using normally distributed noise void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, - sigma); + noiseModel::Isotropic::shared_ptr model = + noiseModel::Isotropic::Sigma(2, sigma); Sampler sampler(model, seed); - for(const auto& key_value: values.filter()) { - values.update(key_value.key, key_value.value + Point2(sampler.sample())); + for (const auto& key_value : values.filter()) { + values.update(key_value.key, + key_value.value + Point2(sampler.sample())); + } + for (const auto& key_value : values.filter()) { + if (key_value.value.rows() == 2) { + values.update(key_value.key, + key_value.value + Point2(sampler.sample())); + } } } @@ -165,19 +192,34 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = /// Perturb all Point3 values using normally distributed noise void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, - sigma); + noiseModel::Isotropic::shared_ptr model = + noiseModel::Isotropic::Sigma(3, sigma); Sampler sampler(model, seed); - for(const auto& key_value: values.filter()) { - values.update(key_value.key, key_value.value + Point3(sampler.sample())); + for (const auto& key_value : values.filter()) { + values.update(key_value.key, + key_value.value + Point3(sampler.sample())); + } + for (const auto& key_value : values.filter()) { + if (key_value.value.rows() == 3) { + values.update(key_value.key, + key_value.value + Point3(sampler.sample())); + } } } -/// Insert a number of initial point values by backprojecting +/** + * @brief Insert a number of initial point values by backprojecting + * + * @param values The values dict to insert the backprojections to. + * @param camera The camera model. + * @param J Vector of key indices. + * @param Z 2*J matrix of pixel values. + * @param depth Initial depth value. + */ void insertBackprojections(Values& values, const PinholeCamera& camera, const Vector& J, const Matrix& Z, double depth) { if (Z.rows() != 2) - throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + throw std::invalid_argument("insertBackProjections: Z must be 2*J"); if (Z.cols() != J.size()) throw std::invalid_argument( "insertBackProjections: J and Z must have same number of entries"); @@ -188,7 +230,17 @@ void insertBackprojections(Values& values, const PinholeCamera& camera, } } -/// Insert multiple projection factors for a single pose key +/** + * @brief Insert multiple projection factors for a single pose key + * + * @param graph The nonlinear factor graph to add the factors to. + * @param i Camera key. + * @param J Vector of key indices. + * @param Z 2*J matrix of pixel values. + * @param model Factor noise model. + * @param K Calibration matrix. + * @param body_P_sensor Pose of the camera sensor in the body frame. + */ void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, const SharedNoiseModel& model, const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index aef41d5fd..8a1ffdd72 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -103,7 +103,7 @@ namespace gtsam { boost::none, boost::optional H2 = boost::none) const override { T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) -#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR +#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR typename traits::ChartJacobian::Jacobian Hlocal; Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); if (H1) *H1 = Hlocal * (*H1); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 67100a0ac..ada304f27 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -164,10 +164,15 @@ namespace gtsam { } /** return the calibration object */ - inline const boost::shared_ptr calibration() const { + const boost::shared_ptr calibration() const { return K_; } + /** return the (optional) sensor pose with respect to the vehicle frame */ + const boost::optional& body_P_sensor() const { + return body_P_sensor_; + } + /** return verbosity */ inline bool verboseCheirality() const { return verboseCheirality_; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 0b0308c5c..380283141 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -178,7 +178,7 @@ protected: DefaultKeyFormatter) const override { std::cout << s << "SmartFactorBase, z = \n"; for (size_t k = 0; k < measured_.size(); ++k) { - std::cout << "measurement, p = " << measured_[k] << "\t"; + std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n"; noiseModel_->print("noise model = "); } if(body_P_sensor_) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 475a9e829..f67ca0740 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -101,7 +101,7 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionFactor\n"; - std::cout << "linearizationMode:\n" << params_.linearizationMode + std::cout << "linearizationMode: " << params_.linearizationMode << std::endl; std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl; diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index c6aa02774..3b8ea86d3 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -138,4 +138,21 @@ Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { return Point2_(K, &CALIBRATION::uncalibrate, xy_hat); } + +/// logmap +// TODO(dellaert): Should work but fails because of a type deduction conflict. +// template +// gtsam::Expression::TangentVector> logmap( +// const gtsam::Expression &x1, const gtsam::Expression &x2) { +// return gtsam::Expression::TangentVector>( +// x1, &T::logmap, x2); +// } + +template +gtsam::Expression::TangentVector> logmap( + const gtsam::Expression &x1, const gtsam::Expression &x2) { + return Expression::TangentVector>( + gtsam::traits::Logmap, between(x1, x2)); +} + } // \namespace gtsam diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp index 294b821d3..b5298989f 100644 --- a/gtsam/slam/tests/testSlamExpressions.cpp +++ b/gtsam/slam/tests/testSlamExpressions.cpp @@ -58,6 +58,13 @@ TEST(SlamExpressions, unrotate) { const Point3_ q_ = unrotate(R_, p_); } +/* ************************************************************************* */ +TEST(SlamExpressions, logmap) { + Pose3_ T1_(0); + Pose3_ T2_(1); + const Vector6_ l = logmap(T1_, T2_); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c7f220c10..997c33846 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -50,7 +50,7 @@ static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): -// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; +// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index ce657e7a0..2e48b0d45 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -20,6 +20,8 @@ #include "FindSeparator.h" +#ifndef GTSAM_USE_SYSTEM_METIS + extern "C" { #include #include "metislib.h" @@ -564,3 +566,5 @@ namespace gtsam { namespace partition { } }} //namespace + +#endif diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index fe49de928..63acc8f18 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -20,6 +20,8 @@ using namespace std; using namespace gtsam; using namespace gtsam::partition; +#ifndef GTSAM_USE_SYSTEM_METIS + /* ************************************************************************* */ // x0 - x1 - x2 // l3 l4 @@ -227,6 +229,8 @@ TEST ( Partition, findSeparator3_with_reduced_camera ) LONGS_EQUAL(2, partitionTable[28]); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp new file mode 100644 index 000000000..c92a13daf --- /dev/null +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ProjectionFactorRollingShutter.cpp + * @brief Basic projection factor for rolling shutter cameras + * @author Yotam Stern + */ + +#include + +namespace gtsam { + +Vector ProjectionFactorRollingShutter::evaluateError( + const Pose3& pose_a, const Pose3& pose_b, const Point3& point, + boost::optional H1, boost::optional H2, + boost::optional H3) const { + try { + Pose3 pose = interpolate(pose_a, pose_b, alpha_, H1, H2); + gtsam::Matrix Hprj; + if (body_P_sensor_) { + if (H1 || H2 || H3) { + gtsam::Matrix HbodySensor; + PinholeCamera camera( + pose.compose(*body_P_sensor_, HbodySensor), *K_); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + measured_); + if (H1) *H1 = Hprj * HbodySensor * (*H1); + if (H2) *H2 = Hprj * HbodySensor * (*H2); + return reprojectionError; + } else { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + return camera.project(point) - measured_; + } + } else { + PinholeCamera camera(pose, *K_); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + measured_); + if (H1) *H1 = Hprj * (*H1); + if (H2) *H2 = Hprj * (*H2); + return reprojectionError; + } + } catch (CheiralityException& e) { + if (H1) *H1 = Matrix::Zero(2, 6); + if (H2) *H2 = Matrix::Zero(2, 6); + if (H3) *H3 = Matrix::Zero(2, 3); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark " + << DefaultKeyFormatter(this->key2()) << " moved behind camera " + << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) throw CheiralityException(this->key2()); + } + return Vector2::Constant(2.0 * K_->fx()); +} + +} // namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h new file mode 100644 index 000000000..c92653c13 --- /dev/null +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -0,0 +1,217 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ProjectionFactorRollingShutter.h + * @brief Basic projection factor for rolling shutter cameras + * @author Yotam Stern + */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Non-linear factor for 2D projection measurement obtained using a rolling + * shutter camera. The calibration is known here. This version takes rolling + * shutter information into account as follows: consider two consecutive poses A + * and B, and a Point2 measurement taken starting at time A using a rolling + * shutter camera. Pose A has timestamp t_A, and Pose B has timestamp t_B. The + * Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) corresponding + * to the time of exposure of the row of the image the pixel belongs to. Let us + * define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose + * interpolated between A and B by the alpha to project the corresponding + * landmark to Point2. + * @addtogroup SLAM + */ + +class ProjectionFactorRollingShutter + : public NoiseModelFactor3 { + protected: + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + double alpha_; ///< interpolation parameter in [0,1] corresponding to the + ///< point2 measurement + boost::shared_ptr K_; ///< shared pointer to calibration object + boost::optional + body_P_sensor_; ///< The pose of the sensor in the body frame + + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: + ///< false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions + ///< (default: false) + + public: + /// shorthand for base class type + typedef NoiseModelFactor3 Base; + + /// shorthand for this class + typedef ProjectionFactorRollingShutter This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + ProjectionFactorRollingShutter() + : measured_(0, 0), + alpha_(0), + throwCheirality_(false), + verboseCheirality_(false) {} + + /** + * Constructor + * @param measured is the 2-dimensional pixel location of point in the image + * (the measurement) + * @param alpha in [0,1] is the rolling shutter parameter for the measurement + * @param model is the noise model + * @param poseKey_a is the key of the first camera + * @param poseKey_b is the key of the second camera + * @param pointKey is the key of the landmark + * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default + * identity) + */ + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) + : Base(model, poseKey_a, poseKey_b, pointKey), + measured_(measured), + alpha_(alpha), + K_(K), + body_P_sensor_(body_P_sensor), + throwCheirality_(false), + verboseCheirality_(false) {} + + /** + * Constructor with exception-handling flags + * @param measured is the 2-dimensional pixel location of point in the image + * (the measurement) + * @param alpha in [0,1] is the rolling shutter parameter for the measurement + * @param model is the noise model + * @param poseKey_a is the key of the first camera + * @param poseKey_b is the key of the second camera + * @param pointKey is the key of the landmark + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are + * rethrown + * @param verboseCheirality determines whether exceptions are printed for + * Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default + * identity) + */ + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, bool throwCheirality, + bool verboseCheirality, + boost::optional body_P_sensor = boost::none) + : Base(model, poseKey_a, poseKey_b, pointKey), + measured_(measured), + alpha_(alpha), + K_(K), + body_P_sensor_(body_P_sensor), + throwCheirality_(throwCheirality), + verboseCheirality_(verboseCheirality) {} + + /** Virtual destructor */ + virtual ~ProjectionFactorRollingShutter() {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "ProjectionFactorRollingShutter, z = "; + traits::Print(measured_); + std::cout << " rolling shutter interpolation param = " << alpha_; + if (this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + + /// equals + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && (alpha_ == e->alpha()) && + traits::Equals(this->measured_, e->measured_, tol) && + this->K_->equals(*e->K_, tol) && + (this->throwCheirality_ == e->throwCheirality_) && + (this->verboseCheirality_ == e->verboseCheirality_) && + ((!body_P_sensor_ && !e->body_P_sensor_) || + (body_P_sensor_ && e->body_P_sensor_ && + body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError( + const Pose3& pose_a, const Pose3& pose_b, const Point3& point, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const override; + + /** return the measurement */ + const Point2& measured() const { return measured_; } + + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { return K_; } + + /** returns the rolling shutter interp param*/ + inline double alpha() const { return alpha_; } + + /** return verbosity */ + inline bool verboseCheirality() const { return verboseCheirality_; } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { return throwCheirality_; } + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(measured_); + ar& BOOST_SERIALIZATION_NVP(alpha_); + ar& BOOST_SERIALIZATION_NVP(K_); + ar& BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar& BOOST_SERIALIZATION_NVP(throwCheirality_); + ar& BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// traits +template <> +struct traits + : public Testable {}; + +} // namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h new file mode 100644 index 000000000..7660ff236 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -0,0 +1,485 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SmartProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses modeling rolling shutter effect with + * given readout time + * @author Luca Carlone + */ + +#pragma once + +#include +#include + +namespace gtsam { +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, + * Eliminating conditionally independent sets in factor graphs: + * a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + */ + +/** + * This factor optimizes two consecutive poses of the body assuming a rolling + * shutter model of the camera with given readout time. The factor requires that + * values contain (for each pixel observation) two consecutive camera poses from + * which the pixel observation pose can be interpolated. + * @addtogroup SLAM + */ +template +class SmartProjectionPoseFactorRollingShutter + : public SmartProjectionFactor { + public: + typedef typename CAMERA::CalibrationType CALIBRATION; + + protected: + /// shared pointer to calibration object (one for each observation) + std::vector> K_all_; + + /// The keys of the pose of the body (with respect to an external world + /// frame): two consecutive poses for each observation + std::vector> world_P_body_key_pairs_; + + /// interpolation factor (one for each observation) to interpolate between + /// pair of consecutive poses + std::vector alphas_; + + /// Pose of the camera in the body frame + std::vector body_P_sensors_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// shorthand for base class type + typedef SmartProjectionFactor> Base; + + /// shorthand for this class + typedef SmartProjectionPoseFactorRollingShutter This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + static const int DimBlock = + 12; ///< size of the variable stacking 2 poses from which the observation + ///< pose is interpolated + static const int DimPose = 6; ///< Pose3 dimension + static const int ZDim = 2; ///< Measurement dimension (Point2) + typedef Eigen::Matrix + MatrixZD; // F blocks (derivatives wrt block of 2 poses) + typedef std::vector> + FBlocks; // vector of F blocks + + /** + * Constructor + * @param Isotropic measurement noise + * @param params internal parameters of the smart factors + */ + SmartProjectionPoseFactorRollingShutter( + const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params) {} + + /** Virtual destructor */ + ~SmartProjectionPoseFactorRollingShutter() override = default; + + /** + * add a new measurement, with 2 pose keys, interpolation factor, camera + * (intrinsic and extrinsic) calibration, and observed pixel. + * @param measured 2-dimensional location of the projection of a single + * landmark in a single view (the measurement), interpolated from the 2 poses + * @param world_P_body_key1 key corresponding to the first body poses (time <= + * time pixel is acquired) + * @param world_P_body_key2 key corresponding to the second body poses (time + * >= time pixel is acquired) + * @param alpha interpolation factor in [0,1], such that if alpha = 0 the + * interpolated pose is the same as world_P_body_key1 + * @param K (fixed) camera intrinsic calibration + * @param body_P_sensor (fixed) camera extrinsic calibration + */ + void add(const Point2& measured, const Key& world_P_body_key1, + const Key& world_P_body_key2, const double& alpha, + const boost::shared_ptr& K, + const Pose3& body_P_sensor = Pose3::identity()) { + // store measurements in base class + this->measured_.push_back(measured); + + // store the pair of keys for each measurement, in the same order + world_P_body_key_pairs_.push_back( + std::make_pair(world_P_body_key1, world_P_body_key2)); + + // also store keys in the keys_ vector: these keys are assumed to be + // unique, so we avoid duplicates here + if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) == + this->keys_.end()) + this->keys_.push_back(world_P_body_key1); // add only unique keys + if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) == + this->keys_.end()) + this->keys_.push_back(world_P_body_key2); // add only unique keys + + // store interpolation factor + alphas_.push_back(alpha); + + // store fixed intrinsic calibration + K_all_.push_back(K); + + // store fixed extrinsics of the camera + body_P_sensors_.push_back(body_P_sensor); + } + + /** + * Variant of the previous "add" function in which we include multiple + * measurements + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m views (the measurements) + * @param world_P_body_key_pairs vector where the i-th element contains a pair + * of keys corresponding to the pair of poses from which the observation pose + * for the i0-th measurement can be interpolated + * @param alphas vector of interpolation params (in [0,1]), one for each + * measurement (in the same order) + * @param Ks vector of (fixed) intrinsic calibration objects + * @param body_P_sensors vector of (fixed) extrinsic calibration objects + */ + void add(const Point2Vector& measurements, + const std::vector>& world_P_body_key_pairs, + const std::vector& alphas, + const std::vector>& Ks, + const std::vector& body_P_sensors) { + assert(world_P_body_key_pairs.size() == measurements.size()); + assert(world_P_body_key_pairs.size() == alphas.size()); + assert(world_P_body_key_pairs.size() == Ks.size()); + for (size_t i = 0; i < measurements.size(); i++) { + add(measurements[i], world_P_body_key_pairs[i].first, + world_P_body_key_pairs[i].second, alphas[i], Ks[i], + body_P_sensors[i]); + } + } + + /** + * Variant of the previous "add" function in which we include multiple + * measurements with the same (intrinsic and extrinsic) calibration + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m views (the measurements) + * @param world_P_body_key_pairs vector where the i-th element contains a pair + * of keys corresponding to the pair of poses from which the observation pose + * for the i0-th measurement can be interpolated + * @param alphas vector of interpolation params (in [0,1]), one for each + * measurement (in the same order) + * @param K (fixed) camera intrinsic calibration (same for all measurements) + * @param body_P_sensor (fixed) camera extrinsic calibration (same for all + * measurements) + */ + void add(const Point2Vector& measurements, + const std::vector>& world_P_body_key_pairs, + const std::vector& alphas, + const boost::shared_ptr& K, + const Pose3& body_P_sensor = Pose3::identity()) { + assert(world_P_body_key_pairs.size() == measurements.size()); + assert(world_P_body_key_pairs.size() == alphas.size()); + for (size_t i = 0; i < measurements.size(); i++) { + add(measurements[i], world_P_body_key_pairs[i].first, + world_P_body_key_pairs[i].second, alphas[i], K, body_P_sensor); + } + } + + /// return the calibration object + const std::vector>& calibration() const { + return K_all_; + } + + /// return (for each observation) the keys of the pair of poses from which we + /// interpolate + const std::vector>& world_P_body_key_pairs() const { + return world_P_body_key_pairs_; + } + + /// return the interpolation factors alphas + const std::vector& alphas() const { return alphas_; } + + /// return the extrinsic camera calibration body_P_sensors + const std::vector& body_P_sensors() const { return body_P_sensors_; } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; + for (size_t i = 0; i < K_all_.size(); i++) { + std::cout << "-- Measurement nr " << i << std::endl; + std::cout << " pose1 key: " + << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; + std::cout << " pose2 key: " + << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; + std::cout << " alpha: " << alphas_[i] << std::endl; + body_P_sensors_[i].print("extrinsic calibration:\n"); + K_all_[i]->print("intrinsic calibration = "); + } + Base::print("", keyFormatter); + } + + /// equals + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const SmartProjectionPoseFactorRollingShutter* e = + dynamic_cast*>( + &p); + + double keyPairsEqual = true; + if (this->world_P_body_key_pairs_.size() == + e->world_P_body_key_pairs().size()) { + for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) { + const Key key1own = world_P_body_key_pairs_[k].first; + const Key key1e = e->world_P_body_key_pairs()[k].first; + const Key key2own = world_P_body_key_pairs_[k].second; + const Key key2e = e->world_P_body_key_pairs()[k].second; + if (!(key1own == key1e) || !(key2own == key2e)) { + keyPairsEqual = false; + break; + } + } + } else { + keyPairsEqual = false; + } + + double extrinsicCalibrationEqual = true; + if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { + for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { + if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { + extrinsicCalibrationEqual = false; + break; + } + } + } else { + extrinsicCalibrationEqual = false; + } + + return e && Base::equals(p, tol) && K_all_ == e->calibration() && + alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; + } + + /** + * Compute jacobian F, E and error vector at a given linearization point + * @param values Values structure which must contain camera poses + * corresponding to keys involved in this factor + * @return Return arguments are the camera jacobians Fs (including the + * jacobian with respect to both body poses we interpolate from), the point + * Jacobian E, and the error vector b. Note that the jacobians are computed + * for a given point. + */ + void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, + const Values& values) const { + if (!this->result_) { + throw("computeJacobiansWithTriangulatedPoint"); + } else { // valid result: compute jacobians + size_t numViews = this->measured_.size(); + E = Matrix::Zero(2 * numViews, + 3); // a Point2 for each view (point jacobian) + b = Vector::Zero(2 * numViews); // a Point2 for each view + // intermediate Jacobians + Eigen::Matrix dProject_dPoseCam; + Eigen::Matrix dInterpPose_dPoseBody1, + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + Eigen::Matrix Ei; + + for (size_t i = 0; i < numViews; i++) { // for each camera/measurement + auto w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + auto w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = alphas_[i]; + // get interpolated pose: + auto w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor, + dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); + auto body_P_cam = body_P_sensors_[i]; + auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); + PinholeCamera camera(w_P_cam, *K_all_[i]); + + // get jacobians and error vector for current measurement + Point2 reprojectionError_i = + Point2(camera.project(*this->result_, dProject_dPoseCam, Ei) - + this->measured_.at(i)); + Eigen::Matrix J; // 2 x 12 + J.block(0, 0, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) + J.block(0, 6, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) + + // fit into the output structures + Fs.push_back(J); + size_t row = 2 * i; + b.segment(row) = -reprojectionError_i; + E.block(row, 0) = Ei; + } + } + } + + /// linearize and return a Hessianfactor that is an approximation of error(p) + boost::shared_ptr> createHessianFactor( + const Values& values, const double lambda = 0.0, + bool diagonalDamping = false) const { + // we may have multiple observation sharing the same keys (due to the + // rolling shutter interpolation), hence the number of unique keys may be + // smaller than 2 * nrMeasurements + size_t nrUniqueKeys = + this->keys_ + .size(); // note: by construction, keys_ only contains unique keys + + // Create structures for Hessian Factors + KeyVector js; + std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector gs(nrUniqueKeys); + + if (this->measured_.size() != + this->cameras(values).size()) // 1 observation per interpolated camera + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "measured_.size() inconsistent with input"); + + // triangulate 3D point at given linearization point + this->triangulateSafe(this->cameras(values)); + + if (!this->result_) { // failed: return "empty/zero" Hessian + if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { + for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) v = Vector::Zero(DimPose); + return boost::make_shared>(this->keys_, + Gs, gs, 0.0); + } else { + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); + } + } + // compute Jacobian given triangulated 3D Point + FBlocks Fs; + Matrix E; + Vector b; + this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values); + + // Whiten using noise model + this->noiseModel_->WhitenSystem(E, b); + for (size_t i = 0; i < Fs.size(); i++) + Fs[i] = this->noiseModel_->Whiten(Fs[i]); + + Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + + // Collect all the key pairs: these are the keys that correspond to the + // blocks in Fs (on which we apply the Schur Complement) + KeyVector nonuniqueKeys; + for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first); + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); + } + + // Build augmented Hessian (with last row/column being the information + // vector) Note: we need to get the augumented hessian wrt the unique keys + // in key_ + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>( + Fs, E, P, b, nonuniqueKeys, this->keys_); + + return boost::make_shared>( + this->keys_, augmentedHessianUniqueKeys); + } + + /** + * error calculates the error of the factor. + */ + double error(const Values& values) const override { + if (this->active(values)) { + return this->totalReprojectionError(this->cameras(values)); + } else { // else of active flag + return 0.0; + } + } + + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses + * corresponding to keys involved in this factor + * @return Cameras + */ + typename Base::Cameras cameras(const Values& values) const override { + size_t numViews = this->measured_.size(); + assert(numViews == K_all_.size()); + assert(numViews == alphas_.size()); + assert(numViews == body_P_sensors_.size()); + assert(numViews == world_P_body_key_pairs_.size()); + + typename Base::Cameras cameras; + for (size_t i = 0; i < numViews; i++) { // for each measurement + const Pose3& w_P_body1 = + values.at(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = + values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = alphas_[i]; + const Pose3& w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor); + const Pose3& body_P_cam = body_P_sensors_[i]; + const Pose3& w_P_cam = w_P_body.compose(body_P_cam); + cameras.emplace_back(w_P_cam, K_all_[i]); + } + return cameras; + } + + /** + * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for + * LM) + * @param values Values structure which must contain camera poses and + * extrinsic pose for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped( + const Values& values, const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different + // linear factors + switch (this->params_.linearizationMode) { + case HESSIAN: + return this->createHessianFactor(values, lambda); + default: + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: unknown linearization " + "mode"); + } + } + + /// linearize + boost::shared_ptr linearize( + const Values& values) const override { + return this->linearizeDamped(values); + } + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(K_all_); + } +}; +// end of class declaration + +/// traits +template +struct traits> + : public Testable> {}; + +} // namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 40d90d614..25be48b0f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -61,10 +61,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose + static const int DimBlock = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement) - typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > FBlocks; // vector of F blocks /** @@ -180,7 +180,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // get jacobians and error vector for current measurement StereoPoint2 reprojectionError_i = StereoPoint2( camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); - Eigen::Matrix J; // 3 x 12 + Eigen::Matrix J; // 3 x 12 J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) // if the right pixel is invalid, fix jacobians @@ -209,8 +209,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we // have a body key and an extrinsic calibration key for each measurement) size_t nrUniqueKeys = keys_.size(); - size_t nrNonuniqueKeys = world_P_body_keys_.size() - + body_P_cam_keys_.size(); // Create structures for Hessian Factors KeyVector js; @@ -246,81 +244,19 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // build augmented Hessian (with last row/column being the information vector) Matrix3 P; - Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + Cameras::ComputePointCovariance <3> (P, E, lambda, diagonalDamping); - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(Fs, E, P, b); - - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, 6); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessianUniqueKeys; - - // here we have to deal with the fact that some cameras may share the same extrinsic key - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix(augmentedHessian.selfadjointView())); - } else { // if multiple cameras share a calibration we have to rearrange - // the results of the Schur complement matrix - std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term - std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); - nonuniqueDims.back() = 1; - augmentedHessian = SymmetricBlockMatrix( - nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - - // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) - KeyVector nonuniqueKeys; - for (size_t i = 0; i < world_P_body_keys_.size(); i++) { - nonuniqueKeys.push_back(world_P_body_keys_.at(i)); - nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); - } - - // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) - std::map keyToSlotMap; - for (size_t k = 0; k < nrUniqueKeys; k++) { - keyToSlotMap[keys_[k]] = k; - } - - // initialize matrix to zero - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); - - // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // and populates an Hessian that only includes the unique keys (that is what we want to return) - for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows - Key key_i = nonuniqueKeys.at(i); - - // update information vector - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], nrUniqueKeys, - augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - - // update blocks - for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols - Key key_j = nonuniqueKeys.at(j); - if (i == j) { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); - } else { // (i < j) - if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], keyToSlotMap[key_j], - augmentedHessian.aboveDiagonalBlock(i, j)); - } else { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); - } - } - } - } - // update bottom right element of the matrix - augmentedHessianUniqueKeys.updateDiagonalBlock( - nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); + // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) + KeyVector nonuniqueKeys; + for (size_t i = 0; i < world_P_body_keys_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_keys_.at(i)); + nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); } + // but we need to get the augumented hessian wrt the unique keys in key_ + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b, + nonuniqueKeys, keys_); + return boost::make_shared < RegularHessianFactor > (keys_, augmentedHessianUniqueKeys); } diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp new file mode 100644 index 000000000..161c9aa55 --- /dev/null +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -0,0 +1,407 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ProjectionFactorRollingShutterRollingShutter.cpp + * @brief Unit tests for ProjectionFactorRollingShutter Class + * @author Luca Carlone + * @date July 2021 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std::placeholders; +using namespace std; +using namespace gtsam; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w = 640, h = 480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); + +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +using symbol_shorthand::L; +using symbol_shorthand::T; +using symbol_shorthand::X; + +// Convenience to define common variables across many tests +static Key poseKey1(X(1)); +static Key poseKey2(X(2)); +static Key pointKey(L(1)); +static double interp_params = 0.5; +static Point2 measurement(323.0, 240.0); +static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Constructor) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Equals) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + CHECK(assert_equal(factor1, factor2)); + } + { // factors are NOT equal (keys are different) + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey1, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal + } + { // factors are NOT equal (different interpolation) + ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1, + poseKey1, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1, + poseKey2, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal + } +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, EqualsWithTransform) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + CHECK(assert_equal(factor1, factor2)); + } + { // factors are NOT equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + Pose3 body_P_sensor2( + Rot3::RzRyRx(0.0, 0.0, 0.0), + Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor2); + CHECK(!assert_equal(factor1, factor2)); + } +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Error) { + { + // Create the factor with a measurement that is 3 pixels off in x + // Camera pose corresponds to the first camera + double t = 0.0; + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); + + // Set the linearization point + Pose3 pose1(Rot3(), Point3(0, 0, -6)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = Vector2(-3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } + { + // Create the factor with a measurement that is 3 pixels off in x + // Camera pose is actually interpolated now + double t = 0.5; + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); + + // Set the linearization point + Pose3 pose1(Rot3(), Point3(0, 0, -8)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = Vector2(-3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } + { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + PinholeCamera camera(poseInterp, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is zero + Vector expectedError = Vector2(0.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, ErrorWithTransform) { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is zero + Vector expectedError = Vector2(0.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Jacobian) { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + PinholeCamera camera(poseInterp, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); + + // Expected Jacobians via numerical derivatives + Matrix H1Expected = numericalDerivative31( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); + + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); + + Matrix H3Expected = numericalDerivative33( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); + + CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-5)); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, JacobianWithTransform) { + // Create measurement by projecting 3D landmark + double t = 0.6; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); + + // Expected Jacobians via numerical derivatives + Matrix H1Expected = numericalDerivative31( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); + + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); + + Matrix H3Expected = numericalDerivative33( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); + + CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-5)); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, cheirality) { + // Create measurement by projecting 3D landmark behind camera + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + PinholeCamera camera(poseInterp, *K); + Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + Point2 measured = Point2(0.0, 0.0); // project would throw an exception + { // check that exception is thrown if we set throwCheirality = true + bool throwCheirality = true; + bool verboseCheirality = true; + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); + CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point), + CheiralityException); + } + { // check that exception is NOT thrown if we set throwCheirality = false, + // and outputs are correct + bool throwCheirality = false; // default + bool verboseCheirality = false; // default + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); + + // Use the factor to calculate the error + Matrix H1Actual, H2Actual, H3Actual; + Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, + H2Actual, H3Actual)); + + // The expected error is zero + Vector expectedError = Vector2::Constant( + 2.0 * K->fx()); // this is what we return when point is behind camera + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5)); + } +#else + { + // everything is well defined, hence this matches the test "Jacobian" above: + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); + + // Expected Jacobians via numerical derivatives + Matrix H1Expected = numericalDerivative31( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); + + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); + + Matrix H3Expected = numericalDerivative33( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); + + CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-5)); + } +#endif +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp new file mode 100644 index 000000000..0b94d2c3f --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -0,0 +1,1145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSmartProjectionPoseFactorRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class + * @author Luca Carlone + * @date July 2021 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "gtsam/slam/tests/smartFactorScenarios.h" +#define DISABLE_TIMING + +using namespace gtsam; +using namespace boost::assign; +using namespace std::placeholders; + +static const double rankTol = 1.0; +// Create a noise model for the pixel error +static const double sigma = 0.1; +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); + +// Convenience for named keys +using symbol_shorthand::L; +using symbol_shorthand::X; + +// tests data +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); +static Symbol x4('X', 4); +static Symbol l0('L', 0); +static Pose3 body_P_sensor = + Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0)); + +static Point2 measurement1(323.0, 240.0); +static Point2 measurement2(200.0, 220.0); +static Point2 measurement3(320.0, 10.0); +static double interp_factor = 0.5; +static double interp_factor1 = 0.3; +static double interp_factor2 = 0.4; +static double interp_factor3 = 0.5; + +/* ************************************************************************* */ +// default Cal3_S2 poses with rolling shutter effect +namespace vanillaPoseRS { +typedef PinholePose Camera; +static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +Pose3 interp_pose1 = interpolate(level_pose, pose_right, interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right, pose_above, interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above, level_pose, interp_factor3); +Camera cam1(interp_pose1, sharedK); +Camera cam2(interp_pose2, sharedK); +Camera cam3(interp_pose3, sharedK); +} // namespace vanillaPoseRS + +LevenbergMarquardtParams lmParams; +typedef SmartProjectionPoseFactorRollingShutter> + SmartFactorRS; + +/* ************************************************************************* */ +TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); +} + +/* ************************************************************************* */ +TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorRS factor1(model, params); +} + +/* ************************************************************************* */ +TEST(SmartProjectionPoseFactorRollingShutter, add) { + using namespace vanillaPose; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); +} + +/* ************************************************************************* */ +TEST(SmartProjectionPoseFactorRollingShutter, Equals) { + using namespace vanillaPose; + + // create fake measurements + Point2Vector measurements; + measurements.push_back(measurement1); + measurements.push_back(measurement2); + measurements.push_back(measurement3); + + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x4)); + + std::vector> intrinsicCalibrations; + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + + std::vector extrinsicCalibrations; + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + // create by adding a batch of measurements with a bunch of calibrations + SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); + factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, + extrinsicCalibrations); + + // create by adding a batch of measurements with a single calibrations + SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); + factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); + + { // create equal factors and show equal returns true + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // create slightly different factors (different keys) and show equal + // returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x2, interp_factor2, sharedK, + body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); + } + { // create slightly different factors (different extrinsics) and show equal + // returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, + body_P_sensor * body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); + } + { // create slightly different factors (different interp factors) and show + // equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor1, sharedK, + body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); + } +} + +static const int DimBlock = 12; ///< size of the variable stacking 2 poses from + ///< which the observation pose is interpolated +static const int ZDim = 2; ///< Measurement dimension (Point2) +typedef Eigen::Matrix + MatrixZD; // F blocks (derivatives wrt camera) +typedef std::vector> + FBlocks; // vector of F blocks + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { + using namespace vanillaPoseRS; + + // Project two landmarks into two cameras + Point2 level_uv = cam1.project(landmark1); + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorId = Pose3::identity(); + + SmartFactorRS factor(model); + factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + // Check triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal( + landmark1, + *point)); // check triangulation result matches expected 3D landmark + + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = factor1.evaluateError( + level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, body_P_sensorId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = factor2.evaluateError( + pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { + // also includes non-identical extrinsic calibration + using namespace vanillaPoseRS; + + // Project two landmarks into two cameras + Point2 pixelError(0.5, 1.0); + Point2 level_uv = cam1.project(landmark1) + pixelError; + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorNonId = body_P_sensor; + + SmartFactorRS factor(model); + factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, + body_P_sensorNonId); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + // Perform triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + EXPECT(point.valid()); // check triangulated point is valid + Point3 landmarkNoisy = *point; + + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorNonId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = + factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, + expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, + body_P_sensorNonId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = + factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, + expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + + // Check errors + double actualError = factor.error(values); // from smart factor + NonlinearFactorGraph nfg; + nfg.add(factor1); + nfg.add(factor2); + values.insert(l0, landmarkNoisy); + double expectedError = nfg.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { + // here we replicate a test in SmartProjectionPoseFactor by setting + // interpolation factors to 0 and 1 (such that the rollingShutter measurements + // falls back to standard pixel measurements) Note: this is a quite extreme + // test since in typical camera you would not have more than 1 measurement per + // landmark at each interpolated pose + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + EXPECT(smartFactor1->triangulateSafe(cameras)); + EXPECT(!smartFactor1->isDegenerate()); + EXPECT(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + EXPECT(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // After eliminating the point, A1 and A2 contain 2-rank information on + // cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; // very large + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(true); + + SmartFactorRS smartFactor1(model, params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor2(model, params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor3(model, params); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_landmarkDistance) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 2; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactorRS smartFactor1(model, params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor2(model, params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor3(model, params); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + + // All factors are disabled (due to the distance threshold) and pose should + // remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_dynamicOutlierRejection) { + using namespace vanillaPoseRS; + // add fourth landmark + Point3 landmark4(5, -0.5, 1); + + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, + measurements_lmk4; + // Project 4 landmarks into cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); + measurements_lmk4.at(0) = + measurements_lmk4.at(0) + Point2(10, 10); // add outlier + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = + 3; // max 3 pixel of average reprojection error + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + params.setEnableEPI(false); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); + smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3( + Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, + 0.01)); // smaller noise, otherwise outlier rejection will kick in + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization + // point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use the factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 3, 6 * 3); + Matrix E = Matrix::Zero(2 * 3, 3); + Vector b = Vector::Zero(6); + + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, + model, x1, x2, l0, sharedK); + Matrix H1Actual, H2Actual, H3Actual; + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(0, 0) = H1Actual; + F.block<2, 6>(0, 6) = H2Actual; + E.block<2, 3>(0, 0) = H3Actual; + + ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, + model, x2, x3, l0, sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(2, 6) = H1Actual; + F.block<2, 6>(2, 12) = H2Actual; + E.block<2, 3>(2, 0) = H3Actual; + + ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, + model, x3, x1, l0, sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(4, 12) = H1Actual; + F.block<2, 6>(4, 0) = H2Actual; + E.block<2, 3>(4, 0) = H3Actual; + + // whiten + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from + // Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactorsRS; + nfg_projFactorsRS.add(factor11); + nfg_projFactorsRS.add(factor12); + nfg_projFactorsRS.add(factor13); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel + // measurements of the same landmark at a single pose, a setup that occurs in + // multi-camera systems + + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create redundant measurements: + Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + key_pairs.push_back(std::make_pair(x1, x2)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + interp_factors.push_back(interp_factor1); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, + sharedK); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization + // point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use standard ProjectionFactorRollingShutter factor to calculate the + // Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(8); + + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], + interp_factor1, model, x1, x2, l0, + sharedK); + Matrix H1Actual, H2Actual, H3Actual; + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(0, 0) = H1Actual; + F.block<2, 6>(0, 6) = H2Actual; + E.block<2, 3>(0, 0) = H3Actual; + + ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], + interp_factor2, model, x2, x3, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(2, 6) = H1Actual; + F.block<2, 6>(2, 12) = H2Actual; + E.block<2, 3>(2, 0) = H3Actual; + + ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], + interp_factor3, model, x3, x1, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(4, 12) = H1Actual; + F.block<2, 6>(4, 0) = H2Actual; + E.block<2, 3>(4, 0) = H3Actual; + + ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], + interp_factor1, model, x1, x2, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); + F.block<2, 6>(6, 0) = H1Actual; + F.block<2, 6>(6, 6) = H2Actual; + E.block<2, 3>(6, 0) = H3Actual; + + // whiten + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from + // Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactorsRS; + nfg_projFactorsRS.add(factor11); + nfg_projFactorsRS.add(factor12); + nfg_projFactorsRS.add(factor13); + nfg_projFactorsRS.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_measurementsFromSamePose) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + // For first factor, we create redundant measurement (taken by the same keys + // as factor 1, to make sure the redundancy in the keys does not create + // problems) + Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + std::vector> key_pairs_redundant = key_pairs; + key_pairs_redundant.push_back( + key_pairs.at(0)); // we readd the first pair of keys + std::vector interp_factors_redundant = interp_factors; + interp_factors_redundant.push_back( + interp_factors.at(0)); // we readd the first interp factor + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, + interp_factors_redundant, sharedK); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); +} + +#ifndef DISABLE_TIMING +#include +// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: +// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 +// children, min: 0 max: 0) +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, timing) { + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + size_t nrTests = 1000; + + for (size_t i = 0; i < nrTests; i++) { + SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SF_RS_LINEARIZE); + smartFactorRS->linearize(values); + gttoc_(SF_RS_LINEARIZE); + } + + for (size_t i = 0; i < nrTests; i++) { + SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + smartFactor->add(measurements_lmk1[0], x1); + smartFactor->add(measurements_lmk1[1], x2); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(RS_LINEARIZE); + smartFactor->linearize(values); + gttoc_(RS_LINEARIZE); + } + tictoc_print_(); +} +#endif + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 7b8347da5..2b3ed3852 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -43,6 +43,7 @@ set(ignore gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector + gtsam::Point2Pairs gtsam::Point3Pairs gtsam::Pose3Pairs gtsam::Pose3Vector @@ -61,10 +62,13 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i + ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) +set(GTSAM_PYTHON_TARGET gtsam_py) +set(GTSAM_PYTHON_UNSTABLE_TARGET gtsam_unstable_py) -pybind_wrap(gtsam_py # target +pybind_wrap(${GTSAM_PYTHON_TARGET} # target "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp "gtsam" # module_name @@ -76,7 +80,7 @@ pybind_wrap(gtsam_py # target ON # use_boost ) -set_target_properties(gtsam_py PROPERTIES +set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam" @@ -96,7 +100,7 @@ create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}") # Add gtsam as a dependency to the install target -set(GTSAM_PYTHON_DEPENDENCIES gtsam_py) +set(GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_TARGET}) if(GTSAM_UNSTABLE_BUILD_PYTHON) @@ -120,7 +124,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) - pybind_wrap(gtsam_unstable_py # target + pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp "gtsam_unstable" # module_name @@ -132,7 +136,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) ON # use_boost ) - set_target_properties(gtsam_unstable_py PROPERTIES + set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam_unstable" @@ -148,7 +152,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) "${GTSAM_UNSTABLE_MODULE_PATH}") # Add gtsam_unstable to the install target - list(APPEND GTSAM_PYTHON_DEPENDENCIES gtsam_unstable_py) + list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_UNSTABLE_TARGET}) endif() @@ -165,6 +169,6 @@ add_custom_target( COMMAND ${CMAKE_COMMAND} -E env # add package to python path so no need to install "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} -m unittest discover + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} - WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests) + WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests") diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 70a00c3dc..d00e47b65 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1,6 +1,12 @@ -from . import utils -from .gtsam import * -from .utils import findExampleDataFile +"""Module definition file for GTSAM""" + +# pylint: disable=import-outside-toplevel, global-variable-not-assigned, possibly-unused-variable, import-error, import-self + +import sys + +from gtsam import gtsam, utils +from gtsam.gtsam import * +from gtsam.utils import findExampleDataFile def _init(): @@ -13,7 +19,7 @@ def _init(): def Point2(x=np.nan, y=np.nan): """Shim for the deleted Point2 type.""" if isinstance(x, np.ndarray): - assert x.shape == (2,), "Point2 takes 2-vector" + assert x.shape == (2, ), "Point2 takes 2-vector" return x # "copy constructor" return np.array([x, y], dtype=float) @@ -22,7 +28,7 @@ def _init(): def Point3(x=np.nan, y=np.nan, z=np.nan): """Shim for the deleted Point3 type.""" if isinstance(x, np.ndarray): - assert x.shape == (3,), "Point3 takes 3-vector" + assert x.shape == (3, ), "Point3 takes 3-vector" return x # "copy constructor" return np.array([x, y, z], dtype=float) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index bb707a8f5..86613234d 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -10,28 +10,30 @@ A script validating and demonstrating the ImuFactor inference. Author: Frank Dellaert, Varun Agrawal """ +# pylint: disable=no-name-in-module,unused-import,arguments-differ + from __future__ import print_function import argparse import math -import gtsam import matplotlib.pyplot as plt import numpy as np +from mpl_toolkits.mplot3d import Axes3D + +import gtsam from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 -from mpl_toolkits.mplot3d import Axes3D from PreintegrationExample import POSES_FIG, PreintegrationExample BIAS_KEY = B(0) - np.set_printoptions(precision=3, suppress=True) class ImuFactorExample(PreintegrationExample): - + """Class to run example of the Imu Factor.""" def __init__(self, twist_scenario="sick_twist"): self.velocity = np.array([2, 0, 0]) self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) @@ -42,9 +44,8 @@ class ImuFactorExample(PreintegrationExample): zero_twist=(np.zeros(3), np.zeros(3)), forward_twist=(np.zeros(3), self.velocity), loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity), - sick_twist=(np.array([math.radians(30), -math.radians(30), 0]), - self.velocity) - ) + sick_twist=(np.array([math.radians(30), -math.radians(30), + 0]), self.velocity)) accBias = np.array([-0.3, 0.1, 0.2]) gyroBias = np.array([0.1, 0.3, -0.1]) @@ -55,19 +56,44 @@ class ImuFactorExample(PreintegrationExample): bias, dt) def addPrior(self, i, graph): + """Add priors at time step `i`.""" state = self.scenario.navState(i) - graph.push_back(gtsam.PriorFactorPose3( - X(i), state.pose(), self.priorNoise)) - graph.push_back(gtsam.PriorFactorVector( - V(i), state.velocity(), self.velNoise)) + graph.push_back( + gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) + graph.push_back( + gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise)) + + def optimize(self, graph, initial): + """Optimize using Levenberg-Marquardt optimization.""" + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("SUMMARY") + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + return result + + def plot(self, result): + """Plot resulting poses.""" + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + plot_pose3(POSES_FIG + 1, pose_i, 1) + i += 1 + plt.title("Estimated Trajectory") + + gtsam.utils.plot.set_axes_equal(POSES_FIG + 1) + + print("Bias Values", result.atConstantBias(BIAS_KEY)) + + plt.ioff() + plt.show() def run(self, T=12, compute_covariances=False, verbose=True): + """Main runner.""" graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) - T = 12 num_poses = T # assumes 1 factor per second initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) @@ -91,14 +117,13 @@ class ImuFactorExample(PreintegrationExample): if k % 10 == 0: self.plotImu(t, measuredOmega, measuredAcc) - if (k+1) % int(1 / self.dt) == 0: + if (k + 1) % int(1 / self.dt) == 0: # Plot every second self.plotGroundTruthPose(t, scale=1) plt.title("Ground Truth Trajectory") # create IMU factor every second - factor = gtsam.ImuFactor(X(i), V(i), - X(i + 1), V(i + 1), + factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) graph.push_back(factor) @@ -108,34 +133,34 @@ class ImuFactorExample(PreintegrationExample): pim.resetIntegration() - rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1) - translationNoise = gtsam.Point3(*np.random.randn(3)*1) + rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1) + translationNoise = gtsam.Point3(*np.random.randn(3) * 1) poseNoise = gtsam.Pose3(rotationNoise, translationNoise) actual_state_i = self.scenario.navState(t + self.dt) print("Actual state at {0}:\n{1}".format( - t+self.dt, actual_state_i)) + t + self.dt, actual_state_i)) noisy_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), - actual_state_i.velocity() + np.random.randn(3)*0.1) + actual_state_i.velocity() + np.random.randn(3) * 0.1) - initial.insert(X(i+1), noisy_state_i.pose()) - initial.insert(V(i+1), noisy_state_i.velocity()) + initial.insert(X(i + 1), noisy_state_i.pose()) + initial.insert(V(i + 1), noisy_state_i.velocity()) i += 1 # add priors on end self.addPrior(num_poses - 1, graph) - initial.print_("Initial values:") + initial.print("Initial values:") - # optimize using Levenberg-Marquardt optimization - params = gtsam.LevenbergMarquardtParams() - params.setVerbosityLM("SUMMARY") - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) - result = optimizer.optimize() + result = self.optimize(graph, initial) - result.print_("Optimized values:") + result.print("Optimized values:") + print("------------------") + print(graph.error(initial)) + print(graph.error(result)) + print("------------------") if compute_covariances: # Calculate and print marginal covariances @@ -148,33 +173,26 @@ class ImuFactorExample(PreintegrationExample): print("Covariance on vel {}:\n{}\n".format( i, marginals.marginalCovariance(V(i)))) - # Plot resulting poses - i = 0 - while result.exists(X(i)): - pose_i = result.atPose3(X(i)) - plot_pose3(POSES_FIG+1, pose_i, 1) - i += 1 - plt.title("Estimated Trajectory") - - gtsam.utils.plot.set_axes_equal(POSES_FIG+1) - - print("Bias Values", result.atConstantBias(BIAS_KEY)) - - plt.ioff() - plt.show() + self.plot(result) if __name__ == '__main__': parser = argparse.ArgumentParser("ImuFactorExample.py") parser.add_argument("--twist_scenario", default="sick_twist", - choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist")) - parser.add_argument("--time", "-T", default=12, - type=int, help="Total time in seconds") + choices=("zero_twist", "forward_twist", "loop_twist", + "sick_twist")) + parser.add_argument("--time", + "-T", + default=12, + type=int, + help="Total time in seconds") parser.add_argument("--compute_covariances", - default=False, action='store_true') + default=False, + action='store_true') parser.add_argument("--verbose", default=False, action='store_true') args = parser.parse_args() - ImuFactorExample(args.twist_scenario).run( - args.time, args.compute_covariances, args.verbose) + ImuFactorExample(args.twist_scenario).run(args.time, + args.compute_covariances, + args.verbose) diff --git a/python/gtsam/examples/SFMdata.py b/python/gtsam/examples/SFMdata.py index 6ac9c5726..ad414a256 100644 --- a/python/gtsam/examples/SFMdata.py +++ b/python/gtsam/examples/SFMdata.py @@ -5,36 +5,39 @@ A structure-from-motion example with landmarks """ # pylint: disable=invalid-name, E1101 +from typing import List + import numpy as np import gtsam +from gtsam import Cal3_S2, Point3, Pose3 -def createPoints(): +def createPoints() -> List[Point3]: # Create the set of ground-truth landmarks - points = [gtsam.Point3(10.0, 10.0, 10.0), - gtsam.Point3(-10.0, 10.0, 10.0), - gtsam.Point3(-10.0, -10.0, 10.0), - gtsam.Point3(10.0, -10.0, 10.0), - gtsam.Point3(10.0, 10.0, -10.0), - gtsam.Point3(-10.0, 10.0, -10.0), - gtsam.Point3(-10.0, -10.0, -10.0), - gtsam.Point3(10.0, -10.0, -10.0)] + points = [ + Point3(10.0, 10.0, 10.0), + Point3(-10.0, 10.0, 10.0), + Point3(-10.0, -10.0, 10.0), + Point3(10.0, -10.0, 10.0), + Point3(10.0, 10.0, -10.0), + Point3(-10.0, 10.0, -10.0), + Point3(-10.0, -10.0, -10.0), + Point3(10.0, -10.0, -10.0), + ] return points -def createPoses(K): - # Create the set of ground-truth poses +def createPoses(K: Cal3_S2) -> List[Pose3]: + """Generate a set of ground-truth camera poses arranged in a circle about the origin.""" radius = 40.0 height = 10.0 - angles = np.linspace(0, 2*np.pi, 8, endpoint=False) + angles = np.linspace(0, 2 * np.pi, 8, endpoint=False) up = gtsam.Point3(0, 0, 1) target = gtsam.Point3(0, 0, 0) poses = [] for theta in angles: - position = gtsam.Point3(radius*np.cos(theta), - radius*np.sin(theta), - height) + position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), height) camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) poses.append(camera.pose()) return poses diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index 93e7ffbaf..b760e4eb5 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -1,8 +1,8 @@ /** - * @file gtsam.cpp + * @file {module_name}.cpp * @brief The auto-generated wrapper C++ source code. - * @author Duy-Nguyen Ta, Fan Jiang, Matthew Sklar - * @date Aug. 18, 2020 + * @author Duy-Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal + * @date Aug. 18, 2020 * * ** THIS FILE IS AUTO-GENERATED, DO NOT MODIFY! ** */ diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h new file mode 100644 index 000000000..56a07cfdd --- /dev/null +++ b/python/gtsam/preamble/basis.h @@ -0,0 +1,14 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +#include diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 498c7dc58..35fe2a577 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -10,9 +10,18 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ +#include + +// Support for binding boost::optional types in C++11. +// https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html +namespace pybind11 { namespace detail { + template + struct type_caster> : optional_caster> {}; +}} PYBIND11_MAKE_OPAQUE( std::vector>); +PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/specializations/basis.h b/python/gtsam/specializations/basis.h new file mode 100644 index 000000000..da8842eaf --- /dev/null +++ b/python/gtsam/specializations/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index e11d3cc4f..a492ce8eb 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -14,6 +14,7 @@ py::bind_vector< std::vector>>( m_, "Point2Vector"); +py::bind_vector>(m_, "Point2Pairs"); py::bind_vector>(m_, "Point3Pairs"); py::bind_vector>(m_, "Pose3Pairs"); py::bind_vector>(m_, "Pose3Vector"); diff --git a/python/gtsam/tests/test_Factors.py b/python/gtsam/tests/test_Factors.py new file mode 100644 index 000000000..3ec8648a4 --- /dev/null +++ b/python/gtsam/tests/test_Factors.py @@ -0,0 +1,34 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for various factors. + +Author: Varun Agrawal +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestNonlinearEquality2Factor(GtsamTestCase): + """ + Test various instantiations of NonlinearEquality2. + """ + def test_point3(self): + """Test for Point3 version.""" + factor = gtsam.NonlinearEquality2Point3(0, 1) + error = factor.evaluateError(gtsam.Point3(0, 0, 0), + gtsam.Point3(0, 0, 0)) + + np.testing.assert_allclose(error, np.zeros(3)) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index e9234a43b..e2561ae52 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -17,8 +17,9 @@ import unittest import gtsam from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer, - GaussNewtonParams, LevenbergMarquardtOptimizer, - LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, + GaussNewtonParams, GncLMParams, GncLMOptimizer, + LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase @@ -77,6 +78,12 @@ class TestScenario(GtsamTestCase): dlParams.setOrdering(ordering) actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() self.assertAlmostEqual(0, fg.error(actual3)) + + # Graduated Non-Convexity (GNC) + gncParams = GncLMParams() + actual4 = GncLMOptimizer(fg, initial_values, gncParams).optimize() + self.assertAlmostEqual(0, fg.error(actual4)) + if __name__ == "__main__": diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 9652b594a..e5ffbad7d 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -6,27 +6,64 @@ All Rights Reserved See LICENSE for the license information Pose2 unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Author: Frank Dellaert & Duy Nguyen Ta & John Lambert """ import unittest import numpy as np import gtsam -from gtsam import Pose2 +from gtsam import Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" - - def test_adjoint(self): + def test_adjoint(self) -> None: """Test adjoint method.""" xi = np.array([1, 2, 3]) expected = np.dot(Pose2.adjointMap_(xi), xi) actual = Pose2.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) + def test_align(self) -> None: + """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. + + Two point clouds represent horseshoe-shapes of the same size, just rotated and translated: + + | X---X + | | + | X---X + ------------------ + | + | + O | O + | | | + O---O + """ + pts_a = [ + Point2(3, 1), + Point2(1, 1), + Point2(1, 3), + Point2(3, 3), + ] + pts_b = [ + Point2(1, -3), + Point2(1, -5), + Point2(-1, -5), + Point2(-1, -3), + ] + + # fmt: on + ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) + bTa = gtsam.align(ab_pairs) + aTb = bTa.inverse() + assert aTb is not None + + for pt_a, pt_b in zip(pts_a, pts_b): + pt_a_ = aTb.transformFrom(pt_b) + assert np.allclose(pt_a, pt_a_) + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_Pose3SLAMExample.py b/python/gtsam/tests/test_Pose3SLAMExample.py index fce171b55..cb5e3b226 100644 --- a/python/gtsam/tests/test_Pose3SLAMExample.py +++ b/python/gtsam/tests/test_Pose3SLAMExample.py @@ -15,14 +15,14 @@ import numpy as np import gtsam from gtsam.utils.test_case import GtsamTestCase -from gtsam.utils.circlePose3 import * +from gtsam.utils.circlePose3 import circlePose3 class TestPose3SLAMExample(GtsamTestCase): - def test_Pose3SLAMExample(self): + def test_Pose3SLAMExample(self) -> None: # Create a hexagon of poses - hexagon = circlePose3(6, 1.0) + hexagon = circlePose3(numPoses=6, radius=1.0) p0 = hexagon.atPose3(0) p1 = hexagon.atPose3(1) @@ -31,7 +31,7 @@ class TestPose3SLAMExample(GtsamTestCase): fg.add(gtsam.NonlinearEqualityPose3(0, p0)) delta = p0.between(p1) covariance = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180])) + np.array([0.05, 0.05, 0.05, np.deg2rad(5.), np.deg2rad(5.), np.deg2rad(5.)])) fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance)) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py new file mode 100644 index 000000000..a1ce01ba2 --- /dev/null +++ b/python/gtsam/tests/test_Rot3.py @@ -0,0 +1,2037 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +See LICENSE for the license information +Rot3 unit tests. +Author: John Lambert +""" +# pylint: disable=no-name-in-module + +import unittest + +import numpy as np + +import gtsam +from gtsam import Rot3 +from gtsam.utils.test_case import GtsamTestCase + + +R1_R2_pairs = [ + ( + [ + [0.994283, -0.10356, 0.0260251], + [0.102811, 0.994289, 0.0286205], + [-0.0288404, -0.0257812, 0.999251], + ], + [ + [-0.994235, 0.0918291, -0.0553602], + [-0.0987317, -0.582632, 0.806718], + [0.0418251, 0.807532, 0.588339], + ], + ), + ( + [ + [0.999823, -0.000724729, 0.0187896], + [0.00220672, 0.996874, -0.0789728], + [-0.0186736, 0.0790003, 0.9967], + ], + [ + [-0.99946, -0.0155217, -0.0289749], + [-0.0306159, 0.760422, 0.648707], + [0.0119641, 0.649244, -0.760487], + ], + ), + ( + [ + [0.999976, 0.00455542, -0.00529608], + [-0.00579633, 0.964214, -0.265062], + [0.00389908, 0.265086, 0.964217], + ], + [ + [-0.999912, -0.0123323, -0.00489179], + [-0.00739095, 0.21159, 0.977331], + [-0.0110179, 0.977281, -0.211663], + ], + ), + ( + [ + [0.998801, 0.0449026, 0.019479], + [-0.0448727, 0.998991, -0.00197348], + [-0.0195479, 0.00109704, 0.999808], + ], + [ + [-0.999144, -0.0406154, -0.00800012], + [0.0406017, -0.999174, 0.00185875], + [-0.00806909, 0.00153352, 0.999966], + ], + ), + ( + [ + [0.587202, 0.00034062, -0.80944], + [0.394859, 0.872825, 0.286815], + [0.706597, -0.488034, 0.51239], + ], + [ + [-0.999565, -0.028095, -0.00905389], + [0.0192863, -0.853838, 0.520182], + [-0.0223455, 0.519782, 0.854007], + ], + ), + ( + [ + [0.998798, 0.0370584, 0.0320815], + [-0.0355966, 0.998353, -0.0449943], + [-0.033696, 0.0437982, 0.998472], + ], + [ + [-0.999942, -0.010745, -0.00132538], + [-0.000998705, -0.0304045, 0.999538], + [-0.0107807, 0.999481, 0.0303914], + ], + ), + ( + [ + [0.998755, 0.00708291, -0.0493744], + [-0.00742097, 0.99995, -0.00666709], + [0.0493247, 0.0070252, 0.998758], + ], + [ + [-0.998434, 0.0104672, 0.0549825], + [0.0115323, 0.999751, 0.0190859], + [-0.0547691, 0.01969, -0.998307], + ], + ), + ( + [ + [0.990471, 0.0997485, -0.0949595], + [-0.117924, 0.970427, -0.210631], + [0.0711411, 0.219822, 0.972943], + ], + [ + [-0.99192, -0.125627, 0.0177888], + [0.126478, -0.967866, 0.217348], + [-0.0100874, 0.217839, 0.975933], + ], + ), + ( + [ + [-0.780894, -0.578319, -0.236116], + [0.34478, -0.0838381, -0.934932], + [0.520894, -0.811491, 0.264862], + ], + [ + [-0.99345, 0.00261746, -0.114244], + [-0.112503, 0.152922, 0.981815], + [0.0200403, 0.988236, -0.151626], + ], + ), + ( + [ + [0.968425, 0.0466097, 0.244911], + [-0.218867, 0.629346, 0.745668], + [-0.119378, -0.775726, 0.619676], + ], + [ + [-0.971208, 0.00666431, -0.238143], + [0.0937886, 0.929584, -0.35648], + [0.218998, -0.368551, -0.903444], + ], + ), + ( + [ + [0.998512, 0.0449168, -0.0309146], + [-0.0344032, 0.958823, 0.281914], + [0.0423043, -0.280431, 0.958941], + ], + [ + [-0.999713, 0.00732431, 0.0228168], + [-0.00759688, 0.806166, -0.59164], + [-0.0227275, -0.591644, -0.805879], + ], + ), + ( + [ + [0.981814, 0.00930728, 0.189617], + [-0.0084101, 0.999949, -0.00553563], + [-0.189659, 0.00384026, 0.981843], + ], + [ + [-0.981359, 0.00722349, -0.192051], + [0.00148564, 0.999549, 0.0300036], + [0.192182, 0.0291591, -0.980927], + ], + ), + ( + [ + [0.972544, -0.215591, 0.0876242], + [0.220661, 0.973915, -0.0529018], + [-0.0739333, 0.0707846, 0.994748], + ], + [ + [-0.971294, 0.215675, -0.100371], + [-0.23035, -0.747337, 0.62324], + [0.0594069, 0.628469, 0.775564], + ], + ), + ( + [ + [0.989488, 0.0152447, 0.143808], + [-0.0160974, 0.999859, 0.00476753], + [-0.143715, -0.00703235, 0.989594], + ], + [ + [-0.988492, 0.0124362, -0.150766], + [0.00992423, 0.999799, 0.0174037], + [0.150952, 0.0157072, -0.988417], + ], + ), + ( + [ + [0.99026, 0.109934, -0.0854388], + [-0.0973012, 0.985345, 0.140096], + [0.099588, -0.130418, 0.986445], + ], + [ + [-0.994239, 0.0206112, 0.1052], + [0.0227944, 0.999548, 0.0195944], + [-0.104748, 0.0218794, -0.994259], + ], + ), + ( + [ + [0.988981, 0.132742, -0.0655406], + [-0.113134, 0.963226, 0.243712], + [0.0954813, -0.233612, 0.96763], + ], + [ + [-0.989473, -0.144453, 0.00888153], + [0.112318, -0.727754, 0.67658], + [-0.0912697, 0.670455, 0.736317], + ], + ), + ( + [ + [0.13315, -0.722685, 0.678231], + [0.255831, 0.686195, 0.680946], + [-0.957508, 0.0828446, 0.276252], + ], + [ + [-0.233019, 0.0127274, -0.97239], + [-0.0143824, 0.99976, 0.0165321], + [0.972367, 0.0178377, -0.23278], + ], + ), + ( + [ + [0.906305, -0.0179617, -0.422243], + [0.0246095, 0.999644, 0.0102984], + [0.421908, -0.0197247, 0.906424], + ], + [ + [-0.90393, 0.0136293, 0.427466], + [0.0169755, 0.999848, 0.0040176], + [-0.427346, 0.0108879, -0.904024], + ], + ), + ( + [ + [0.999808, 0.0177784, -0.00826505], + [-0.0177075, 0.999806, 0.00856939], + [0.0084158, -0.00842139, 0.999929], + ], + [ + [-0.999901, -0.0141114, 0.00072392], + [0.00130602, -0.0413336, 0.999145], + [-0.0140699, 0.999047, 0.0413473], + ], + ), + ( + [ + [0.985811, -0.161425, 0.0460375], + [0.154776, 0.980269, 0.12295], + [-0.0649764, -0.11408, 0.991344], + ], + [ + [-0.985689, 0.137931, -0.09692], + [-0.162627, -0.626622, 0.762168], + [0.0443951, 0.767022, 0.640085], + ], + ), + ( + [ + [0.956652, -0.0116044, 0.291001], + [0.05108, 0.990402, -0.128428], + [-0.286718, 0.137726, 0.948064], + ], + [ + [-0.956189, 0.00996594, -0.292585], + [-0.0397033, 0.985772, 0.16333], + [0.29005, 0.167791, -0.942189], + ], + ), + ( + [ + [0.783763, -0.0181248, -0.620796], + [-0.0386421, 0.996214, -0.0778717], + [0.619857, 0.0850218, 0.780095], + ], + [ + [-0.780275, 0.0093644, 0.625368], + [-0.0221791, 0.998845, -0.0426297], + [-0.625045, -0.0471329, -0.779165], + ], + ), + ( + [ + [0.890984, 0.0232464, -0.453439], + [-0.0221215, 0.999725, 0.00778511], + [0.453495, 0.00309433, 0.891253], + ], + [ + [-0.890178, 0.0290103, 0.45469], + [0.0337152, 0.999429, 0.0022403], + [-0.454366, 0.0173244, -0.890648], + ], + ), + ( + [ + [0.998177, -0.0119546, 0.0591504], + [0.00277494, 0.988238, 0.152901], + [-0.0602825, -0.152458, 0.98647], + ], + [ + [-0.997444, 0.00871865, -0.0709414], + [0.0197108, 0.987598, -0.155762], + [0.0687035, -0.156762, -0.985246], + ], + ), + ( + [ + [0.985214, 0.164929, 0.0463837], + [-0.166966, 0.984975, 0.0441225], + [-0.0384096, -0.0512146, 0.997949], + ], + [ + [-0.999472, -0.000819214, -0.0325087], + [-0.00344291, 0.99673, 0.0807324], + [0.0323362, 0.0808019, -0.996206], + ], + ), + ( + [ + [0.998499, 0.0465241, 0.0288955], + [-0.0454764, 0.99832, -0.0359142], + [-0.0305178, 0.0345463, 0.998937], + ], + [ + [-0.999441, 0.00412484, -0.0332105], + [0.00374685, 0.999928, 0.0114307], + [0.0332552, 0.0112999, -0.999384], + ], + ), + ( + [ + [0.10101, -0.943239, -0.316381], + [0.841334, -0.0887423, 0.533182], + [-0.530994, -0.320039, 0.784615], + ], + [ + [-0.725665, 0.0153749, -0.687878], + [-0.304813, 0.889109, 0.34143], + [0.616848, 0.457438, -0.640509], + ], + ), + ( + [ + [0.843428, 0.174952, 0.507958], + [0.0435637, 0.920106, -0.389239], + [-0.535473, 0.350423, 0.768422], + ], + [ + [-0.835464, 0.0040872, -0.549533], + [0.00227251, 0.999989, 0.00398241], + [0.549543, 0.00207845, -0.835464], + ], + ), + ( + [ + [0.999897, -0.0142888, -0.00160177], + [0.0141561, 0.997826, -0.064364], + [0.00251798, 0.0643346, 0.997925], + ], + [ + [-0.999956, 0.00898988, 0.00296485], + [0.00900757, 0.999941, 0.00601779], + [-0.00291058, 0.00604429, -0.999978], + ], + ), + ( + [ + [0.999477, -0.0204859, 0.0250096], + [0.0126204, 0.959462, 0.281557], + [-0.0297637, -0.281094, 0.959219], + ], + [ + [-0.999384, 0.0172602, -0.0305795], + [-0.0254425, 0.24428, 0.969371], + [0.0242012, 0.969551, -0.24369], + ], + ), + ( + [ + [0.984597, 0.173474, -0.0218106], + [-0.15145, 0.783891, -0.602145], + [-0.0873592, 0.596173, 0.798089], + ], + [ + [-0.998608, -0.0432858, 0.0301827], + [-0.00287128, 0.615692, 0.787983], + [-0.0526917, 0.786797, -0.61496], + ], + ), + ( + [ + [0.917099, -0.3072, 0.254083], + [0.303902, 0.951219, 0.0531566], + [-0.258018, 0.0284665, 0.965721], + ], + [ + [-0.931191, 0.347008, -0.111675], + [-0.352102, -0.77686, 0.522032], + [0.0943935, 0.52543, 0.845586], + ], + ), + ( + [ + [0.991706, 0.0721037, -0.106393], + [-0.0995017, 0.954693, -0.280464], + [0.0813505, 0.288725, 0.95395], + ], + [ + [-0.995306, 0.00106317, 0.0967833], + [0.0167662, 0.986717, 0.161583], + [-0.0953259, 0.162447, -0.982103], + ], + ), + ( + [ + [0.997078, 0.0478273, -0.0595641], + [-0.0348316, 0.978617, 0.202719], + [0.067986, -0.200052, 0.977424], + ], + [ + [-0.997925, -0.0439691, 0.0470461], + [0.0643829, -0.695474, 0.715663], + [0.00125305, 0.717207, 0.696861], + ], + ), + ( + [ + [0.972749, -0.0233882, -0.230677], + [0.0255773, 0.999652, 0.00650349], + [0.230445, -0.0122264, 0.973009], + ], + [ + [-0.973286, 0.0147558, 0.229126], + [0.0145644, 0.999891, -0.00252631], + [-0.229138, 0.000878362, -0.973394], + ], + ), + ( + [ + [0.999271, 0.00700481, 0.0375381], + [-0.0348202, 0.57069, 0.820427], + [-0.0156757, -0.821136, 0.570517], + ], + [ + [-0.999805, -0.0198049, 0.000539906], + [0.0179848, -0.89584, 0.444015], + [-0.00831113, 0.443938, 0.89602], + ], + ), + ( + [ + [0.975255, -0.0207895, 0.220104], + [0.0252764, 0.999526, -0.0175888], + [-0.219634, 0.022717, 0.975318], + ], + [ + [-0.975573, 0.0128154, -0.219304], + [0.0106554, 0.999882, 0.0110292], + [0.219419, 0.00842303, -0.975594], + ], + ), + ( + [ + [-0.433961, -0.658151, -0.615236], + [0.610442, -0.717039, 0.336476], + [-0.6626, -0.229548, 0.71293], + ], + [ + [-0.998516, -0.00675119, -0.054067], + [-0.00405539, 0.99875, -0.0498174], + [0.0543358, -0.0495237, -0.997296], + ], + ), + ( + [ + [0.942764, -0.0126807, -0.333221], + [-0.0017175, 0.999079, -0.042879], + [0.333458, 0.0409971, 0.941873], + ], + [ + [-0.942228, -0.0109444, 0.334798], + [0.0110573, 0.997905, 0.0637396], + [-0.334794, 0.0637589, -0.940133], + ], + ), + ( + [ + [0.962038, 0.0147987, 0.272515], + [-0.0185974, 0.999762, 0.0113615], + [-0.272283, -0.0159982, 0.962084], + ], + [ + [-0.959802, 0.0113708, -0.280451], + [0.00982126, 0.999928, 0.00692958], + [0.280509, 0.00389678, -0.959845], + ], + ), + ( + [ + [0.998414, 0.0139348, 0.0545528], + [-0.0226877, 0.986318, 0.163283], + [-0.0515311, -0.164262, 0.98507], + ], + [ + [-0.998641, -0.000695993, -0.0521343], + [0.0182534, 0.931965, -0.362087], + [0.0488394, -0.362547, -0.930686], + ], + ), + ( + [ + [0.999705, -0.0234518, -0.00633743], + [0.0235916, 0.999458, 0.0229643], + [0.00579544, -0.023107, 0.999716], + ], + [ + [-0.999901, 0.004436, 0.0133471], + [-0.00306106, 0.85758, -0.514342], + [-0.0137278, -0.514332, -0.857481], + ], + ), + ( + [ + [0.998617, -0.051704, 0.0094837], + [0.0484292, 0.975079, 0.216506], + [-0.0204416, -0.215748, 0.976235], + ], + [ + [-0.999959, -0.00295958, -0.00862907], + [-0.00313279, 0.999792, 0.0201361], + [0.00856768, 0.0201625, -0.999761], + ], + ), + ( + [ + [0.999121, 0.0345472, -0.023733], + [-0.0333175, 0.998174, 0.0503881], + [0.0254304, -0.0495531, 0.998448], + ], + [ + [-0.999272, -0.0337466, 0.0178065], + [0.0200629, -0.0677624, 0.9975], + [-0.0324556, 0.997131, 0.0683898], + ], + ), + ( + [ + [0.989017, 0.139841, -0.0478525], + [-0.131355, 0.683201, -0.718319], + [-0.0677572, 0.716715, 0.694067], + ], + [ + [-0.995236, 0.00457798, 0.097401], + [0.097488, 0.0258334, 0.994902], + [0.00203912, 0.999657, -0.0261574], + ], + ), + ( + [ + [0.961528, 0.249402, 0.11516], + [-0.204522, 0.9298, -0.306009], + [-0.183395, 0.270684, 0.945038], + ], + [ + [-0.999566, -0.0233216, 0.0180679], + [0.012372, 0.224583, 0.974377], + [-0.0267822, 0.974177, -0.224197], + ], + ), + ( + [ + [0.865581, 0.0252563, -0.500131], + [0.0302583, 0.994265, 0.102578], + [0.499853, -0.103923, 0.859853], + ], + [ + [-0.866693, 0.0042288, 0.498824], + [0.0129331, 0.999818, 0.0139949], + [-0.498674, 0.0185807, -0.866591], + ], + ), + ( + [ + [0.998999, -0.0213419, -0.0393009], + [-0.0007582, 0.870578, -0.492031], + [0.0447153, 0.491568, 0.86969], + ], + [ + [-0.999207, -0.0184688, 0.0353073], + [0.00153266, 0.867625, 0.497218], + [-0.0398164, 0.496876, -0.866908], + ], + ), + ( + [ + [0.96567, -0.00482973, 0.259728], + [0.00349956, 0.999978, 0.00558359], + [-0.259749, -0.00448297, 0.965666], + ], + [ + [-0.962691, -0.00113074, -0.270609], + [-5.93716e-05, 0.999992, -0.00396767], + [0.270612, -0.00380337, -0.962683], + ], + ), + ( + [ + [0.948799, 0.287027, -0.131894], + [-0.300257, 0.949181, -0.0943405], + [0.0981135, 0.129112, 0.986764], + ], + [ + [-0.993593, -0.0406684, 0.105449], + [-0.0506857, 0.994269, -0.0941326], + [-0.101017, -0.0988741, -0.98996], + ], + ), + ( + [ + [0.998935, 0.0451118, 0.0097202], + [-0.0418086, 0.973879, -0.223183], + [-0.0195345, 0.222539, 0.974728], + ], + [ + [-0.999821, 0.00821522, -0.0170658], + [0.00742187, 0.998912, 0.046048], + [0.0174255, 0.0459131, -0.998794], + ], + ), + ( + [ + [0.99577, 0.00458459, 0.0917705], + [-0.00244288, 0.999722, -0.0234365], + [-0.0918524, 0.0231131, 0.995504], + ], + [ + [-0.995956, 0.0137721, -0.0887945], + [0.0122932, 0.999777, 0.0171801], + [0.0890113, 0.0160191, -0.995903], + ], + ), + ( + [ + [0.97931, 0.0219899, 0.201169], + [-0.0159226, 0.99937, -0.0317288], + [-0.201739, 0.0278692, 0.979043], + ], + [ + [-0.980952, 0.00507266, -0.19419], + [0.00310821, 0.999941, 0.010419], + [0.194231, 0.00961706, -0.98091], + ], + ), + ( + [ + [0.999616, 0.00550326, -0.0271537], + [-0.0048286, 0.99968, 0.0248495], + [0.0272817, -0.0247088, 0.999322], + ], + [ + [-0.999689, -0.00054899, 0.0249588], + [-0.00125497, 0.999599, -0.0282774], + [-0.0249333, -0.0282998, -0.999289], + ], + ), + ( + [ + [0.998036, -0.00755259, -0.0621791], + [0.0417502, 0.820234, 0.570502], + [0.0466927, -0.571978, 0.818939], + ], + [ + [-0.999135, -0.0278203, 0.0309173], + [-0.00855238, 0.864892, 0.501886], + [-0.0407029, 0.501187, -0.864382], + ], + ), + ( + [ + [0.958227, 0.00271545, 0.285997], + [-0.00426128, 0.999979, 0.00478282], + [-0.285979, -0.00580174, 0.958218], + ], + [ + [-0.958726, 0.011053, -0.284121], + [0.0138068, 0.999875, -0.00769161], + [0.284001, -0.0112968, -0.958759], + ], + ), + ( + [ + [-0.804547, -0.48558, -0.341929], + [0.517913, -0.855425, -0.00382581], + [-0.290637, -0.180168, 0.939718], + ], + [ + [0.993776, -0.0469383, -0.101033], + [-0.110087, -0.274676, -0.955214], + [0.0170842, 0.96039, -0.278134], + ], + ), + ( + [ + [0.991875, -0.0022313, -0.127195], + [-0.00198041, 0.999454, -0.0329762], + [0.127199, 0.0329602, 0.991329], + ], + [ + [-0.992632, -0.0090772, 0.120844], + [-0.00870494, 0.999956, 0.00360636], + [-0.120871, 0.00252786, -0.992667], + ], + ), + ( + [ + [0.999305, -0.0252534, 0.0274367], + [0.026144, 0.999126, -0.0326002], + [-0.0265895, 0.0332948, 0.999092], + ], + [ + [-0.999314, -0.0038532, -0.0368519], + [-0.00441323, 0.999876, 0.0151263], + [0.036789, 0.0152787, -0.999207], + ], + ), + ( + [ + [0.999843, -0.00958823, 0.0148803], + [0.00982469, 0.999825, -0.0159002], + [-0.0147253, 0.0160439, 0.999763], + ], + [ + [-0.999973, 0.00673608, -0.00308692], + [-0.0067409, -0.999977, 0.00116827], + [-0.00307934, 0.00119013, 0.999995], + ], + ), + ( + [ + [0.981558, -0.00727741, 0.191028], + [-0.00866166, 0.996556, 0.0824708], + [-0.190971, -0.0826044, 0.978114], + ], + [ + [-0.980202, 0.0179519, -0.197188], + [0.00957606, 0.999014, 0.0433472], + [0.197772, 0.0406008, -0.979408], + ], + ), + ( + [ + [0.966044, 0.0143709, 0.257977], + [-0.0157938, 0.999869, 0.00344404], + [-0.257894, -0.00740153, 0.966145], + ], + [ + [-0.965532, 0.0100318, -0.260094], + [0.00950897, 0.999949, 0.00326797], + [0.260113, 0.000682242, -0.965579], + ], + ), + ( + [ + [0.999965, 0.00727991, -0.00412134], + [-0.00802642, 0.973769, -0.227397], + [0.00235781, 0.227422, 0.973794], + ], + [ + [-0.999877, 0.00698241, 0.0141441], + [0.0103867, 0.966295, 0.257228], + [-0.0118713, 0.257343, -0.966248], + ], + ), + ( + [ + [0.951385, -0.0297966, 0.306561], + [-0.0314555, 0.980706, 0.19294], + [-0.306395, -0.193204, 0.932092], + ], + [ + [-0.99981, 0.00389172, -0.0191159], + [-0.00386326, -0.999991, -0.00150593], + [-0.0191215, -0.00143146, 0.999816], + ], + ), + ( + [ + [0.986772, -0.120673, 0.10825], + [0.0543962, 0.875511, 0.480126], + [-0.152713, -0.467887, 0.870495], + ], + [ + [-0.991246, 0.125848, -0.0399414], + [-0.129021, -0.85897, 0.495507], + [0.0280503, 0.496321, 0.867686], + ], + ), + ( + [ + [-0.804799, -0.588418, 0.0778637], + [-0.514399, 0.756902, 0.403104], + [-0.296129, 0.284365, -0.911836], + ], + [ + [0.98676, -0.0939473, 0.132227], + [0.162179, 0.557277, -0.814336], + [0.0028177, 0.824995, 0.565135], + ], + ), + ( + [ + [0.878935, 0.115231, 0.462813], + [0.0845639, 0.917349, -0.388998], + [-0.469386, 0.381041, 0.796546], + ], + [ + [-0.869533, 0.00193279, -0.493873], + [-0.00419575, 0.999927, 0.0113007], + [0.493859, 0.0118986, -0.869462], + ], + ), + ( + [ + [0.951881, 0.20828, 0.224816], + [-0.305582, 0.700797, 0.644595], + [-0.023294, -0.682277, 0.730722], + ], + [ + [-0.999787, 0.0141074, -0.0151097], + [-0.000971554, 0.698061, 0.716038], + [0.0206489, 0.7159, -0.697898], + ], + ), + ( + [ + [0.999538, 0.0192173, 0.0235334], + [-0.0189064, 0.999732, -0.0133635], + [-0.0237839, 0.0129124, 0.999634], + ], + [ + [-0.999807, 0.00286378, -0.0194776], + [0.0026258, 0.999922, 0.0122308], + [0.0195111, 0.0121774, -0.999736], + ], + ), + ( + [ + [0.998468, 0.041362, -0.0367422], + [-0.0364453, 0.991404, 0.125658], + [0.0416238, -0.124127, 0.991393], + ], + [ + [-0.997665, -0.0658235, 0.0183602], + [0.0216855, -0.0501652, 0.998507], + [-0.064804, 0.99657, 0.0514739], + ], + ), + ( + [ + [0.995563, 0.0493669, 0.0801057], + [-0.0272233, 0.966027, -0.257002], + [-0.0900717, 0.253681, 0.963085], + ], + [ + [-0.999228, -0.034399, -0.0190572], + [0.0250208, -0.929986, 0.366743], + [-0.0303386, 0.365984, 0.930127], + ], + ), + ( + [ + [0.952898, 0.0122933, 0.303043], + [-0.00568444, 0.999727, -0.0226807], + [-0.303239, 0.0198898, 0.952707], + ], + [ + [-0.951155, 0.0127759, -0.308452], + [0.000612627, 0.999219, 0.0394978], + [0.308716, 0.0373795, -0.95042], + ], + ), + ( + [ + [0.923096, -0.000313887, 0.38457], + [0.00948258, 0.999714, -0.0219453], + [-0.384454, 0.0239044, 0.922835], + ], + [ + [-0.922662, -0.00403523, -0.385589], + [-0.0119834, 0.999762, 0.0182116], + [0.385424, 0.0214239, -0.922491], + ], + ), + ( + [ + [0.991575, 0.0945042, -0.0885834], + [-0.10112, 0.99216, -0.0734349], + [0.080949, 0.0817738, 0.993358], + ], + [ + [-0.990948, -0.127974, 0.0405639], + [0.096351, -0.467557, 0.878697], + [-0.0934839, 0.874651, 0.475655], + ], + ), + ( + [ + [0.997148, 0.010521, 0.0747407], + [-0.0079726, 0.999379, -0.034313], + [-0.0750553, 0.0336192, 0.996612], + ], + [ + [-0.996543, 0.00988805, -0.0825019], + [0.00939476, 0.999936, 0.0063645], + [0.0825595, 0.00556751, -0.996572], + ], + ), + ( + [ + [0.991261, 0.00474444, -0.131831], + [-0.00205841, 0.999788, 0.0205036], + [0.131901, -0.020053, 0.99106], + ], + [ + [-0.990924, 4.45275e-05, 0.134427], + [0.00614714, 0.998969, 0.0449827], + [-0.134286, 0.0454008, -0.989903], + ], + ), + ( + [ + [0.992266, -0.0947916, 0.0801474], + [0.100889, 0.992006, -0.0757987], + [-0.0723216, 0.0832984, 0.993897], + ], + [ + [-0.992701, 0.0817686, -0.0886652], + [-0.114283, -0.40263, 0.908203], + [0.0385633, 0.911704, 0.409035], + ], + ), + ( + [ + [0.99696, -0.00808565, -0.0774951], + [0.0585083, 0.734519, 0.676061], + [0.0514552, -0.67854, 0.732759], + ], + [ + [-0.9998, 0.0053398, -0.0193164], + [-0.0162677, -0.779206, 0.626556], + [-0.0117055, 0.626745, 0.779137], + ], + ), + ( + [ + [0.961501, 0.0133645, -0.274475], + [-0.016255, 0.999834, -0.00825889], + [0.274319, 0.0124025, 0.961559], + ], + [ + [-0.963687, 0.000179203, 0.267042], + [0.00670194, 0.999701, 0.023515], + [-0.266958, 0.0244509, -0.9634], + ], + ), + ( + [ + [0.99877, 0.0413462, -0.0273572], + [-0.0263673, 0.91029, 0.413131], + [0.0419844, -0.411902, 0.910261], + ], + [ + [-0.998035, -0.0613039, 0.0130407], + [-0.00146496, 0.230815, 0.972998], + [-0.0626594, 0.971065, -0.230452], + ], + ), + ( + [ + [0.999657, 0.0261608, 0.00141675], + [-0.0261957, 0.998937, 0.0379393], + [-0.000422719, -0.0379634, 0.999279], + ], + [ + [-0.998896, -0.0310033, -0.0353275], + [0.0315452, -0.999392, -0.0148857], + [-0.0348445, -0.0159846, 0.999265], + ], + ), + ( + [ + [0.77369, 0.0137861, 0.633415], + [-0.0186509, 0.999826, 0.00102049], + [-0.63329, -0.0126033, 0.773812], + ], + [ + [-0.773069, 0.0156632, -0.634129], + [0.00418312, 0.999799, 0.0195956], + [0.634308, 0.0124961, -0.772979], + ], + ), + ( + [ + [0.952827, -0.024521, -0.302522], + [-0.00541318, 0.9952, -0.0977158], + [0.303465, 0.0947439, 0.94812], + ], + [ + [-0.952266, -0.00806089, 0.305165], + [0.00351941, 0.999295, 0.037378], + [-0.305252, 0.0366678, -0.951567], + ], + ), + ( + [ + [-0.172189, 0.949971, 0.260587], + [-0.86961, -0.0223234, -0.493235], + [-0.462741, -0.311539, 0.829948], + ], + [ + [-0.672964, 0.0127645, -0.739567], + [0.00429523, 0.999902, 0.0133494], + [0.739664, 0.00580721, -0.672953], + ], + ), + ( + [ + [0.637899, -0.440017, 0.632036], + [-0.52883, 0.346333, 0.774849], + [-0.559842, -0.828516, -0.0117683], + ], + [ + [-0.0627307, -0.0314554, -0.997536], + [-0.733537, 0.679201, 0.0247117], + [0.67675, 0.733279, -0.0656804], + ], + ), + ( + [ + [0.998402, 0.00284932, -0.0564372], + [0.000393713, 0.998353, 0.0573683], + [0.0565077, -0.0572989, 0.996757], + ], + [ + [-0.997878, 0.000941416, 0.0651252], + [-2.16756e-05, 0.999891, -0.0147853], + [-0.065132, -0.0147552, -0.997768], + ], + ), + ( + [ + [0.9999, 0.0141438, -0.000431687], + [-0.0140882, 0.9979, 0.063225], + [0.00132502, -0.0632125, 0.997999], + ], + [ + [-0.999515, -0.0308197, -0.00482715], + [-0.00160551, -0.103741, 0.994605], + [-0.0311554, 0.994128, 0.10364], + ], + ), + ( + [ + [-0.201909, 0.0267804, 0.979038], + [-0.0159062, 0.999405, -0.0306179], + [-0.979275, -0.0217548, -0.201363], + ], + [ + [0.261235, 0.951613, -0.161839], + [0.0758567, 0.146901, 0.986239], + [0.962292, -0.269916, -0.03381], + ], + ), + ( + [ + [0.998335, -0.0191576, -0.0544038], + [0.0163271, 0.998513, -0.0520045], + [0.0553192, 0.0510297, 0.997164], + ], + [ + [-0.998811, -0.00846127, 0.0480344], + [-0.0051736, 0.997661, 0.0681593], + [-0.0484988, 0.0678295, -0.996519], + ], + ), + ( + [ + [0.999973, 0.00227282, -0.00699658], + [-0.00137504, 0.992062, 0.125744], + [0.00722684, -0.125731, 0.992038], + ], + [ + [-0.999995, -0.00337061, 4.25756e-05], + [-0.00333677, 0.991528, 0.129853], + [-0.00047993, 0.129852, -0.991534], + ], + ), + ( + [ + [0.998908, 0.0216581, -0.041392], + [-0.0327304, 0.956678, -0.289302], + [0.0333331, 0.290341, 0.956342], + ], + [ + [-0.998254, -0.0377592, 0.0454422], + [0.00744647, 0.682591, 0.730764], + [-0.0586112, 0.729825, -0.681118], + ], + ), + ( + [ + [0.999387, -0.0042571, -0.0347599], + [0.00485203, 0.999843, 0.017049], + [0.0346819, -0.0172072, 0.99925], + ], + [ + [-0.999976, 0.00260242, -0.00669664], + [-0.00250352, -0.999889, -0.0147361], + [-0.00673422, -0.0147175, 0.99987], + ], + ), + ( + [ + [0.906103, -0.398828, -0.141112], + [0.381512, 0.914475, -0.13485], + [0.182826, 0.0683519, 0.980766], + ], + [ + [-0.996568, -0.0321282, -0.0763021], + [-0.0823787, 0.476597, 0.875254], + [0.00824509, 0.878535, -0.477609], + ], + ), + ( + [ + [0.908356, 0.316033, -0.273884], + [-0.231421, -0.165634, -0.95865], + [-0.34833, 0.934178, -0.0773183], + ], + [ + [-0.999889, -0.0146322, -0.00295739], + [-0.0149238, 0.974974, 0.221815], + [-0.000362257, 0.221835, -0.975085], + ], + ), + ( + [ + [0.999507, -0.00834631, 0.0302637], + [0.00899248, 0.999733, -0.0212785], + [-0.030078, 0.0215401, 0.999315], + ], + [ + [-0.999538, 0.00785187, -0.0293621], + [0.00739788, 0.999852, 0.0155394], + [0.0294797, 0.0153149, -0.999448], + ], + ), + ( + [ + [0.999951, -0.00729441, -0.00672921], + [0.00313753, 0.87564, -0.482954], + [0.00941523, 0.48291, 0.87562], + ], + [ + [-0.999984, -0.005202, -0.00277372], + [0.00340465, -0.893745, 0.448565], + [-0.00481353, 0.448548, 0.893747], + ], + ), + ( + [ + [0.998028, -0.0569885, 0.0263322], + [0.0489091, 0.968801, 0.242967], + [-0.039357, -0.2412, 0.969677], + ], + [ + [-0.997066, 0.0422415, -0.0638525], + [-0.0760293, -0.448184, 0.890703], + [0.00900662, 0.892944, 0.45008], + ], + ), + ( + [ + [0.999745, 0.00860777, 0.0208747], + [-0.00827114, 0.999835, -0.0161595], + [-0.0210103, 0.0159827, 0.999651], + ], + [ + [-0.999576, 0.0148733, -0.0251161], + [0.0151027, 0.999846, -0.00898035], + [0.0249787, -0.00935575, -0.999646], + ], + ), + ( + [ + [0.91924, 0.0372116, -0.391934], + [-0.00675798, 0.996868, 0.0787959], + [0.393639, -0.0697837, 0.916613], + ], + [ + [-0.921919, 0.00882585, 0.387286], + [0.00588498, 0.999944, -0.00877866], + [-0.387342, -0.00581387, -0.921919], + ], + ), + ( + [ + [0.998324, -0.0029024, 0.0577924], + [0.00236766, 0.999954, 0.00931901], + [-0.0578167, -0.00916657, 0.998285], + ], + [ + [-0.99892, -0.0025688, -0.0464413], + [-0.00203721, 0.999932, -0.0114927], + [0.0464676, -0.0113855, -0.998857], + ], + ), + ( + [ + [0.993986, 0.0163462, -0.108279], + [-0.0612924, 0.902447, -0.426418], + [0.090746, 0.43049, 0.898022], + ], + [ + [-0.994519, -0.0767804, 0.0709843], + [0.0579273, 0.160607, 0.985318], + [-0.0870543, 0.984028, -0.15528], + ], + ), + ( + [ + [0.997351, 0.0715122, -0.0132892], + [-0.0707087, 0.996067, 0.0533919], + [0.0170551, -0.0523108, 0.998485], + ], + [ + [-0.997704, -0.066002, 0.015281], + [0.064101, -0.846657, 0.528267], + [-0.0219278, 0.528033, 0.848942], + ], + ), + ( + [ + [0.999839, 0.00714662, -0.0164633], + [-0.00859425, 0.99594, -0.0896085], + [0.0157561, 0.0897356, 0.995841], + ], + [ + [-0.999773, 0.0079918, 0.0197854], + [0.00864136, 0.999419, 0.0329623], + [-0.0195105, 0.0331255, -0.999262], + ], + ), + ( + [ + [-0.773738, 0.630074, 0.0658454], + [-0.622848, -0.737618, -0.260731], + [-0.115711, -0.242749, 0.963163], + ], + [ + [-0.740005, 0.000855199, -0.672604], + [-0.0106008, 0.99986, 0.0129348], + [0.672521, 0.0167018, -0.739892], + ], + ), + ( + [ + [0.969039, -0.00110643, -0.246907], + [-0.121454, 0.868509, -0.480564], + [0.214973, 0.495673, 0.841484], + ], + [ + [-0.981168, -0.150714, 0.120811], + [0.172426, -0.401504, 0.89948], + [-0.0870583, 0.903372, 0.419929], + ], + ), + ( + [ + [0.589015, 0.80692, 0.0440651], + [-0.806467, 0.583447, 0.0959135], + [0.0516848, -0.0920316, 0.994414], + ], + [ + [-0.99998, 0.00434293, -0.00486489], + [0.00437139, 0.999973, -0.00588975], + [0.00483918, -0.00591087, -0.999972], + ], + ), + ( + [ + [0.999972, 0.000781564, 0.00750023], + [-0.0031568, 0.946655, 0.322235], + [-0.00684828, -0.322249, 0.94663], + ], + [ + [-0.999817, -0.0178453, -0.00691725], + [-0.0189272, 0.975556, 0.218934], + [0.00284118, 0.219025, -0.975716], + ], + ), + ( + [ + [-0.969668, 0.219101, -0.108345], + [0.172364, 0.298654, -0.938667], + [-0.173305, -0.928871, -0.32736], + ], + [ + [-0.999917, 0.0111423, -0.00656864], + [-0.00977865, -0.318874, 0.947748], + [0.00846644, 0.947733, 0.318955], + ], + ), + ( + [ + [-0.808574, -0.185515, -0.558383], + [0.174641, -0.981898, 0.0733309], + [-0.561879, -0.038223, 0.826336], + ], + [ + [-0.873416, 0.0121808, -0.486824], + [-0.00495714, 0.999413, 0.0338998], + [0.486951, 0.032022, -0.872843], + ], + ), + ( + [ + [0.999295, 0.0295658, -0.0231234], + [-0.0251771, 0.984904, 0.17126], + [0.0278378, -0.170557, 0.984954], + ], + [ + [-0.998834, -0.040128, 0.026921], + [0.0327412, -0.152276, 0.987798], + [-0.0355388, 0.987524, 0.153411], + ], + ), + ( + [ + [0.996021, -0.0050677, -0.0889802], + [0.0042919, 0.999951, -0.00890794], + [0.089021, 0.0084906, 0.995994], + ], + [ + [-0.995726, -0.00858132, 0.0919686], + [-0.00615004, 0.999625, 0.0266854], + [-0.0921631, 0.0260058, -0.995405], + ], + ), + ( + [ + [0.563325, 0.812296, 0.151129], + [-0.316559, 0.381143, -0.868632], + [-0.763188, 0.441481, 0.471847], + ], + [ + [-0.980048, -0.0115108, -0.198437], + [-0.168991, 0.573853, 0.801335], + [0.104649, 0.818877, -0.564348], + ], + ), + ( + [ + [0.984844, -0.0288271, 0.17103], + [0.0260588, 0.999491, 0.0184094], + [-0.171474, -0.0136736, 0.985094], + ], + [ + [-0.984637, -0.00367691, -0.174577], + [-0.00649229, 0.999858, 0.0155587], + [0.174495, 0.0164532, -0.984521], + ], + ), + ( + [ + [0.99985, 0.000720773, -0.0172841], + [-0.00075051, 0.999998, -0.0017141], + [0.0172828, 0.00172682, 0.999849], + ], + [ + [-0.999926, -0.00413456, 0.0114842], + [-0.00368343, 0.999231, 0.0390359], + [-0.0116368, 0.0389908, -0.999172], + ], + ), + ( + [ + [0.997976, 0.0603523, -0.0200139], + [-0.0558618, 0.982551, 0.177404], + [0.0303714, -0.175927, 0.983935], + ], + [ + [-0.996867, -0.0790953, 0.00217996], + [0.0318842, -0.376338, 0.925935], + [-0.0724181, 0.923101, 0.37768], + ], + ), + ( + [ + [0.94678, -0.00538407, -0.321837], + [0.00249113, 0.999953, -0.0094], + [0.321872, 0.008098, 0.946749], + ], + [ + [-0.945694, 0.0255694, 0.324053], + [0.0240377, 0.999673, -0.00872898], + [-0.32417, -0.000465377, -0.945999], + ], + ), + ( + [ + [0.846059, 0.435245, -0.307807], + [0.318073, 0.0512036, 0.946682], + [0.4278, -0.898855, -0.0951187], + ], + [ + [-0.217213, -0.0389124, 0.975352], + [0.742195, 0.642416, 0.190918], + [-0.634011, 0.765368, -0.11066], + ], + ), + ( + [ + [0.914988, -0.0538229, -0.399875], + [-0.0459455, 0.970717, -0.23579], + [0.400857, 0.234117, 0.885722], + ], + [ + [-0.919706, 0.00194642, 0.392606], + [0.105539, 0.964406, 0.242451], + [-0.378159, 0.264418, -0.887176], + ], + ), + ( + [ + [0.970915, -0.183858, 0.153365], + [0.209801, 0.96196, -0.174974], + [-0.115361, 0.202061, 0.972555], + ], + [ + [-0.975509, 0.21077, -0.0629391], + [-0.218082, -0.964089, 0.151576], + [-0.0287314, 0.161588, 0.986441], + ], + ), + ( + [ + [0.99369, -0.00515149, -0.112044], + [0.00366664, 0.999903, -0.0134545], + [0.112102, 0.0129588, 0.993612], + ], + [ + [-0.99406, 0.00631892, 0.108668], + [0.00878985, 0.999713, 0.022273], + [-0.108496, 0.0230956, -0.99383], + ], + ), + ( + [ + [0.995917, 0.0137529, 0.089215], + [-0.0145079, 0.999864, 0.00781912], + [-0.0890954, -0.00908151, 0.995982], + ], + [ + [-0.996188, 0.012059, -0.0864113], + [0.0126654, 0.999899, -0.00647346], + [0.0863245, -0.00754306, -0.99624], + ], + ), + ( + [ + [0.84563, -0.0032436, -0.533759], + [0.0040093, 0.999992, 0.000275049], + [0.533754, -0.00237259, 0.845636], + ], + [ + [-0.849818, -0.00755214, 0.527023], + [-0.00734806, 0.99997, 0.00248074], + [-0.527026, -0.00176415, -0.849848], + ], + ), + ( + [ + [0.736067, -0.212675, -0.642631], + [-0.447028, 0.560168, -0.697408], + [0.508303, 0.800613, 0.31725], + ], + [ + [-0.684029, 0.0061039, 0.729431], + [0.0260275, 0.999532, 0.0160434], + [-0.728992, 0.0299595, -0.683868], + ], + ), + ( + [ + [0.993949, 0.00461705, -0.109742], + [-0.00653155, 0.999833, -0.0170925], + [0.109644, 0.0177058, 0.993813], + ], + [ + [-0.994446, 0.0218439, 0.102965], + [0.0227578, 0.999711, 0.00770966], + [-0.102767, 0.0100102, -0.994656], + ], + ), + ( + [ + [0.996005, -0.0103388, 0.0886959], + [-0.0291635, 0.901147, 0.432531], + [-0.0843999, -0.43339, 0.897246], + ], + [ + [-0.999947, 0.00833193, -0.00598923], + [-0.0101526, -0.887864, 0.459993], + [-0.00148526, 0.46003, 0.887902], + ], + ), + ( + [ + [0.981518, 0.0114609, 0.191025], + [-0.0104683, 0.999926, -0.00620422], + [-0.191082, 0.00408984, 0.981565], + ], + [ + [-0.979556, 0.000134379, -0.201176], + [-0.00817302, 0.999148, 0.0404628], + [0.20101, 0.0412799, -0.97872], + ], + ), + ( + [ + [0.997665, -0.0372296, -0.0572574], + [0.0379027, 0.999224, 0.0107148], + [0.0568141, -0.01286, 0.998302], + ], + [ + [-0.997794, 0.00389749, 0.0662921], + [0.00639122, 0.999278, 0.0374446], + [-0.0660983, 0.0377856, -0.997099], + ], + ), + ( + [ + [0.981618, -0.0105643, -0.190564], + [0.00329498, 0.999256, -0.0384229], + [0.190828, 0.0370887, 0.980923], + ], + [ + [-0.981673, -0.000810695, 0.190576], + [0.00398375, 0.999685, 0.0247729], + [-0.190536, 0.0250779, -0.981361], + ], + ), + ( + [ + [-0.544941, -0.812151, -0.208446], + [0.812337, -0.449791, -0.37121], + [0.207722, -0.371617, 0.90485], + ], + [ + [-0.121327, -0.000366672, -0.992614], + [-0.955208, 0.271977, 0.116655], + [0.269926, 0.962303, -0.0333484], + ], + ), + ( + [ + [0.637701, -0.219537, 0.738336], + [0.735715, 0.457522, -0.499397], + [-0.228168, 0.861671, 0.453279], + ], + [ + [-0.741797, 0.0196167, -0.670339], + [-0.00209087, 0.9995, 0.0315629], + [0.670623, 0.0248149, -0.741385], + ], + ), + ( + [ + [0.99813, -0.0590625, -0.0157485], + [0.0589086, 0.998213, -0.0100649], + [0.0163148, 0.00911833, 0.999825], + ], + [ + [-0.99893, 0.0258783, -0.0383385], + [-0.0440455, -0.279068, 0.959261], + [0.014125, 0.959924, 0.279908], + ], + ), + ( + [ + [0.999558, 0.0028395, -0.0296019], + [-0.00492321, 0.997496, -0.0705578], + [0.0293274, 0.0706723, 0.997068], + ], + [ + [-0.999532, -0.0305627, -0.00231546], + [0.00957406, -0.38309, 0.923664], + [-0.0291167, 0.923206, 0.383202], + ], + ), + ( + [ + [0.99814, -0.0528437, -0.0303853], + [0.0590889, 0.96123, 0.269341], + [0.0149743, -0.270636, 0.962565], + ], + [ + [-0.999464, 0.00781117, 0.0318024], + [-0.000588355, 0.966696, -0.255928], + [-0.0327423, -0.255809, -0.966173], + ], + ), + ( + [ + [-0.936685, 0.234194, 0.260336], + [-0.233325, -0.97178, 0.034698], + [0.261116, -0.0282419, 0.964894], + ], + [ + [0.999511, 0.00582072, 0.0307461], + [0.0289012, 0.204922, -0.978352], + [-0.0119956, 0.978762, 0.204654], + ], + ), + ( + [ + [0.973616, -0.019218, -0.227384], + [0.0030011, 0.99744, -0.0714512], + [0.228175, 0.0688836, 0.97118], + ], + [ + [-0.974738, 0.0190271, 0.222547], + [0.0222378, 0.999682, 0.0119297], + [-0.222249, 0.0165771, -0.97485], + ], + ), + ( + [ + [0.997273, 0.0453471, -0.0582173], + [-0.0234007, 0.942529, 0.333303], + [0.0699858, -0.331032, 0.941021], + ], + [ + [-0.996269, -0.0613496, 0.0607196], + [-0.0100285, 0.780948, 0.624516], + [-0.0857328, 0.621576, -0.77865], + ], + ), + ( + [ + [0.999511, 0.0274482, -0.0149865], + [-0.0305945, 0.957511, -0.286769], + [0.00647846, 0.287087, 0.957883], + ], + [ + [-0.999443, -0.0260559, 0.0209038], + [0.0148505, 0.213942, 0.976734], + [-0.0299225, 0.976499, -0.213437], + ], + ), + ( + [ + [0.621123, 0.722893, 0.302708], + [-0.48353, 0.657448, -0.577894], + [-0.61677, 0.212574, 0.757896], + ], + [ + [-0.996888, -0.0217614, -0.0757776], + [-0.0783897, 0.376159, 0.923234], + [0.00841386, 0.926299, -0.376694], + ], + ), + ( + [ + [0.974426, 0.0128384, -0.224341], + [-0.0123842, 0.999917, 0.00343166], + [0.224367, -0.00056561, 0.974505], + ], + [ + [-0.973234, -0.00506667, 0.229763], + [-0.000498848, 0.999801, 0.0199346], + [-0.229818, 0.0192865, -0.973043], + ], + ), + ( + [ + [0.994721, -0.0881097, 0.0526082], + [0.0972904, 0.972774, -0.210345], + [-0.0326424, 0.214353, 0.976211], + ], + [ + [-0.994309, 0.0920529, -0.0536268], + [-0.105538, -0.782431, 0.613729], + [0.0145358, 0.615896, 0.787694], + ], + ), + ( + [ + [0.998677, -0.0372894, 0.0354002], + [0.0242326, 0.948589, 0.315583], + [-0.0453481, -0.314308, 0.948237], + ], + [ + [-0.999066, -0.00910724, -0.0422707], + [-0.024629, 0.923353, 0.383161], + [0.0355411, 0.383844, -0.922715], + ], + ), + ( + [ + [0.931525, 0.00831028, 0.363583], + [0.0192806, 0.997204, -0.0721909], + [-0.363167, 0.0742577, 0.92876], + ], + [ + [-0.930052, -0.00174384, -0.367425], + [-0.0268673, 0.997634, 0.0632737], + [0.366445, 0.0687194, -0.927899], + ], + ), + ( + [ + [-0.50483, -0.819216, 0.272087], + [0.775688, -0.568816, -0.273414], + [0.378753, 0.0730272, 0.922612], + ], + [ + [-0.981596, 0.00031926, 0.190974], + [0.00652401, 0.999471, 0.0318616], + [-0.190863, 0.0325211, -0.981079], + ], + ), + ( + [ + [0.990518, -0.00195099, -0.137368], + [-0.00164696, 0.999659, -0.0260735], + [0.137372, 0.0260526, 0.990177], + ], + [ + [-0.991078, 0.00934835, 0.132961], + [0.0106057, 0.999905, 0.00875176], + [-0.132866, 0.0100839, -0.991083], + ], + ), + ( + [ + [0.935049, -0.353081, 0.0318997], + [0.257018, 0.737114, 0.624984], + [-0.244184, -0.576192, 0.779985], + ], + [ + [-0.977342, -0.00167896, -0.211667], + [-0.0448634, 0.978894, 0.199386], + [0.206864, 0.204364, -0.956789], + ], + ), + ( + [ + [0.998464, 0.0501172, 0.0236119], + [-0.0498618, 0.998692, -0.0112844], + [-0.0241466, 0.0100898, 0.999658], + ], + [ + [-0.999931, -0.0037971, -0.0112195], + [-0.00640916, 0.970027, 0.242913], + [0.00996085, 0.242968, -0.969984], + ], + ), + ( + [ + [0.999893, -0.0108217, 0.00984537], + [0.011201, 0.999164, -0.0393194], + [-0.00941164, 0.0394255, 0.999178], + ], + [ + [-0.999886, 0.00730461, -0.0133396], + [-0.0118202, -0.925163, 0.379391], + [-0.00956982, 0.379504, 0.925142], + ], + ), + ( + [ + [0.990922, -0.086745, 0.102709], + [0.0847349, 0.99612, 0.0237834], + [-0.104373, -0.0148644, 0.994427], + ], + [ + [-0.994922, -0.00197458, -0.10064], + [-0.00242513, 0.999988, 0.00435525], + [0.10063, 0.00457739, -0.994914], + ], + ), + ( + [ + [0.999856, 0.00210734, -0.0168511], + [-0.00557165, 0.978053, -0.20828], + [0.0160424, 0.208344, 0.977924], + ], + [ + [-0.999698, 0.0048691, 0.0241226], + [-0.00154306, 0.965899, -0.258915], + [-0.0245606, -0.258874, -0.9656], + ], + ), + ( + [ + [0.992858, -0.0249864, -0.116659], + [0.0419872, 0.988447, 0.145634], + [0.111673, -0.149492, 0.982436], + ], + [ + [-0.992324, 0.0357741, 0.118384], + [-0.0419528, 0.803113, -0.594348], + [-0.116338, -0.594752, -0.795447], + ], + ), + ( + [ + [0.986821, -0.00531913, 0.161729], + [0.00797365, 0.999844, -0.0157688], + [-0.16162, 0.0168505, 0.986709], + ], + [ + [-0.985867, 0.0119402, -0.167109], + [0.0141227, 0.99983, -0.0118784], + [0.166939, -0.0140704, -0.985868], + ], + ), + ( + [ + [0.999693, -0.0158939, -0.0190113], + [0.0103599, 0.96501, -0.262007], + [0.0225104, 0.261729, 0.964879], + ], + [ + [-0.999344, -0.0314781, -0.0180051], + [-0.0250895, 0.241673, 0.970034], + [-0.0261833, 0.969847, -0.242305], + ], + ), + ( + [ + [0.977445, 0.0293661, 0.209138], + [-0.0723687, 0.976903, 0.201057], + [-0.198403, -0.211657, 0.956994], + ], + [ + [-0.976437, 0.00895131, -0.215624], + [0.0552894, 0.976169, -0.20985], + [0.208607, -0.216827, -0.953663], + ], + ), + ( + [ + [0.994593, 0.0974797, -0.0358119], + [-0.0822288, 0.949838, 0.301737], + [0.0634288, -0.297161, 0.952718], + ], + [ + [-0.994192, -0.10746, -0.00604622], + [0.078812, -0.7651, 0.639071], + [-0.0733003, 0.634882, 0.769124], + ], + ), + ( + [ + [0.365674, 0.282077, -0.88697], + [-0.609177, 0.793033, 0.00105565], + [0.703694, 0.539936, 0.461826], + ], + [ + [-0.469534, 0.0109062, 0.882848], + [0.0060785, 0.99994, -0.00911984], + [-0.882894, 0.00108445, -0.469572], + ], + ), + ( + [ + [0.999956, 0.00903085, 0.0025358], + [-0.00862738, 0.991574, -0.129252], + [-0.00368169, 0.129224, 0.991609], + ], + [ + [-0.999976, 0.00322491, -0.00637541], + [0.00379751, 0.995755, -0.0919687], + [0.00605176, -0.0919907, -0.995743], + ], + ), + ( + [ + [0.999982, -0.00398882, -0.00441072], + [0.00411881, 0.999545, 0.0298655], + [0.00428959, -0.0298832, 0.999544], + ], + [ + [-0.999931, -0.00315547, -0.0114491], + [0.00300966, -0.999914, 0.0128304], + [-0.0114875, 0.012796, 0.999853], + ], + ), + ( + [ + [0.996613, 0.0781452, -0.0256245], + [-0.0610516, 0.91178, 0.406116], + [0.0550999, -0.403175, 0.913462], + ], + [ + [-0.996368, -0.084671, 0.00909851], + [0.0540149, -0.545774, 0.83619], + [-0.0658352, 0.833644, 0.548365], + ], + ), + ( + [ + [0.961059, 0.139318, 0.238654], + [-0.117488, 0.987672, -0.103448], + [-0.250124, 0.0713812, 0.965579], + ], + [ + [-0.973397, 0.00782581, -0.228998], + [-0.0621109, 0.952986, 0.296581], + [0.220553, 0.302913, -0.927147], + ], + ), + ( + [ + [0.156415, -0.982138, 0.104589], + [-0.568896, -0.176149, -0.803323], + [0.807398, 0.0661518, -0.586287], + ], + [ + [-0.992155, 0.0934304, -0.0830664], + [-0.121171, -0.555137, 0.822887], + [0.0307694, 0.826496, 0.562102], + ], + ), + ( + [ + [0.997973, 0.0130328, -0.0622976], + [-0.011111, 0.999455, 0.0310968], + [0.0626689, -0.0303416, 0.997573], + ], + [ + [-0.997391, -0.00094697, 0.0722014], + [-0.00271076, 0.9997, -0.024334], + [-0.0721567, -0.024466, -0.997094], + ], + ), + ( + [ + [0.913504, -0.0125928, -0.406634], + [-0.108363, 0.95588, -0.27304], + [0.392132, 0.293487, 0.871836], + ], + [ + [-0.909813, 0.0115348, 0.414861], + [0.128636, 0.958223, 0.255464], + [-0.394582, 0.28579, -0.873287], + ], + ), + ( + [ + [0.932595, -0.0693644, 0.354197], + [0.0984415, 0.993036, -0.0647231], + [-0.347241, 0.0952281, 0.932928], + ], + [ + [-0.930498, 0.00578599, -0.366252], + [-0.106202, 0.952666, 0.284867], + [0.350564, 0.303964, -0.885839], + ], + ), + ( + [ + [0.995668, -0.00475737, 0.0928567], + [0.00890154, 0.99898, -0.0442667], + [-0.0925514, 0.0449015, 0.994695], + ], + [ + [-0.996077, -0.0107986, -0.0878355], + [0.00749423, 0.978669, -0.205305], + [0.0881789, -0.205158, -0.974749], + ], + ), + ( + [ + [0.99948, 0.0321999, 0.00146151], + [-0.0321302, 0.998886, -0.0345513], + [-0.00257243, 0.0344864, 0.999402], + ], + [ + [-0.999953, 0.00726142, -0.0065326], + [0.00488529, 0.950962, 0.30927], + [0.00845801, 0.309223, -0.950953], + ], + ), +] + + +class TestRot3(GtsamTestCase): + """Test selected Rot3 methods.""" + + def test_axisangle(self) -> None: + """Test .axisAngle() method.""" + # fmt: off + R = np.array( + [ + [ -0.999957, 0.00922903, 0.00203116], + [ 0.00926964, 0.999739, 0.0208927], + [ -0.0018374, 0.0209105, -0.999781] + ]) + # fmt: on + + # get back angle in radians + _, actual_angle = Rot3(R).axisAngle() + expected_angle = 3.1396582 + np.testing.assert_almost_equal(actual_angle, expected_angle, 1e-7) + + def test_axis_angle_stress_test(self) -> None: + """Test that .axisAngle() yields angles less than 180 degrees for specific inputs.""" + for (R1, R2) in R1_R2_pairs: + R1 = Rot3(np.array(R1)) + R2 = Rot3(np.array(R2)) + + i1Ri2 = R1.between(R2) + + axis, angle = i1Ri2.axisAngle() + angle_deg = np.rad2deg(angle) + assert angle_deg < 180 + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_Utilities.py b/python/gtsam/tests/test_Utilities.py new file mode 100644 index 000000000..851684f12 --- /dev/null +++ b/python/gtsam/tests/test_Utilities.py @@ -0,0 +1,196 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Utilities unit tests. +Author: Varun Agrawal +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestUtilites(GtsamTestCase): + """Test various GTSAM utilities.""" + def test_createKeyList(self): + """Test createKeyList.""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeyList(I) + self.assertEqual(kl.size(), 3) + + kl = gtsam.utilities.createKeyList("s", I) + self.assertEqual(kl.size(), 3) + + def test_createKeyVector(self): + """Test createKeyVector.""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeyVector(I) + self.assertEqual(len(kl), 3) + + kl = gtsam.utilities.createKeyVector("s", I) + self.assertEqual(len(kl), 3) + + def test_createKeySet(self): + """Test createKeySet.""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeySet(I) + self.assertEqual(kl.size(), 3) + + kl = gtsam.utilities.createKeySet("s", I) + self.assertEqual(kl.size(), 3) + + def test_extractPoint2(self): + """Test extractPoint2.""" + initial = gtsam.Values() + point2 = gtsam.Point2(0.0, 0.1) + initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) + initial.insert(2, point2) + np.testing.assert_equal(gtsam.utilities.extractPoint2(initial), + point2.reshape(-1, 2)) + + def test_extractPoint3(self): + """Test extractPoint3.""" + initial = gtsam.Values() + point3 = gtsam.Point3(0.0, 0.1, 0.0) + initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) + initial.insert(2, point3) + np.testing.assert_equal(gtsam.utilities.extractPoint3(initial), + point3.reshape(-1, 3)) + + def test_allPose2s(self): + """Test allPose2s.""" + initial = gtsam.Values() + initial.insert(0, gtsam.Pose2()) + initial.insert(1, gtsam.Pose2(1, 1, 1)) + initial.insert(2, gtsam.Point2(1, 1)) + initial.insert(3, gtsam.Point3(1, 2, 3)) + result = gtsam.utilities.allPose2s(initial) + self.assertEqual(result.size(), 2) + + def test_extractPose2(self): + """Test extractPose2.""" + initial = gtsam.Values() + pose2 = np.asarray((0.0, 0.1, 0.1)) + + initial.insert(1, gtsam.Pose2(*pose2)) + initial.insert(2, gtsam.Point3(0.0, 0.1, 0.0)) + np.testing.assert_allclose(gtsam.utilities.extractPose2(initial), + pose2.reshape(-1, 3)) + + def test_allPose3s(self): + """Test allPose3s.""" + initial = gtsam.Values() + initial.insert(0, gtsam.Pose3()) + initial.insert(2, gtsam.Point2(1, 1)) + initial.insert(1, gtsam.Pose3()) + initial.insert(3, gtsam.Point3(1, 2, 3)) + result = gtsam.utilities.allPose3s(initial) + self.assertEqual(result.size(), 2) + + def test_extractPose3(self): + """Test extractPose3.""" + initial = gtsam.Values() + pose3 = np.asarray([1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.]) + initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) + initial.insert(2, gtsam.Pose3()) + np.testing.assert_allclose(gtsam.utilities.extractPose3(initial), + pose3.reshape(-1, 12)) + + def test_perturbPoint2(self): + """Test perturbPoint2.""" + values = gtsam.Values() + values.insert(0, gtsam.Pose3()) + values.insert(1, gtsam.Point2(1, 1)) + gtsam.utilities.perturbPoint2(values, 1.0) + self.assertTrue( + not np.allclose(values.atPoint2(1), gtsam.Point2(1, 1))) + + def test_perturbPose2(self): + """Test perturbPose2.""" + values = gtsam.Values() + values.insert(0, gtsam.Pose2()) + values.insert(1, gtsam.Point2(1, 1)) + gtsam.utilities.perturbPose2(values, 1, 1) + self.assertTrue(values.atPose2(0) != gtsam.Pose2()) + + def test_perturbPoint3(self): + """Test perturbPoint3.""" + values = gtsam.Values() + point3 = gtsam.Point3(0, 0, 0) + values.insert(0, gtsam.Pose2()) + values.insert(1, point3) + gtsam.utilities.perturbPoint3(values, 1) + self.assertTrue(not np.allclose(values.atPoint3(1), point3)) + + def test_insertBackprojections(self): + """Test insertBackprojections.""" + values = gtsam.Values() + cam = gtsam.PinholeCameraCal3_S2() + gtsam.utilities.insertBackprojections( + values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]), + 10) + np.testing.assert_allclose(values.atPoint3(0), + gtsam.Point3(200, 200, 10)) + + def test_insertProjectionFactors(self): + """Test insertProjectionFactors.""" + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), + gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2()) + self.assertEqual(graph.size(), 2) + + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), + gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(), + gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))) + self.assertEqual(graph.size(), 2) + + def test_reprojectionErrors(self): + """Test reprojectionErrors.""" + pixels = np.asarray([[20, 30], [20, 30]]) + I = [1, 2] + K = gtsam.Cal3_S2() + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K) + values = gtsam.Values() + values.insert(0, gtsam.Pose3()) + cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K) + gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10) + errors = gtsam.utilities.reprojectionErrors(graph, values) + np.testing.assert_allclose(errors, np.zeros((2, 2))) + + def test_localToWorld(self): + """Test localToWorld.""" + local = gtsam.Values() + local.insert(0, gtsam.Point2(10, 10)) + local.insert(1, gtsam.Pose2(6, 11, 0.0)) + base = gtsam.Pose2(1, 0, 0) + world = gtsam.utilities.localToWorld(local, base) + + expected_point2 = gtsam.Point2(11, 10) + expected_pose2 = gtsam.Pose2(7, 11, 0) + np.testing.assert_allclose(world.atPoint2(0), expected_point2) + np.testing.assert_allclose( + world.atPose2(1).matrix(), expected_pose2.matrix()) + + user_keys = [1] + world = gtsam.utilities.localToWorld(local, base, user_keys) + np.testing.assert_allclose( + world.atPose2(1).matrix(), expected_pose2.matrix()) + + # Raise error since 0 is not in user_keys + self.assertRaises(RuntimeError, world.atPoint2, 0) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py new file mode 100644 index 000000000..5d3c5ace3 --- /dev/null +++ b/python/gtsam/tests/test_basis.py @@ -0,0 +1,96 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Basis unit tests. +Author: Frank Dellaert & Gerry Chen (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase +from gtsam.symbol_shorthand import B + + +class TestBasis(GtsamTestCase): + """ + Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and Chebyshev2. + + It tests FitBasis by fitting to a ground-truth function that can be represented exactly in + the basis, then checking that the regression (fit result) matches the function. For the + Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is + used. + """ + def setUp(self): + self.N = 2 + self.x = [0., 0.5, 0.75] + self.interpx = np.linspace(0., 1., 10) + self.noise = gtsam.noiseModel.Unit.Create(1) + + def evaluate(self, basis, fitparams, x): + """ + Until wrapper for Basis functors are ready, + this is how to evaluate a basis function. + """ + return basis.WeightMatrix(self.N, x) @ fitparams + + def fit_basis_helper(self, fitter, basis, f=lambda x: x): + """Helper method to fit data to a specified fitter using a specified basis.""" + data = {x: f(x) for x in self.x} + fit = fitter(data, self.noise, self.N) + coeff = fit.parameters() + interpy = self.evaluate(basis, coeff, self.interpx) + return interpy + + def test_fit_basis_fourier(self): + """Fit a Fourier basis.""" + + f = lambda x: 0.7 * np.cos(x) + interpy = self.fit_basis_helper(gtsam.FitBasisFourierBasis, + gtsam.FourierBasis, f) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + def test_fit_basis_chebyshev1basis(self): + """Fit a Chebyshev1 basis.""" + + f = lambda x: x + interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev1Basis, + gtsam.Chebyshev1Basis, f) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + def test_fit_basis_chebyshev2basis(self): + """Fit a Chebyshev2 basis.""" + + f = lambda x: x + interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev2Basis, + gtsam.Chebyshev2Basis) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + def test_fit_basis_chebyshev2(self): + """Fit a Chebyshev2 pseudospectral basis.""" + + f = lambda x: x + interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev2, + gtsam.Chebyshev2) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py index e36657178..6cae98ff5 100644 --- a/python/gtsam/tests/test_dsf_map.py +++ b/python/gtsam/tests/test_dsf_map.py @@ -6,49 +6,68 @@ All Rights Reserved See LICENSE for the license information Unit tests for Disjoint Set Forest. -Author: Frank Dellaert & Varun Agrawal +Author: Frank Dellaert & Varun Agrawal & John Lambert """ # pylint: disable=invalid-name, no-name-in-module, no-member from __future__ import print_function import unittest +from typing import Tuple import gtsam +from gtsam import IndexPair from gtsam.utils.test_case import GtsamTestCase class TestDSFMap(GtsamTestCase): """Tests for DSFMap.""" - def test_all(self): + def test_all(self) -> None: """Test everything in DFSMap.""" - def key(index_pair): + + def key(index_pair) -> Tuple[int, int]: return index_pair.i(), index_pair.j() dsf = gtsam.DSFMapIndexPair() pair1 = gtsam.IndexPair(1, 18) self.assertEqual(key(dsf.find(pair1)), key(pair1)) pair2 = gtsam.IndexPair(2, 2) - + # testing the merge feature of dsf dsf.merge(pair1, pair2) self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2))) - def test_sets(self): - from gtsam import IndexPair + def test_sets(self) -> None: + """Ensure that pairs are merged correctly during Union-Find. + + An IndexPair (i,k) representing a unique key might represent the + k'th detected keypoint in image i. For the data below, merging such + measurements into feature tracks across frames should create 2 distinct sets. + """ dsf = gtsam.DSFMapIndexPair() - dsf.merge(IndexPair(0, 1), IndexPair(1,2)) - dsf.merge(IndexPair(0, 1), IndexPair(3,4)) - dsf.merge(IndexPair(4,5), IndexPair(6,8)) + dsf.merge(IndexPair(0, 1), IndexPair(1, 2)) + dsf.merge(IndexPair(0, 1), IndexPair(3, 4)) + dsf.merge(IndexPair(4, 5), IndexPair(6, 8)) sets = dsf.sets() + merged_sets = set() + for i in sets: + set_keys = [] s = sets[i] for val in gtsam.IndexPairSetAsArray(s): - val.i() - val.j() + set_keys.append((val.i(), val.j())) + merged_sets.add(tuple(set_keys)) + + # fmt: off + expected_sets = { + ((0, 1), (1, 2), (3, 4)), # set 1 + ((4, 5), (6, 8)) # set 2 + } + # fmt: on + assert expected_sets == merged_sets -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/utils/circlePose3.py b/python/gtsam/utils/circlePose3.py index e1def9427..5cd3a07ce 100644 --- a/python/gtsam/utils/circlePose3.py +++ b/python/gtsam/utils/circlePose3.py @@ -1,10 +1,10 @@ -import gtsam -import math + import numpy as np -from math import pi +import gtsam +from gtsam import Values -def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): +def circlePose3(numPoses: int = 8, radius: float = 1.0, symbolChar: str = '\0') -> Values: """ circlePose3 generates a set of poses in a circle. This function returns those poses inside a gtsam.Values object, with sequential @@ -21,14 +21,21 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): values = gtsam.Values() theta = 0.0 - dtheta = 2 * pi / numPoses + dtheta = 2 * np.pi / numPoses gRo = gtsam.Rot3( - np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) + np.array( + [ + [0., 1., 0.], + [1., 0., 0.], + [0., 0., -1.] + ], order='F' + ) + ) for i in range(numPoses): key = gtsam.symbol(symbolChar, i) - gti = gtsam.Point3(radius * math.cos(theta), radius * math.sin(theta), 0) - oRi = gtsam.Rot3.Yaw( - -theta) # negative yaw goes counterclockwise, with Z down ! + gti = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), 0) + # negative yaw goes counterclockwise, with Z down ! + oRi = gtsam.Rot3.Yaw(-theta) gTi = gtsam.Pose3(gRo.compose(oRi), gti) values.insert(key, gTi) theta = theta + dtheta diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 7f48d03a3..7ea393077 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -2,25 +2,31 @@ # pylint: disable=no-member, invalid-name +from typing import Iterable, Optional, Tuple + import matplotlib.pyplot as plt import numpy as np from matplotlib import patches from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import import gtsam +from gtsam import Marginals, Point3, Pose2, Pose3, Values -def set_axes_equal(fignum): +def set_axes_equal(fignum: int) -> None: """ Make axes of 3D plot have equal scale so that spheres appear as spheres, cubes as cubes, etc.. This is one possible solution to Matplotlib's ax.set_aspect('equal') and ax.axis('equal') not working for 3D. Args: - fignum (int): An integer representing the figure number for Matplotlib. + fignum: An integer representing the figure number for Matplotlib. """ fig = plt.figure(fignum) - ax = fig.gca(projection='3d') + if not fig.axes: + ax = fig.add_subplot(projection='3d') + else: + ax = fig.axes[0] limits = np.array([ ax.get_xlim3d(), @@ -36,21 +42,22 @@ def set_axes_equal(fignum): ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) -def ellipsoid(rx, ry, rz, n): +def ellipsoid(rx: float, ry: float, rz: float, + n: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """ Numpy equivalent of Matlab's ellipsoid function. Args: - rx (double): Radius of ellipsoid in X-axis. - ry (double): Radius of ellipsoid in Y-axis. - rz (double): Radius of ellipsoid in Z-axis. - n (int): The granularity of the ellipsoid plotted. + rx: Radius of ellipsoid in X-axis. + ry: Radius of ellipsoid in Y-axis. + rz: Radius of ellipsoid in Z-axis. + n: The granularity of the ellipsoid plotted. Returns: - tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot. + The points in the x, y and z axes to use for the surface plot. """ - u = np.linspace(0, 2*np.pi, n+1) - v = np.linspace(0, np.pi, n+1) + u = np.linspace(0, 2 * np.pi, n + 1) + v = np.linspace(0, np.pi, n + 1) x = -rx * np.outer(np.cos(u), np.sin(v)).T y = -ry * np.outer(np.sin(u), np.sin(v)).T z = -rz * np.outer(np.ones_like(u), np.cos(v)).T @@ -58,7 +65,12 @@ def ellipsoid(rx, ry, rz, n): return x, y, z -def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): +def plot_covariance_ellipse_3d(axes, + origin: Point3, + P: np.ndarray, + scale: float = 1, + n: int = 8, + alpha: float = 0.5) -> None: """ Plots a Gaussian as an uncertainty ellipse @@ -68,12 +80,12 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): Args: axes (matplotlib.axes.Axes): Matplotlib axes. - origin (gtsam.Point3): The origin in the world frame. - P (numpy.ndarray): The marginal covariance matrix of the 3D point + origin: The origin in the world frame. + P: The marginal covariance matrix of the 3D point which will be represented as an ellipse. - scale (float): Scaling factor of the radii of the covariance ellipse. - n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses. - alpha (float): Transparency value for the plotted surface in the range [0, 1]. + scale: Scaling factor of the radii of the covariance ellipse. + n: Defines the granularity of the ellipse. Higher values indicate finer ellipses. + alpha: Transparency value for the plotted surface in the range [0, 1]. """ k = 11.82 U, S, _ = np.linalg.svd(P) @@ -90,20 +102,23 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): np.kron(U[:, 2:3], zc) n = data.shape[1] x = data[0:n, :] + origin[0] - y = data[n:2*n, :] + origin[1] - z = data[2*n:, :] + origin[2] + y = data[n:2 * n, :] + origin[1] + z = data[2 * n:, :] + origin[2] axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') -def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): +def plot_pose2_on_axes(axes, + pose: Pose2, + axis_length: float = 0.1, + covariance: np.ndarray = None) -> None: """ Plot a 2D pose on given axis `axes` with given `axis_length`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. - pose (gtsam.Pose2): The pose to be plotted. - axis_length (float): The length of the camera axes. + pose: The pose to be plotted. + axis_length: The length of the camera axes. covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ @@ -131,28 +146,38 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): k = 5.0 angle = np.arctan2(v[1, 0], v[0, 0]) - e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k), - np.rad2deg(angle), fill=False) + e1 = patches.Ellipse(origin, + np.sqrt(w[0] * k), + np.sqrt(w[1] * k), + np.rad2deg(angle), + fill=False) axes.add_patch(e1) -def plot_pose2(fignum, pose, axis_length=0.1, covariance=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_pose2( + fignum: int, + pose: Pose2, + axis_length: float = 0.1, + covariance: np.ndarray = None, + axis_labels=("X axis", "Y axis", "Z axis"), +) -> plt.Figure: """ Plot a 2D pose on given figure with given `axis_length`. Args: - fignum (int): Integer representing the figure number to use for plotting. - pose (gtsam.Pose2): The pose to be plotted. - axis_length (float): The length of the camera axes. - covariance (numpy.ndarray): Marginal covariance matrix to plot + fignum: Integer representing the figure number to use for plotting. + pose: The pose to be plotted. + axis_length: The length of the camera axes. + covariance: Marginal covariance matrix to plot the uncertainty of the estimation. axis_labels (iterable[string]): List of axis labels to set. """ # get figure object fig = plt.figure(fignum) axes = fig.gca() - plot_pose2_on_axes(axes, pose, axis_length=axis_length, + plot_pose2_on_axes(axes, + pose, + axis_length=axis_length, covariance=covariance) axes.set_xlabel(axis_labels[0]) @@ -161,32 +186,40 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None, return fig -def plot_point3_on_axes(axes, point, linespec, P=None): +def plot_point3_on_axes(axes, + point: Point3, + linespec: str, + P: Optional[np.ndarray] = None) -> None: """ Plot a 3D point on given axis `axes` with given `linespec`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. - point (gtsam.Point3): The point to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + point: The point to be plotted. + linespec: String representing formatting options for Matplotlib. + P: Marginal covariance matrix to plot the uncertainty of the estimation. """ axes.plot([point[0]], [point[1]], [point[2]], linespec) if P is not None: plot_covariance_ellipse_3d(axes, point, P) -def plot_point3(fignum, point, linespec, P=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_point3( + fignum: int, + point: Point3, + linespec: str, + P: np.ndarray = None, + axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"), +) -> plt.Figure: """ Plot a 3D point on given figure with given `linespec`. Args: - fignum (int): Integer representing the figure number to use for plotting. - point (gtsam.Point3): The point to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. + fignum: Integer representing the figure number to use for plotting. + point: The point to be plotted. + linespec: String representing formatting options for Matplotlib. + P: Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels: List of axis labels to set. Returns: fig: The matplotlib figure. @@ -203,8 +236,12 @@ def plot_point3(fignum, point, linespec, P=None, return fig -def plot_3d_points(fignum, values, linespec="g*", marginals=None, - title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_3d_points(fignum, + values, + linespec="g*", + marginals=None, + title="3D Points", + axis_labels=('X axis', 'Y axis', 'Z axis')): """ Plots the Point3s in `values`, with optional covariances. Finds all the Point3 objects in the given Values object and plots them. @@ -232,7 +269,10 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None, else: covariance = None - fig = plot_point3(fignum, point, linespec, covariance, + fig = plot_point3(fignum, + point, + linespec, + covariance, axis_labels=axis_labels) except RuntimeError: @@ -280,26 +320,34 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): plot_covariance_ellipse_3d(axes, origin, gPp) -def plot_pose3(fignum, pose, axis_length=0.1, P=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_pose3( + fignum: int, + pose: Pose3, + axis_length: float = 0.1, + P: np.ndarray = None, + axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"), +) -> plt.Figure: """ Plot a 3D pose on given figure with given `axis_length`. Args: - fignum (int): Integer representing the figure number to use for plotting. + fignum: Integer representing the figure number to use for plotting. pose (gtsam.Pose3): 3D pose to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. + axis_length: The length of the camera axes. + P: Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels: List of axis labels to set. Returns: fig: The matplotlib figure. """ # get figure object fig = plt.figure(fignum) - axes = fig.gca(projection='3d') - plot_pose3_on_axes(axes, pose, P=P, - axis_length=axis_length) + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] + + plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) axes.set_xlabel(axis_labels[0]) axes.set_ylabel(axis_labels[1]) @@ -308,18 +356,24 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None, return fig -def plot_trajectory(fignum, values, scale=1, marginals=None, - title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_trajectory( + fignum: int, + values: Values, + scale: float = 1, + marginals: Marginals = None, + title: str = "Plot Trajectory", + axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"), +) -> None: """ Plot a complete 2D/3D trajectory using poses in `values`. Args: - fignum (int): Integer representing the figure number to use for plotting. - values (gtsam.Values): Values containing some Pose2 and/or Pose3 values. - scale (float): Value to scale the poses by. - marginals (gtsam.Marginals): Marginalized probability values of the estimation. + fignum: Integer representing the figure number to use for plotting. + values: Values containing some Pose2 and/or Pose3 values. + scale: Value to scale the poses by. + marginals: Marginalized probability values of the estimation. Used to plot uncertainty bounds. - title (string): The title of the plot. + title: The title of the plot. axis_labels (iterable[string]): List of axis labels to set. """ fig = plt.figure(fignum) @@ -338,7 +392,9 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, else: covariance = None - plot_pose2_on_axes(axes, pose, covariance=covariance, + plot_pose2_on_axes(axes, + pose, + covariance=covariance, axis_length=scale) # Then 3D poses, if any @@ -350,27 +406,29 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, else: covariance = None - plot_pose3_on_axes(axes, pose, P=covariance, - axis_length=scale) + plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale) fig.suptitle(title) fig.canvas.set_window_title(title.lower()) -def plot_incremental_trajectory(fignum, values, start=0, - scale=1, marginals=None, - time_interval=0.0): +def plot_incremental_trajectory(fignum: int, + values: Values, + start: int = 0, + scale: float = 1, + marginals: Optional[Marginals] = None, + time_interval: float = 0.0) -> None: """ Incrementally plot a complete 3D trajectory using poses in `values`. Args: - fignum (int): Integer representing the figure number to use for plotting. - values (gtsam.Values): Values dict containing the poses. - start (int): Starting index to start plotting from. - scale (float): Value to scale the poses by. - marginals (gtsam.Marginals): Marginalized probability values of the estimation. + fignum: Integer representing the figure number to use for plotting. + values: Values dict containing the poses. + start: Starting index to start plotting from. + scale: Value to scale the poses by. + marginals: Marginalized probability values of the estimation. Used to plot uncertainty bounds. - time_interval (float): Time in seconds to pause between each rendering. + time_interval: Time in seconds to pause between each rendering. Used to create animation effect. """ fig = plt.figure(fignum) diff --git a/python/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py index 32ccbc8fa..51852760a 100644 --- a/python/gtsam/utils/visual_data_generator.py +++ b/python/gtsam/utils/visual_data_generator.py @@ -1,8 +1,10 @@ from __future__ import print_function +from typing import Tuple -import numpy as np import math +import numpy as np from math import pi + import gtsam from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2 @@ -12,7 +14,7 @@ class Options: Options to generate test scenario """ - def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): + def __init__(self, triangle: bool = False, nrCameras: int = 3, K=Cal3_S2()) -> None: """ Options to generate test scenario @param triangle: generate a triangle scene with 3 points if True, otherwise @@ -29,12 +31,12 @@ class GroundTruth: Object holding generated ground-truth data """ - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None: self.K = K self.cameras = [Pose3()] * nrCameras self.points = [Point3(0, 0, 0)] * nrPoints - def print_(self, s=""): + def print_(self, s="") -> None: print(s) print("K = ", self.K) print("Cameras: ", len(self.cameras)) @@ -54,7 +56,7 @@ class Data: class NoiseModels: pass - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None: self.K = K self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] @@ -72,7 +74,7 @@ class Data: self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) -def generate_data(options): +def generate_data(options) -> Tuple[Data, GroundTruth]: """ Generate ground-truth and measurement data. """ K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) diff --git a/wrap/DOCS.md b/wrap/DOCS.md index c8285baef..f08f741ff 100644 --- a/wrap/DOCS.md +++ b/wrap/DOCS.md @@ -133,9 +133,10 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the template class Class2 { ... }; typedef Class2 MyInstantiatedClass; ``` - - Templates can also be defined for methods, properties and static methods. + - Templates can also be defined for constructors, methods, properties and static methods. - In the class definition, appearances of the template argument(s) will be replaced with their instantiated types, e.g. `void setValue(const T& value);`. + - Values scoped within templates are supported. E.g. one can use the form `T::Value` where T is a template, as an argument to a method. - To refer to the instantiation of the template class itself, use `This`, i.e. `static This Create();`. - To create new instantiations in other modules, you must copy-and-paste the whole class definition into the new module, but use only your new instantiation types. diff --git a/wrap/gtwrap/interface_parser/classes.py b/wrap/gtwrap/interface_parser/classes.py index 11317962d..841c963c2 100644 --- a/wrap/gtwrap/interface_parser/classes.py +++ b/wrap/gtwrap/interface_parser/classes.py @@ -119,24 +119,27 @@ class Constructor: Can have 0 or more arguments. """ rule = ( - IDENT("name") # + Optional(Template.rule("template")) # + + IDENT("name") # + LPAREN # + ArgumentList.rule("args_list") # + RPAREN # + SEMI_COLON # BR - ).setParseAction(lambda t: Constructor(t.name, t.args_list)) + ).setParseAction(lambda t: Constructor(t.name, t.args_list, t.template)) def __init__(self, name: str, args: ArgumentList, + template: Union[Template, Any], parent: Union["Class", Any] = ''): self.name = name self.args = args + self.template = template self.parent = parent def __repr__(self) -> str: - return "Constructor: {}".format(self.name) + return "Constructor: {}{}".format(self.name, self.args) class Operator: @@ -260,17 +263,9 @@ class Class: + RBRACE # + SEMI_COLON # BR ).setParseAction(lambda t: Class( - t.template, - t.is_virtual, - t.name, - t.parent_class, - t.members.ctors, - t.members.methods, - t.members.static_methods, - t.members.properties, - t.members.operators, - t.members.enums - )) + t.template, t.is_virtual, t.name, t.parent_class, t.members.ctors, t. + members.methods, t.members.static_methods, t.members.properties, t. + members.operators, t.members.enums)) def __init__( self, diff --git a/wrap/gtwrap/interface_parser/function.py b/wrap/gtwrap/interface_parser/function.py index 9fe1f56f0..9e68c6ece 100644 --- a/wrap/gtwrap/interface_parser/function.py +++ b/wrap/gtwrap/interface_parser/function.py @@ -81,7 +81,7 @@ class ArgumentList: return ArgumentList([]) def __repr__(self) -> str: - return self.args_list.__repr__() + return repr(tuple(self.args_list)) def __len__(self) -> int: return len(self.args_list) diff --git a/wrap/gtwrap/template_instantiator.py b/wrap/gtwrap/template_instantiator.py index 0cda93d5d..b4d79655d 100644 --- a/wrap/gtwrap/template_instantiator.py +++ b/wrap/gtwrap/template_instantiator.py @@ -9,6 +9,17 @@ from typing import Any, Iterable, List, Sequence import gtwrap.interface_parser as parser +def is_scoped_template(template_typenames, str_arg_typename): + """ + Check if the template given by `str_arg_typename` is a scoped template, + and if so, return what template and index matches the scoped template correctly. + """ + for idx, template in enumerate(template_typenames): + if template in str_arg_typename.split("::"): + return template, idx + return False, -1 + + def instantiate_type(ctype: parser.Type, template_typenames: List[str], instantiations: List[parser.Typename], @@ -41,7 +52,30 @@ def instantiate_type(ctype: parser.Type, str_arg_typename = str(ctype.typename) - if str_arg_typename in template_typenames: + # Check if template is a scoped template e.g. T::Value where T is the template + scoped_template, scoped_idx = is_scoped_template(template_typenames, + str_arg_typename) + + # Instantiate templates which have enumerated instantiations in the template. + # E.g. `template`. + + # Instantiate scoped templates, e.g. T::Value. + if scoped_template: + # Create a copy of the instantiation so we can modify it. + instantiation = deepcopy(instantiations[scoped_idx]) + # Replace the part of the template with the instantiation + instantiation.name = str_arg_typename.replace(scoped_template, + instantiation.name) + return parser.Type( + typename=instantiation, + is_const=ctype.is_const, + is_shared_ptr=ctype.is_shared_ptr, + is_ptr=ctype.is_ptr, + is_ref=ctype.is_ref, + is_basic=ctype.is_basic, + ) + # Check for exact template match. + elif str_arg_typename in template_typenames: idx = template_typenames.index(str_arg_typename) return parser.Type( typename=instantiations[idx], @@ -51,14 +85,15 @@ def instantiate_type(ctype: parser.Type, is_ref=ctype.is_ref, is_basic=ctype.is_basic, ) + + # If a method has the keyword `This`, we replace it with the (instantiated) class. elif str_arg_typename == 'This': + # Check if the class is template instantiated + # so we can replace it with the instantiated version. if instantiated_class: name = instantiated_class.original.name namespaces_name = instantiated_class.namespaces() namespaces_name.append(name) - # print("INST: {}, {}, CPP: {}, CLS: {}".format( - # ctype, instantiations, cpp_typename, instantiated_class.instantiations - # ), file=sys.stderr) cpp_typename = parser.Typename( namespaces_name, instantiations=instantiated_class.instantiations) @@ -71,6 +106,14 @@ def instantiate_type(ctype: parser.Type, is_ref=ctype.is_ref, is_basic=ctype.is_basic, ) + + # Case when 'This' is present in the type namespace, e.g `This::Subclass`. + elif 'This' in str_arg_typename: + # Simply get the index of `This` in the namespace and replace it with the instantiated name. + namespace_idx = ctype.typename.namespaces.index('This') + ctype.typename.namespaces[namespace_idx] = cpp_typename.name + return ctype + else: return ctype @@ -368,19 +411,46 @@ class InstantiatedClass(parser.Class): """ instantiated_ctors = [] - for ctor in self.original.ctors: + def instantiate(instantiated_ctors, ctor, typenames, instantiations): instantiated_args = instantiate_args_list( ctor.args.list(), typenames, - self.instantiations, + instantiations, self.cpp_typename(), ) instantiated_ctors.append( parser.Constructor( name=self.name, args=parser.ArgumentList(instantiated_args), + template=self.original.template, parent=self, )) + return instantiated_ctors + + for ctor in self.original.ctors: + # Add constructor templates to the typenames and instantiations + if isinstance(ctor.template, parser.template.Template): + typenames.extend(ctor.template.typenames) + + # Get all combinations of template args + for instantiations in itertools.product( + *ctor.template.instantiations): + instantiations = self.instantiations + list(instantiations) + + instantiated_ctors = instantiate( + instantiated_ctors, + ctor, + typenames=typenames, + instantiations=instantiations) + + else: + # If no constructor level templates, just use the class templates + instantiated_ctors = instantiate( + instantiated_ctors, + ctor, + typenames=typenames, + instantiations=self.instantiations) + return instantiated_ctors def instantiate_static_methods(self, typenames): diff --git a/wrap/tests/expected/matlab/MyFactorPosePoint2.m b/wrap/tests/expected/matlab/MyFactorPosePoint2.m index 2dd4b5428..e4cd92196 100644 --- a/wrap/tests/expected/matlab/MyFactorPosePoint2.m +++ b/wrap/tests/expected/matlab/MyFactorPosePoint2.m @@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(56, my_ptr); + class_wrapper(62, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = class_wrapper(57, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(63, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - class_wrapper(58, obj.ptr_MyFactorPosePoint2); + class_wrapper(64, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end @@ -36,7 +36,7 @@ classdef MyFactorPosePoint2 < handle % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter') - class_wrapper(59, this, varargin{:}); + class_wrapper(65, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/wrap/tests/expected/matlab/class_wrapper.cpp b/wrap/tests/expected/matlab/class_wrapper.cpp index fab9c1450..48d6b2dce 100644 --- a/wrap/tests/expected/matlab/class_wrapper.cpp +++ b/wrap/tests/expected/matlab/class_wrapper.cpp @@ -33,6 +33,8 @@ typedef std::set*> Collector_Multip static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; typedef std::set*> Collector_ForwardKinematics; static Collector_ForwardKinematics collector_ForwardKinematics; +typedef std::set*> Collector_TemplatedConstructor; +static Collector_TemplatedConstructor collector_TemplatedConstructor; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -97,6 +99,12 @@ void _deleteAllObjects() collector_ForwardKinematics.erase(iter++); anyDeleted = true; } } + { for(Collector_TemplatedConstructor::iterator iter = collector_TemplatedConstructor.begin(); + iter != collector_TemplatedConstructor.end(); ) { + delete *iter; + collector_TemplatedConstructor.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -682,7 +690,76 @@ void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, } } -void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_TemplatedConstructor.insert(self); +} + +void TemplatedConstructor_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new TemplatedConstructor()); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + string& arg = *unwrap_shared_ptr< string >(in[0], "ptr_string"); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + int arg = unwrap< int >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double arg = unwrap< double >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_deconstructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_TemplatedConstructor",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_TemplatedConstructor::iterator item; + item = collector_TemplatedConstructor.find(self); + if(item != collector_TemplatedConstructor.end()) { + delete self; + collector_TemplatedConstructor.erase(item); + } +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -691,7 +768,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -706,7 +783,7 @@ void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -719,7 +796,7 @@ void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin } } -void MyFactorPosePoint2_print_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -909,16 +986,34 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1); break; case 56: - MyFactorPosePoint2_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); + TemplatedConstructor_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); break; case 57: - MyFactorPosePoint2_constructor_57(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_57(nargout, out, nargin-1, in+1); break; case 58: - MyFactorPosePoint2_deconstructor_58(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_58(nargout, out, nargin-1, in+1); break; case 59: - MyFactorPosePoint2_print_59(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_59(nargout, out, nargin-1, in+1); + break; + case 60: + TemplatedConstructor_constructor_60(nargout, out, nargin-1, in+1); + break; + case 61: + TemplatedConstructor_deconstructor_61(nargout, out, nargin-1, in+1); + break; + case 62: + MyFactorPosePoint2_collectorInsertAndMakeBase_62(nargout, out, nargin-1, in+1); + break; + case 63: + MyFactorPosePoint2_constructor_63(nargout, out, nargin-1, in+1); + break; + case 64: + MyFactorPosePoint2_deconstructor_64(nargout, out, nargin-1, in+1); + break; + case 65: + MyFactorPosePoint2_print_65(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/wrap/tests/expected/matlab/template_wrapper.cpp b/wrap/tests/expected/matlab/template_wrapper.cpp new file mode 100644 index 000000000..532bdd57a --- /dev/null +++ b/wrap/tests/expected/matlab/template_wrapper.cpp @@ -0,0 +1,225 @@ +#include +#include + +#include +#include +#include + + + +typedef ScopedTemplate ScopedTemplateResult; + +typedef std::set*> Collector_TemplatedConstructor; +static Collector_TemplatedConstructor collector_TemplatedConstructor; +typedef std::set*> Collector_ScopedTemplateResult; +static Collector_ScopedTemplateResult collector_ScopedTemplateResult; + + +void _deleteAllObjects() +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + bool anyDeleted = false; + { for(Collector_TemplatedConstructor::iterator iter = collector_TemplatedConstructor.begin(); + iter != collector_TemplatedConstructor.end(); ) { + delete *iter; + collector_TemplatedConstructor.erase(iter++); + anyDeleted = true; + } } + { for(Collector_ScopedTemplateResult::iterator iter = collector_ScopedTemplateResult.begin(); + iter != collector_ScopedTemplateResult.end(); ) { + delete *iter; + collector_ScopedTemplateResult.erase(iter++); + anyDeleted = true; + } } + + if(anyDeleted) + cout << + "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" + "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n" + "module, so that your recompiled module is used instead of the old one." << endl; + std::cout.rdbuf(outbuf); +} + +void _template_RTTIRegister() { + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_template_rttiRegistry_created"); + if(!alreadyCreated) { + std::map types; + + + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(const StringPair& rtti_matlab: types) { + int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); + if(fieldId < 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); + mxSetFieldByNumber(registry, 0, fieldId, matlabName); + } + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(registry); + + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(newAlreadyCreated); + } +} + +void TemplatedConstructor_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_TemplatedConstructor.insert(self); +} + +void TemplatedConstructor_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new TemplatedConstructor()); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + string& arg = *unwrap_shared_ptr< string >(in[0], "ptr_string"); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + int arg = unwrap< int >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double arg = unwrap< double >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_deconstructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_TemplatedConstructor",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_TemplatedConstructor::iterator item; + item = collector_TemplatedConstructor.find(self); + if(item != collector_TemplatedConstructor.end()) { + delete self; + collector_TemplatedConstructor.erase(item); + } +} + +void ScopedTemplateResult_collectorInsertAndMakeBase_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_ScopedTemplateResult.insert(self); +} + +void ScopedTemplateResult_constructor_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Result::Value& arg = *unwrap_shared_ptr< Result::Value >(in[0], "ptr_Result::Value"); + Shared *self = new Shared(new ScopedTemplate(arg)); + collector_ScopedTemplateResult.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void ScopedTemplateResult_deconstructor_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr> Shared; + checkArguments("delete_ScopedTemplateResult",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_ScopedTemplateResult::iterator item; + item = collector_ScopedTemplateResult.find(self); + if(item != collector_ScopedTemplateResult.end()) { + delete self; + collector_ScopedTemplateResult.erase(item); + } +} + + +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + _template_RTTIRegister(); + + int id = unwrap(in[0]); + + try { + switch(id) { + case 0: + TemplatedConstructor_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + break; + case 1: + TemplatedConstructor_constructor_1(nargout, out, nargin-1, in+1); + break; + case 2: + TemplatedConstructor_constructor_2(nargout, out, nargin-1, in+1); + break; + case 3: + TemplatedConstructor_constructor_3(nargout, out, nargin-1, in+1); + break; + case 4: + TemplatedConstructor_constructor_4(nargout, out, nargin-1, in+1); + break; + case 5: + TemplatedConstructor_deconstructor_5(nargout, out, nargin-1, in+1); + break; + case 6: + ScopedTemplateResult_collectorInsertAndMakeBase_6(nargout, out, nargin-1, in+1); + break; + case 7: + ScopedTemplateResult_constructor_7(nargout, out, nargin-1, in+1); + break; + case 8: + ScopedTemplateResult_deconstructor_8(nargout, out, nargin-1, in+1); + break; + } + } catch(const std::exception& e) { + mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); + } + + std::cout.rdbuf(outbuf); +} diff --git a/wrap/tests/expected/python/class_pybind.cpp b/wrap/tests/expected/python/class_pybind.cpp index 4c2371a42..f36c0a84c 100644 --- a/wrap/tests/expected/python/class_pybind.cpp +++ b/wrap/tests/expected/python/class_pybind.cpp @@ -86,6 +86,12 @@ PYBIND11_MODULE(class_py, m_) { py::class_>(m_, "ForwardKinematics") .def(py::init(), py::arg("robot"), py::arg("start_link_name"), py::arg("end_link_name"), py::arg("joint_angles"), py::arg("l2Tp") = gtsam::Pose3()); + py::class_>(m_, "TemplatedConstructor") + .def(py::init<>()) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")); + py::class_, std::shared_ptr>>(m_, "MyFactorPosePoint2") .def(py::init>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel")) .def("print",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) diff --git a/wrap/tests/expected/python/enum_pybind.cpp b/wrap/tests/expected/python/enum_pybind.cpp index ffc68ece0..73b74bd86 100644 --- a/wrap/tests/expected/python/enum_pybind.cpp +++ b/wrap/tests/expected/python/enum_pybind.cpp @@ -69,6 +69,16 @@ PYBIND11_MODULE(enum_py, m_) { .value("Groot", gtsam::MCU::GotG::Groot); + py::class_, std::shared_ptr>> optimizergaussnewtonparams(m_gtsam, "OptimizerGaussNewtonParams"); + optimizergaussnewtonparams + .def("setVerbosity",[](gtsam::Optimizer* self, const Optimizer::Verbosity value){ self->setVerbosity(value);}, py::arg("value")); + + py::enum_::Verbosity>(optimizergaussnewtonparams, "Verbosity", py::arithmetic()) + .value("SILENT", gtsam::Optimizer::Verbosity::SILENT) + .value("SUMMARY", gtsam::Optimizer::Verbosity::SUMMARY) + .value("VERBOSE", gtsam::Optimizer::Verbosity::VERBOSE); + + #include "python/specializations.h" diff --git a/wrap/tests/expected/python/templates_pybind.cpp b/wrap/tests/expected/python/templates_pybind.cpp new file mode 100644 index 000000000..4d13d3731 --- /dev/null +++ b/wrap/tests/expected/python/templates_pybind.cpp @@ -0,0 +1,38 @@ + + +#include +#include +#include +#include +#include "gtsam/nonlinear/utilities.h" // for RedirectCout. + + +#include "wrap/serialization.h" +#include + + + + + +using namespace std; + +namespace py = pybind11; + +PYBIND11_MODULE(templates_py, m_) { + m_.doc() = "pybind11 wrapper of templates_py"; + + + py::class_>(m_, "TemplatedConstructor") + .def(py::init<>()) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")); + + py::class_, std::shared_ptr>>(m_, "ScopedTemplateResult") + .def(py::init(), py::arg("arg")); + + +#include "python/specializations.h" + +} + diff --git a/wrap/tests/fixtures/class.i b/wrap/tests/fixtures/class.i index 9e30b17b5..9923ffce7 100644 --- a/wrap/tests/fixtures/class.i +++ b/wrap/tests/fixtures/class.i @@ -7,6 +7,7 @@ class FunRange { template class Fun { + static This staticMethodWithThis(); template @@ -118,5 +119,14 @@ class ForwardKinematics { const gtsam::Pose3& l2Tp = gtsam::Pose3()); }; +// Test for templated constructor +class TemplatedConstructor { + TemplatedConstructor(); + + template + TemplatedConstructor(const T& arg); +}; + + class SuperCoolFactor; typedef SuperCoolFactor SuperCoolFactorPose3; diff --git a/wrap/tests/fixtures/enum.i b/wrap/tests/fixtures/enum.i index 9386a33df..71918c25a 100644 --- a/wrap/tests/fixtures/enum.i +++ b/wrap/tests/fixtures/enum.i @@ -42,4 +42,17 @@ class MCU { }; +template +class Optimizer { + enum Verbosity { + SILENT, + SUMMARY, + VERBOSE + }; + + void setVerbosity(const This::Verbosity value); +}; + +typedef gtsam::Optimizer OptimizerGaussNewtonParams; + } // namespace gtsam diff --git a/wrap/tests/fixtures/templates.i b/wrap/tests/fixtures/templates.i new file mode 100644 index 000000000..c485041c6 --- /dev/null +++ b/wrap/tests/fixtures/templates.i @@ -0,0 +1,15 @@ +// Test for templated constructor +class TemplatedConstructor { + TemplatedConstructor(); + + template + TemplatedConstructor(const T& arg); +}; + +// Test for a scoped value inside a template +template +class ScopedTemplate { + // T should be properly substituted here. + ScopedTemplate(const T::Value& arg); +}; + diff --git a/wrap/tests/test_interface_parser.py b/wrap/tests/test_interface_parser.py index 95987f42f..25e4178a6 100644 --- a/wrap/tests/test_interface_parser.py +++ b/wrap/tests/test_interface_parser.py @@ -314,6 +314,25 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual(5, len(ret.args)) self.assertEqual("gtsam::Pose3()", ret.args.list()[4].default) + def test_constructor_templated(self): + """Test for templated class constructor.""" + f = """ + template + Class(); + """ + ret = Constructor.rule.parseString(f)[0] + self.assertEqual("Class", ret.name) + self.assertEqual(0, len(ret.args)) + + f = """ + template + Class(const T& name); + """ + ret = Constructor.rule.parseString(f)[0] + self.assertEqual("Class", ret.name) + self.assertEqual(1, len(ret.args)) + self.assertEqual("const T & name", ret.args.args_list[0].to_cpp()) + def test_operator_overload(self): """Test for operator overloading.""" # Unary operator diff --git a/wrap/tests/test_matlab_wrapper.py b/wrap/tests/test_matlab_wrapper.py index 112750721..89e53ab21 100644 --- a/wrap/tests/test_matlab_wrapper.py +++ b/wrap/tests/test_matlab_wrapper.py @@ -123,6 +123,23 @@ class TestWrap(unittest.TestCase): for file in files: self.compare_and_diff(file) + def test_templates(self): + """Test interface file with template info.""" + file = osp.join(self.INTERFACE_DIR, 'templates.i') + + wrapper = MatlabWrapper( + module_name='template', + top_module_namespace=['gtsam'], + ignore_classes=[''], + ) + + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) + + files = ['template_wrapper.cpp'] + + for file in files: + self.compare_and_diff(file) + def test_inheritance(self): """Test interface file with class inheritance definitions.""" file = osp.join(self.INTERFACE_DIR, 'inheritance.i') diff --git a/wrap/tests/test_pybind_wrapper.py b/wrap/tests/test_pybind_wrapper.py index 67c637d14..b47b4aca1 100644 --- a/wrap/tests/test_pybind_wrapper.py +++ b/wrap/tests/test_pybind_wrapper.py @@ -95,6 +95,14 @@ class TestWrap(unittest.TestCase): self.compare_and_diff('class_pybind.cpp', output) + def test_templates(self): + """Test interface file with templated class.""" + source = osp.join(self.INTERFACE_DIR, 'templates.i') + output = self.wrap_content([source], 'templates_py', + self.PYTHON_ACTUAL_DIR) + + self.compare_and_diff('templates_pybind.cpp', output) + def test_inheritance(self): """Test interface file with class inheritance definitions.""" source = osp.join(self.INTERFACE_DIR, 'inheritance.i')