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 6abbb5596..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 \ @@ -111,9 +113,9 @@ function test () # Actual testing if [ "$(uname)" == "Linux" ]; then - make -j$(nproc) + make -j$(nproc) check elif [ "$(uname)" == "Darwin" ]; then - make -j$(sysctl -n hw.physicalcpu) + make -j$(sysctl -n hw.physicalcpu) check fi finish diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 3bb3fa298..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 @@ -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/CMakeLists.txt b/CMakeLists.txt index 2fdadc68a..b8480867e 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 @@ -102,6 +105,11 @@ endif() GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) +if (GTSAM_BUILD_UNSTABLE) + GtsamMakeConfigFile(GTSAM_UNSTABLE "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") + export(TARGETS ${GTSAM_UNSTABLE_EXPORTED_TARGETS} FILE GTSAM_UNSTABLE-exports.cmake) +endif() + # Check for doxygen availability - optional dependency find_package(Doxygen) diff --git a/INSTALL.md b/INSTALL.md index 3a0f2896a..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.65 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.65 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/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/GtsamMakeConfigFile.cmake b/cmake/GtsamMakeConfigFile.cmake index 0479a2524..91cb98a8c 100644 --- a/cmake/GtsamMakeConfigFile.cmake +++ b/cmake/GtsamMakeConfigFile.cmake @@ -27,6 +27,8 @@ function(GtsamMakeConfigFile PACKAGE_NAME) # here. if(NOT DEFINED ${PACKAGE_NAME}_VERSION AND DEFINED ${PACKAGE_NAME}_VERSION_STRING) set(${PACKAGE_NAME}_VERSION ${${PACKAGE_NAME}_VERSION_STRING}) + elseif(NOT DEFINED ${PACKAGE_NAME}_VERSION_STRING) + set(${PACKAGE_NAME}_VERSION ${GTSAM_VERSION_STRING}) endif() # Version file 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..52ee75494 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/doc/math.lyx b/doc/math.lyx index 2533822a7..4ee89a9cc 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5082,6 +5082,394 @@ reference "ex:projection" \end_inset +\end_layout + +\begin_layout Subsection +Derivative of Adjoint +\begin_inset CommandInset label +LatexCommand label +name "subsec:pose3_adjoint_deriv" + +\end_inset + + +\end_layout + +\begin_layout Standard +Consider +\begin_inset Formula $f:SE(3)\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$ +\end_inset + + is defined as +\begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$ +\end_inset + +. + The derivative is notated (see Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Derivatives-of-Actions" +plural "false" +caps "false" +noprefix "false" + +\end_inset + +): +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +Df_{(T,\xi_{b})}(\xi,\delta\xi_{b})=D_{1}f_{(T,\xi_{b})}(\xi)+D_{2}f_{(T,\xi_{b})}(\delta\xi_{b}) +\] + +\end_inset + +First, computing +\begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$ +\end_inset + + is easy, as its matrix is simply +\begin_inset Formula $Ad_{T}$ +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +f(T,\xi_{b}+\delta\xi_{b})=Ad_{T}(\widehat{\xi_{b}+\delta\xi_{b}})=Ad_{T}(\hat{\xi}_{b})+Ad_{T}(\delta\hat{\xi}_{b}) +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T} +\] + +\end_inset + +We will derive +\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi)$ +\end_inset + + using two approaches. + In the first, we'll define +\begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$ +\end_inset + +. + From Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Derivatives-of-Actions" +plural "false" +caps "false" +noprefix "false" + +\end_inset + +, +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +D_{2}g_{(T,\xi)}(\xi) & =T\hat{\xi}\\ +D_{2}g_{(T,\xi)}^{-1}(\xi) & =-\hat{\xi}T^{-1} +\end{align*} + +\end_inset + +Now we can use the definition of the Adjoint representation +\begin_inset Formula $Ad_{g}\hat{\xi}=g\hat{\xi}g^{-1}$ +\end_inset + + (aka conjugation by +\begin_inset Formula $g$ +\end_inset + +) then apply product rule and simplify: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}_{b}g^{-1}\right)(\xi)\\ + & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}_{b}g^{-1}(T,0)+g(T,0)\hat{\xi}_{b}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\ + & =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\ + & =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\ + & =Ad_{T}(ad_{\hat{\xi}}\hat{\xi}_{b})\\ + & =-Ad_{T}(ad_{\hat{\xi}_{b}}\hat{\xi})\\ +D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}}) +\end{align*} + +\end_inset + +Where +\begin_inset Formula $ad_{\hat{\xi}}:\mathfrak{g}\rightarrow\mathfrak{g}$ +\end_inset + + is the adjoint map of the lie algebra. +\end_layout + +\begin_layout Standard +The second, perhaps more intuitive way of deriving +\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$ +\end_inset + +, would be to use the fact that the derivative at the origin +\begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$ +\end_inset + + by definition of the adjoint +\begin_inset Formula $ad_{\xi}$ +\end_inset + +. + Then applying the property +\begin_inset Formula $Ad_{AB}=Ad_{A}Ad_{B}$ +\end_inset + +, +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}}(\hat{\xi}_{b})\right)=-Ad_{T}\left(ad_{\hat{\xi}_{b}}(\hat{\xi})\right) +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Derivative of AdjointTranspose +\end_layout + +\begin_layout Standard +The transpose of the Adjoint, +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $Ad_{T}^{T}:\mathfrak{g^{*}\rightarrow g^{*}}$ +\end_inset + +, is useful as a way to change the reference frame of vectors in the dual + space +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +(note the +\begin_inset Formula $^{*}$ +\end_inset + + denoting that we are now in the dual space) +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none +. + To be more concrete, where +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +as +\begin_inset Formula $Ad_{T}\hat{\xi}_{b}$ +\end_inset + + converts the +\emph on +twist +\emph default + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\xi_{b}$ +\end_inset + + from the +\begin_inset Formula $T$ +\end_inset + + frame, +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ +\end_inset + + converts the +\family default +\series default +\shape default +\size default +\emph on +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +wrench +\emph default + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\xi_{b}^{*}$ +\end_inset + + from the +\begin_inset Formula $T$ +\end_inset + + frame +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +. + It's difficult to apply a similar derivation as in Section +\begin_inset CommandInset ref +LatexCommand ref +reference "subsec:pose3_adjoint_deriv" +plural "false" +caps "false" +noprefix "false" + +\end_inset + + for the derivative of +\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ +\end_inset + + because +\begin_inset Formula $Ad_{T}^{T}$ +\end_inset + + cannot be naturally defined as a conjugation, so we resort to crunching + through the algebra. + The details are omitted but the result is a form that vaguely resembles + (but does not exactly match) +\begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$ +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +\begin{bmatrix}\omega_{T}\\ +v_{T} +\end{bmatrix}^{*} & \triangleq Ad_{T}^{T}\hat{\xi}_{b}^{*}\\ +D_{1}Ad_{T}^{T}\hat{\xi}_{b}^{*}(\xi) & =\begin{bmatrix}\hat{\omega}_{T} & \hat{v}_{T}\\ +\hat{v}_{T} & 0 +\end{bmatrix} +\end{align*} + +\end_inset + + \end_layout \begin_layout Subsection diff --git a/doc/math.pdf b/doc/math.pdf index 8dc7270f1..40980354e 100644 Binary files a/doc/math.pdf and b/doc/math.pdf differ 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/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp index e2ca49647..cb60b2516 100644 --- a/examples/IMUKittiExampleGPS.cpp +++ b/examples/IMUKittiExampleGPS.cpp @@ -11,21 +11,23 @@ /** * @file IMUKittiExampleGPS - * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE - * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics + * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI + * VISION BENCHMARK SUITE + * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ + * Electronics */ // GTSAM related includes. +#include #include #include #include -#include -#include -#include #include #include #include -#include +#include +#include +#include #include #include @@ -34,35 +36,35 @@ using namespace std; using namespace gtsam; -using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) -using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) +using symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) struct KittiCalibration { - double body_ptx; - double body_pty; - double body_ptz; - double body_prx; - double body_pry; - double body_prz; - double accelerometer_sigma; - double gyroscope_sigma; - double integration_sigma; - double accelerometer_bias_sigma; - double gyroscope_bias_sigma; - double average_delta_t; + double body_ptx; + double body_pty; + double body_ptz; + double body_prx; + double body_pry; + double body_prz; + double accelerometer_sigma; + double gyroscope_sigma; + double integration_sigma; + double accelerometer_bias_sigma; + double gyroscope_bias_sigma; + double average_delta_t; }; struct ImuMeasurement { - double time; - double dt; - Vector3 accelerometer; - Vector3 gyroscope; // omega + double time; + double dt; + Vector3 accelerometer; + Vector3 gyroscope; // omega }; struct GpsMeasurement { - double time; - Vector3 position; // x,y,z + double time; + Vector3 position; // x,y,z }; const string output_filename = "IMUKittiExampleGPSResults.csv"; @@ -70,290 +72,313 @@ const string output_filename = "IMUKittiExampleGPSResults.csv"; void loadKittiData(KittiCalibration& kitti_calibration, vector& imu_measurements, vector& gps_measurements) { - string line; + string line; - // Read IMU metadata and compute relative sensor pose transforms - // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma - // AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT - string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); - ifstream imu_metadata(imu_metadata_file.c_str()); + // Read IMU metadata and compute relative sensor pose transforms + // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma + // GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma + // AverageDeltaT + string imu_metadata_file = + findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); + ifstream imu_metadata(imu_metadata_file.c_str()); - printf("-- Reading sensor metadata\n"); + printf("-- Reading sensor metadata\n"); - getline(imu_metadata, line, '\n'); // ignore the first line + getline(imu_metadata, line, '\n'); // ignore the first line - // Load Kitti calibration - getline(imu_metadata, line, '\n'); - sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", - &kitti_calibration.body_ptx, - &kitti_calibration.body_pty, - &kitti_calibration.body_ptz, - &kitti_calibration.body_prx, - &kitti_calibration.body_pry, - &kitti_calibration.body_prz, - &kitti_calibration.accelerometer_sigma, - &kitti_calibration.gyroscope_sigma, - &kitti_calibration.integration_sigma, - &kitti_calibration.accelerometer_bias_sigma, - &kitti_calibration.gyroscope_bias_sigma, - &kitti_calibration.average_delta_t); - printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", - kitti_calibration.body_ptx, - kitti_calibration.body_pty, - kitti_calibration.body_ptz, - kitti_calibration.body_prx, - kitti_calibration.body_pry, - kitti_calibration.body_prz, - kitti_calibration.accelerometer_sigma, - kitti_calibration.gyroscope_sigma, - kitti_calibration.integration_sigma, - kitti_calibration.accelerometer_bias_sigma, - kitti_calibration.gyroscope_bias_sigma, - kitti_calibration.average_delta_t); + // Load Kitti calibration + getline(imu_metadata, line, '\n'); + sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", + &kitti_calibration.body_ptx, &kitti_calibration.body_pty, + &kitti_calibration.body_ptz, &kitti_calibration.body_prx, + &kitti_calibration.body_pry, &kitti_calibration.body_prz, + &kitti_calibration.accelerometer_sigma, + &kitti_calibration.gyroscope_sigma, + &kitti_calibration.integration_sigma, + &kitti_calibration.accelerometer_bias_sigma, + &kitti_calibration.gyroscope_bias_sigma, + &kitti_calibration.average_delta_t); + printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", + kitti_calibration.body_ptx, kitti_calibration.body_pty, + kitti_calibration.body_ptz, kitti_calibration.body_prx, + kitti_calibration.body_pry, kitti_calibration.body_prz, + kitti_calibration.accelerometer_sigma, + kitti_calibration.gyroscope_sigma, kitti_calibration.integration_sigma, + kitti_calibration.accelerometer_bias_sigma, + kitti_calibration.gyroscope_bias_sigma, + kitti_calibration.average_delta_t); - // Read IMU data - // Time dt accelX accelY accelZ omegaX omegaY omegaZ - string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); - printf("-- Reading IMU measurements from file\n"); - { - ifstream imu_data(imu_data_file.c_str()); - getline(imu_data, line, '\n'); // ignore the first line + // Read IMU data + // Time dt accelX accelY accelZ omegaX omegaY omegaZ + string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); + printf("-- Reading IMU measurements from file\n"); + { + ifstream imu_data(imu_data_file.c_str()); + getline(imu_data, line, '\n'); // ignore the first line - double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0; - while (!imu_data.eof()) { - getline(imu_data, line, '\n'); - sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", - &time, &dt, - &acc_x, &acc_y, &acc_z, - &gyro_x, &gyro_y, &gyro_z); + double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, + gyro_y = 0, gyro_z = 0; + while (!imu_data.eof()) { + getline(imu_data, line, '\n'); + sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt, + &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z); - ImuMeasurement measurement; - measurement.time = time; - measurement.dt = dt; - measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); - measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); - imu_measurements.push_back(measurement); - } + ImuMeasurement measurement; + measurement.time = time; + measurement.dt = dt; + measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); + measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); + imu_measurements.push_back(measurement); } + } - // Read GPS data - // Time,X,Y,Z - string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); - printf("-- Reading GPS measurements from file\n"); - { - ifstream gps_data(gps_data_file.c_str()); - getline(gps_data, line, '\n'); // ignore the first line + // Read GPS data + // Time,X,Y,Z + string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); + printf("-- Reading GPS measurements from file\n"); + { + ifstream gps_data(gps_data_file.c_str()); + getline(gps_data, line, '\n'); // ignore the first line - double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; - while (!gps_data.eof()) { - getline(gps_data, line, '\n'); - sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); + double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; + while (!gps_data.eof()) { + getline(gps_data, line, '\n'); + sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); - GpsMeasurement measurement; - measurement.time = time; - measurement.position = Vector3(gps_x, gps_y, gps_z); - gps_measurements.push_back(measurement); - } + GpsMeasurement measurement; + measurement.time = time; + measurement.position = Vector3(gps_x, gps_y, gps_z); + gps_measurements.push_back(measurement); } + } } int main(int argc, char* argv[]) { - KittiCalibration kitti_calibration; - vector imu_measurements; - vector gps_measurements; - loadKittiData(kitti_calibration, imu_measurements, gps_measurements); + KittiCalibration kitti_calibration; + vector imu_measurements; + vector gps_measurements; + loadKittiData(kitti_calibration, imu_measurements, gps_measurements); - Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz, - kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz) - .finished(); - auto body_T_imu = Pose3::Expmap(BodyP); - if (!body_T_imu.equals(Pose3(), 1e-5)) { - printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same"); - exit(-1); - } + Vector6 BodyP = + (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, + kitti_calibration.body_ptz, kitti_calibration.body_prx, + kitti_calibration.body_pry, kitti_calibration.body_prz) + .finished(); + auto body_T_imu = Pose3::Expmap(BodyP); + if (!body_T_imu.equals(Pose3(), 1e-5)) { + printf( + "Currently only support IMUinBody is identity, i.e. IMU and body frame " + "are the same"); + exit(-1); + } - // Configure different variables - // double t_offset = gps_measurements[0].time; - size_t first_gps_pose = 1; - size_t gps_skip = 10; // Skip this many GPS measurements each time - double g = 9.8; - auto w_coriolis = Vector3::Zero(); // zero vector + // Configure different variables + // double t_offset = gps_measurements[0].time; + size_t first_gps_pose = 1; + size_t gps_skip = 10; // Skip this many GPS measurements each time + double g = 9.8; + auto w_coriolis = Vector3::Zero(); // zero vector - // Configure noise models - auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), - Vector3::Constant(1.0/0.07)) - .finished()); + // Configure noise models + auto noise_model_gps = noiseModel::Diagonal::Precisions( + (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07)) + .finished()); - // Set initial conditions for the estimated trajectory - // initial pose is the reference frame (navigation frame) - auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position); - // the vehicle is stationary at the beginning at position 0,0,0 - Vector3 current_velocity_global = Vector3::Zero(); - auto current_bias = imuBias::ConstantBias(); // init with zero bias + // Set initial conditions for the estimated trajectory + // initial pose is the reference frame (navigation frame) + auto current_pose_global = + Pose3(Rot3(), gps_measurements[first_gps_pose].position); + // the vehicle is stationary at the beginning at position 0,0,0 + Vector3 current_velocity_global = Vector3::Zero(); + auto current_bias = imuBias::ConstantBias(); // init with zero bias - auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), - Vector3::Constant(1.0)) - .finished()); - auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0)); - auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100), - Vector3::Constant(5.00e-05)) - .finished()); + auto sigma_init_x = noiseModel::Diagonal::Precisions( + (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished()); + auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0)); + auto sigma_init_b = noiseModel::Diagonal::Sigmas( + (Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05)) + .finished()); - // Set IMU preintegration parameters - Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2); - Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2); - // error committed in integrating position from velocities - Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2); + // Set IMU preintegration parameters + Matrix33 measured_acc_cov = + I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2); + Matrix33 measured_omega_cov = + I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2); + // error committed in integrating position from velocities + Matrix33 integration_error_cov = + I_3x3 * pow(kitti_calibration.integration_sigma, 2); - auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g); - imu_params->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous - imu_params->integrationCovariance = integration_error_cov; // integration uncertainty continuous - imu_params->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous - imu_params->omegaCoriolis = w_coriolis; + auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g); + imu_params->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + imu_params->integrationCovariance = + integration_error_cov; // integration uncertainty continuous + imu_params->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous + imu_params->omegaCoriolis = w_coriolis; - std::shared_ptr current_summarized_measurement = nullptr; + std::shared_ptr current_summarized_measurement = + nullptr; - // Set ISAM2 parameters and create ISAM2 solver object - ISAM2Params isam_params; - isam_params.factorization = ISAM2Params::CHOLESKY; - isam_params.relinearizeSkip = 10; + // Set ISAM2 parameters and create ISAM2 solver object + ISAM2Params isam_params; + isam_params.factorization = ISAM2Params::CHOLESKY; + isam_params.relinearizeSkip = 10; - ISAM2 isam(isam_params); + ISAM2 isam(isam_params); - // Create the factor graph and values object that will store new factors and values to add to the incremental graph - NonlinearFactorGraph new_factors; - Values new_values; // values storing the initial estimates of new nodes in the factor graph + // Create the factor graph and values object that will store new factors and + // values to add to the incremental graph + NonlinearFactorGraph new_factors; + Values new_values; // values storing the initial estimates of new nodes in + // the factor graph - /// Main loop: - /// (1) we read the measurements - /// (2) we create the corresponding factors in the graph - /// (3) we solve the graph to obtain and optimal estimate of robot trajectory - printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n"); - size_t j = 0; - for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { - // At each non=IMU measurement we initialize a new node in the graph - auto current_pose_key = X(i); - auto current_vel_key = V(i); - auto current_bias_key = B(i); - double t = gps_measurements[i].time; + /// Main loop: + /// (1) we read the measurements + /// (2) we create the corresponding factors in the graph + /// (3) we solve the graph to obtain and optimal estimate of robot trajectory + printf( + "-- Starting main loop: inference is performed at each time step, but we " + "plot trajectory every 10 steps\n"); + size_t j = 0; + size_t included_imu_measurement_count = 0; - if (i == first_gps_pose) { - // Create initial estimate and prior on initial pose, velocity, and biases - new_values.insert(current_pose_key, current_pose_global); - new_values.insert(current_vel_key, current_velocity_global); - new_values.insert(current_bias_key, current_bias); - new_factors.emplace_shared>(current_pose_key, current_pose_global, sigma_init_x); - new_factors.emplace_shared>(current_vel_key, current_velocity_global, sigma_init_v); - new_factors.emplace_shared>(current_bias_key, current_bias, sigma_init_b); - } else { - double t_previous = gps_measurements[i-1].time; + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { + // At each non=IMU measurement we initialize a new node in the graph + auto current_pose_key = X(i); + auto current_vel_key = V(i); + auto current_bias_key = B(i); + double t = gps_measurements[i].time; - // Summarize IMU data between the previous GPS measurement and now - current_summarized_measurement = std::make_shared(imu_params, current_bias); - static size_t included_imu_measurement_count = 0; - while (j < imu_measurements.size() && imu_measurements[j].time <= t) { - if (imu_measurements[j].time >= t_previous) { - current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer, - imu_measurements[j].gyroscope, - imu_measurements[j].dt); - included_imu_measurement_count++; - } - j++; - } + if (i == first_gps_pose) { + // Create initial estimate and prior on initial pose, velocity, and biases + new_values.insert(current_pose_key, current_pose_global); + new_values.insert(current_vel_key, current_velocity_global); + new_values.insert(current_bias_key, current_bias); + new_factors.emplace_shared>( + current_pose_key, current_pose_global, sigma_init_x); + new_factors.emplace_shared>( + current_vel_key, current_velocity_global, sigma_init_v); + new_factors.emplace_shared>( + current_bias_key, current_bias, sigma_init_b); + } else { + double t_previous = gps_measurements[i - 1].time; - // Create IMU factor - auto previous_pose_key = X(i-1); - auto previous_vel_key = V(i-1); - auto previous_bias_key = B(i-1); + // Summarize IMU data between the previous GPS measurement and now + current_summarized_measurement = + std::make_shared(imu_params, + current_bias); - new_factors.emplace_shared(previous_pose_key, previous_vel_key, - current_pose_key, current_vel_key, - previous_bias_key, *current_summarized_measurement); - - // Bias evolution as given in the IMU metadata - auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() << - Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma), - Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma)) - .finished()); - new_factors.emplace_shared>(previous_bias_key, - current_bias_key, - imuBias::ConstantBias(), - sigma_between_b); - - // Create GPS factor - auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position); - if ((i % gps_skip) == 0) { - new_factors.emplace_shared>(current_pose_key, gps_pose, noise_model_gps); - new_values.insert(current_pose_key, gps_pose); - - printf("################ POSE INCLUDED AT TIME %lf ################\n", t); - cout << gps_pose.translation(); - printf("\n\n"); - } else { - new_values.insert(current_pose_key, current_pose_global); - } - - // Add initial values for velocity and bias based on the previous estimates - new_values.insert(current_vel_key, current_velocity_global); - new_values.insert(current_bias_key, current_bias); - - // Update solver - // ======================================================================= - // We accumulate 2*GPSskip GPS measurements before updating the solver at - // first so that the heading becomes observable. - if (i > (first_gps_pose + 2*gps_skip)) { - printf("################ NEW FACTORS AT TIME %lf ################\n", t); - new_factors.print(); - - isam.update(new_factors, new_values); - - // Reset the newFactors and newValues list - new_factors.resize(0); - new_values.clear(); - - // Extract the result/current estimates - Values result = isam.calculateEstimate(); - - current_pose_global = result.at(current_pose_key); - current_velocity_global = result.at(current_vel_key); - current_bias = result.at(current_bias_key); - - printf("\n################ POSE AT TIME %lf ################\n", t); - current_pose_global.print(); - printf("\n\n"); - } + while (j < imu_measurements.size() && imu_measurements[j].time <= t) { + if (imu_measurements[j].time >= t_previous) { + current_summarized_measurement->integrateMeasurement( + imu_measurements[j].accelerometer, imu_measurements[j].gyroscope, + imu_measurements[j].dt); + included_imu_measurement_count++; } + j++; + } + + // Create IMU factor + auto previous_pose_key = X(i - 1); + auto previous_vel_key = V(i - 1); + auto previous_bias_key = B(i - 1); + + new_factors.emplace_shared( + previous_pose_key, previous_vel_key, current_pose_key, + current_vel_key, previous_bias_key, *current_summarized_measurement); + + // Bias evolution as given in the IMU metadata + auto sigma_between_b = noiseModel::Diagonal::Sigmas( + (Vector6() << Vector3::Constant( + sqrt(included_imu_measurement_count) * + kitti_calibration.accelerometer_bias_sigma), + Vector3::Constant(sqrt(included_imu_measurement_count) * + kitti_calibration.gyroscope_bias_sigma)) + .finished()); + new_factors.emplace_shared>( + previous_bias_key, current_bias_key, imuBias::ConstantBias(), + sigma_between_b); + + // Create GPS factor + auto gps_pose = + Pose3(current_pose_global.rotation(), gps_measurements[i].position); + if ((i % gps_skip) == 0) { + new_factors.emplace_shared>( + current_pose_key, gps_pose, noise_model_gps); + new_values.insert(current_pose_key, gps_pose); + + printf("############ POSE INCLUDED AT TIME %.6lf ############\n", + t); + cout << gps_pose.translation(); + printf("\n\n"); + } else { + new_values.insert(current_pose_key, current_pose_global); + } + + // Add initial values for velocity and bias based on the previous + // estimates + new_values.insert(current_vel_key, current_velocity_global); + new_values.insert(current_bias_key, current_bias); + + // Update solver + // ======================================================================= + // We accumulate 2*GPSskip GPS measurements before updating the solver at + // first so that the heading becomes observable. + if (i > (first_gps_pose + 2 * gps_skip)) { + printf("############ NEW FACTORS AT TIME %.6lf ############\n", + t); + new_factors.print(); + + isam.update(new_factors, new_values); + + // Reset the newFactors and newValues list + new_factors.resize(0); + new_values.clear(); + + // Extract the result/current estimates + Values result = isam.calculateEstimate(); + + current_pose_global = result.at(current_pose_key); + current_velocity_global = result.at(current_vel_key); + current_bias = result.at(current_bias_key); + + printf("\n############ POSE AT TIME %lf ############\n", t); + current_pose_global.print(); + printf("\n\n"); + } } + } - // Save results to file - printf("\nWriting results to file...\n"); - FILE* fp_out = fopen(output_filename.c_str(), "w+"); - fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n"); + // Save results to file + printf("\nWriting results to file...\n"); + FILE* fp_out = fopen(output_filename.c_str(), "w+"); + fprintf(fp_out, + "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n"); - Values result = isam.calculateEstimate(); - for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { - auto pose_key = X(i); - auto vel_key = V(i); - auto bias_key = B(i); + Values result = isam.calculateEstimate(); + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { + auto pose_key = X(i); + auto vel_key = V(i); + auto bias_key = B(i); - auto pose = result.at(pose_key); - auto velocity = result.at(vel_key); - auto bias = result.at(bias_key); + auto pose = result.at(pose_key); + auto velocity = result.at(vel_key); + auto bias = result.at(bias_key); - auto pose_quat = pose.rotation().toQuaternion(); - auto gps = gps_measurements[i].position; + auto pose_quat = pose.rotation().toQuaternion(); + auto gps = gps_measurements[i].position; - cout << "State at #" << i << endl; - cout << "Pose:" << endl << pose << endl; - cout << "Velocity:" << endl << velocity << endl; - cout << "Bias:" << endl << bias << endl; + cout << "State at #" << i << endl; + cout << "Pose:" << endl << pose << endl; + cout << "Velocity:" << endl << velocity << endl; + cout << "Bias:" << endl << bias << endl; - fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", - gps_measurements[i].time, - pose.x(), pose.y(), pose.z(), - pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), - gps(0), gps(1), gps(2)); - } + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + gps_measurements[i].time, pose.x(), pose.y(), pose.z(), + pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0), + gps(1), gps(2)); + } - fclose(fp_out); + fclose(fp_out); } 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 34422edd7..ac7c2a9a5 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -320,28 +320,28 @@ T expm(const Vector& x, int K=7) { } /** - * 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), with optional jacobians. + * 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, typename MakeOptionalJacobian::type Hx = boost::none, typename MakeOptionalJacobian::type Hy = boost::none) { - if (Hx || Hy) { - typename traits::TangentVector log_Xinv_Y; 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 - T Xinv_Y = traits::Between(X, Y, between_H_x); // between_H_y = identity - log_Xinv_Y = traits::Logmap(Xinv_Y, log_H); - Xinv_Y = traits::Expmap(t * log_Xinv_Y, exp_H); - Xinv_Y = traits::Compose(X, Xinv_Y, 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 Xinv_Y; + 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)))); + 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/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/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 7a88f72eb..30cec3b9a 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - tbb::task::spawn_root_and_wait( - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold)); + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 87d5b0d4c..dc1b45906 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task, tbb::task_list +#include // tbb::task_group #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask : public tbb::task + class PreOrderTask { public: const boost::shared_ptr& treeNode; @@ -42,28 +42,30 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; bool makeNewTasks; - bool isPostOrderPhase; + // Keep track of order phase across multiple calls to the same functor + mutable bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - bool makeNewTasks = true) + tbb::task_group& tg, bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), + tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - tbb::task* execute() override + void operator()() const { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return nullptr; } else { @@ -71,14 +73,10 @@ namespace gtsam { { if(!treeNode->children.empty()) { - // Allocate post-order task as a continuation - isPostOrderPhase = true; - recycle_as_continuation(); - bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - tbb::task* firstChild = 0; - tbb::task_list childTasks; + // If we have child tasks, start subtasks and wait for them to complete + tbb::task_group ctg; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -86,37 +84,30 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - tbb::task* childTask = - new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, overThreshold); - if (firstChild) - childTasks.push_back(*childTask); - else - firstChild = childTask; + ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, ctg, overThreshold)); } + ctg.wait(); - // If we have child tasks, start subtasks and wait for them to complete - set_ref_count((int)treeNode->children.size()); - spawn(childTasks); - return firstChild; + // Allocate post-order task as a continuation + isPostOrderPhase = true; + tg.run(*this); } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const { for(const boost::shared_ptr& child: node->children) { @@ -131,7 +122,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask : public tbb::task + class RootTask { public: const ROOTS& roots; @@ -139,38 +130,31 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold) : + int problemSizeThreshold, tbb::task_group& tg) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold) {} + problemSizeThreshold(problemSizeThreshold), tg(tg) {} - tbb::task* execute() override + void operator()() const { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children - tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tasks.push_back(*new(allocate_child()) - PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); + tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); } - // Set TBB ref count - set_ref_count(1 + (int) roots.size()); - // Spawn tasks - spawn_and_wait_for_all(tasks); - // Return nullptr - return nullptr; } }; template - RootTask& - CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); - } + tbb::task_group tg; + tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + } } 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 c09eba9bb..143d4bc3c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -147,113 +147,123 @@ public: * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * Fixed size version - */ - template // N = 2 or 3 (point dimension), 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)); - } - } // end of for over cameras - - augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); - return augmentedHessian; - } + 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. + * 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 // 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::aligned_allocator > >& Fs, + 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) { - + 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); + // Marginalize point: note - we reuse the standard SchurComplement function. + SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs, E, P, b); - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term + // 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; - // here we have to deal with the fact that some blocks may share the same keys - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + // 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 { // 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 + } 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) + // 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 + // 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) + // 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 + // Update information vector. augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i], nrUniqueKeys, augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - // update blocks + // Update blocks. for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols Key key_j = jacobianKeys.at(j); if (i == j) { @@ -267,45 +277,20 @@ public: } else { augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); } } } } - // update bottom right element of the matrix + + // Update bottom right element of the matrix. augmentedHessianUniqueKeys.updateDiagonalBlock( nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); } return augmentedHessianUniqueKeys; } - /** - * non-templated version of function above - */ - static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6( - const std::vector, - Eigen::aligned_allocator > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const KeyVector jacobianKeys, const KeyVector hessianKeys) { - return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b, - jacobianKeys, - hessianKeys); - } - - /** - * non-templated version of function above - */ - static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_6_6( - const std::vector, - Eigen::aligned_allocator > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const KeyVector jacobianKeys, const KeyVector hessianKeys) { - return SchurComplementAndRearrangeBlocks<3,6,6>(Fs, E, P, b, - jacobianKeys, - hessianKeys); - } - /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F 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/Pose3.cpp b/gtsam/geometry/Pose3.cpp index c183e32ed..d0e60f3fd 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -63,6 +63,46 @@ Matrix6 Pose3::AdjointMap() const { return adj; } +/* ************************************************************************* */ +// Calculate AdjointMap applied to xi_b, with Jacobians +Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose, + OptionalJacobian<6, 6> H_xib) const { + const Matrix6 Ad = AdjointMap(); + + // Jacobians + // D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b + // D2 Ad_T(xi_b) = Ad_T + // See docs/math.pdf for more details. + // In D1 calculation, we could be more efficient by writing it out, but do not + // for readability + if (H_pose) *H_pose = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; + + return Ad * xi_b; +} + +/* ************************************************************************* */ +/// The dual version of Adjoint +Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, + OptionalJacobian<6, 6> H_x) const { + const Matrix6 Ad = AdjointMap(); + const Vector6 AdTx = Ad.transpose() * x; + + // Jacobians + // See docs/math.pdf for more details. + if (H_pose) { + const auto w_T_hat = skewSymmetric(AdTx.head<3>()), + v_T_hat = skewSymmetric(AdTx.tail<3>()); + *H_pose << w_T_hat, v_T_hat, // + /* */ v_T_hat, Z_3x3; + } + if (H_x) { + *H_x = Ad.transpose(); + } + + return AdTx; +} + /* ************************************************************************* */ Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d76e1b41a..b6107120e 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -145,15 +145,22 @@ public: * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ - Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect + Matrix6 AdjointMap() const; /** - * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame + * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a + * body-fixed velocity, transforming it to the spatial frame * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + * Note that H_xib = AdjointMap() */ - Vector6 Adjoint(const Vector6& xi_b) const { - return AdjointMap() * xi_b; - } /// FIXME Not tested - marked as incorrect + Vector6 Adjoint(const Vector6& xi_b, + OptionalJacobian<6, 6> H_this = boost::none, + OptionalJacobian<6, 6> H_xib = boost::none) const; + + /// The dual version of Adjoint + Vector6 AdjointTranspose(const Vector6& x, + OptionalJacobian<6, 6> H_this = boost::none, + OptionalJacobian<6, 6> H_x = boost::none) const; /** * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 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/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index 01b749df4..15d5128bc 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -31,6 +31,7 @@ namespace gtsam { class GTSAM_EXPORT EmptyCal { public: + enum { dimension = 0 }; EmptyCal(){} virtual ~EmptyCal() = default; using shared_ptr = boost::shared_ptr; 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 e583c0e80..144f28314 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -185,9 +185,8 @@ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { // Actual SymmetricBlockMatrix augmentedHessianBM = - Set::SchurComplementAndRearrangeBlocks_3_12_6(Fs, E, P, b, - nonuniqueKeys, - uniqueKeys); + Set::SchurComplementAndRearrangeBlocks<3, 12, 6>( + Fs, E, P, b, nonuniqueKeys, uniqueKeys); Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); // Expected diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 7c1fa81e6..f0f2c0ccd 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -145,6 +145,81 @@ TEST(Pose3, Adjoint_full) EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } +/* ************************************************************************* */ +// Check Adjoint numerical derivatives +TEST(Pose3, Adjoint_jacobians) +{ + Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + // Check evaluation sanity check + EQUALITY(static_cast(T.AdjointMap() * xi), T.Adjoint(xi)); + EQUALITY(static_cast(T2.AdjointMap() * xi), T2.Adjoint(xi)); + EQUALITY(static_cast(T3.AdjointMap() * xi), T3.Adjoint(xi)); + + // Check jacobians + Matrix6 actualH1, actualH2, expectedH1, expectedH2; + std::function Adjoint_proxy = + [&](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); }; + + T.Adjoint(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi); + expectedH2 = numericalDerivative22(Adjoint_proxy, T, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T2.Adjoint(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(Adjoint_proxy, T2, xi); + expectedH2 = numericalDerivative22(Adjoint_proxy, T2, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T3.Adjoint(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(Adjoint_proxy, T3, xi); + expectedH2 = numericalDerivative22(Adjoint_proxy, T3, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + +/* ************************************************************************* */ +// Check AdjointTranspose and jacobians +TEST(Pose3, AdjointTranspose) +{ + Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + // Check evaluation + EQUALITY(static_cast(T.AdjointMap().transpose() * xi), + T.AdjointTranspose(xi)); + EQUALITY(static_cast(T2.AdjointMap().transpose() * xi), + T2.AdjointTranspose(xi)); + EQUALITY(static_cast(T3.AdjointMap().transpose() * xi), + T3.AdjointTranspose(xi)); + + // Check jacobians + Matrix6 actualH1, actualH2, expectedH1, expectedH2; + std::function AdjointTranspose_proxy = + [&](const Pose3& T, const Vector6& xi) { + return T.AdjointTranspose(xi); + }; + + T.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T2.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T3.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) TEST(Pose3, Adjoint_hat) 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/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/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 13eaee7e3..24c4b9a0d 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -510,4 +510,33 @@ namespace gtsam { return optimize(function); } + /* ************************************************************************* */ + void GaussianFactorGraph::printErrors( + const VectorValues& values, const std::string& str, + const KeyFormatter& keyFormatter, + const std::function& + printCondition) const { + cout << str << "size: " << size() << endl << endl; + for (size_t i = 0; i < (*this).size(); i++) { + const sharedFactor& factor = (*this)[i]; + const double errorValue = + (factor != nullptr ? (*this)[i]->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + stringstream ss; + ss << "Factor " << i << ": "; + if (factor == nullptr) { + cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + cout << "error = " << errorValue << "\n"; + } + cout << endl; // only one "endl" at end might be faster, \n for each + // factor + } + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index e3304d5e8..d41374854 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -375,6 +375,14 @@ namespace gtsam { /** In-place version e <- A*x that takes an iterator. */ void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; + void printErrors( + const VectorValues& x, + const std::string& str = "GaussianFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = + [](const Factor*, double, size_t) { return true; }) const; /// @} private: diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 63047bf4f..c74161f26 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; @@ -221,6 +221,7 @@ class VectorValues { //Constructors VectorValues(); VectorValues(const gtsam::VectorValues& other); + VectorValues(const gtsam::VectorValues& first, const gtsam::VectorValues& second); //Named Constructors static gtsam::VectorValues Zero(const gtsam::VectorValues& model); @@ -289,6 +290,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 = @@ -393,6 +401,7 @@ class GaussianFactorGraph { // error and probability double error(const gtsam::VectorValues& c) const; double probPrime(const gtsam::VectorValues& c) const; + void printErrors(const gtsam::VectorValues& c, string str = "GaussianFactorGraph: ", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; gtsam::GaussianFactorGraph clone() const; gtsam::GaussianFactorGraph negate() const; diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 48a5a35de..7a879c3ef 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -88,6 +88,8 @@ virtual class PreintegratedRotationParams { virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { PreintegrationParams(Vector n_gravity); + gtsam::Vector n_gravity; + static gtsam::PreintegrationParams* MakeSharedD(double g); static gtsam::PreintegrationParams* MakeSharedU(double g); static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 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/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index cf2462dfc..b2ef6f055 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -246,6 +246,18 @@ struct apply_compose { return x.compose(y, H1, H2); } }; + +template <> +struct apply_compose { + double operator()(const double& x, const double& y, + OptionalJacobian<1, 1> H1 = boost::none, + OptionalJacobian<1, 1> H2 = boost::none) const { + if (H1) H1->setConstant(y); + if (H2) H2->setConstant(x); + return x * y; + } +}; + } // Global methods: 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/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 61f164b43..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 { @@ -527,7 +530,14 @@ 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; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 80262ae3f..92f4998a2 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -293,6 +293,19 @@ TEST(Expression, compose3) { EXPECT(expected == R3.keys()); } +/* ************************************************************************* */ +// Test compose with double type (should be multiplication). +TEST(Expression, compose4) { + // Create expression + gtsam::Key key = 1; + Double_ R1(key), R2(key); + Double_ R3 = R1 * R2; + + // Check keys + set expected = list_of(1); + EXPECT(expected == R3.keys()); +} + /* ************************************************************************* */ // Test with ternary function. Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, 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/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/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 4cfe87513..72b49c9ac 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -306,7 +306,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { // 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::SchurComplementAndRearrangeBlocks_3_6_6( + Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>( Fs, E, P, b, nonUniqueKeys_, this->keys_); return boost::make_shared < RegularHessianFactor 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/lago.cpp b/gtsam/slam/lago.cpp index 70caa424f..f8b092f86 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -36,7 +36,7 @@ static const Matrix I = I_1x1; static const Matrix I3 = I_3x3; static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = - noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished()); + noiseModel::Diagonal::Sigmas(Vector1(0)); static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 1c04fd14c..60000dbab 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -334,5 +334,11 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { Vector evaluateError(const T& R1, const T& R2); }; - + +#include +namespace lago { + gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); + gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); +} + } // 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_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 13c061b9b..98a1b4ef9 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -100,12 +100,12 @@ endif() install( TARGETS gtsam_unstable - EXPORT GTSAM-exports + EXPORT GTSAM_UNSTABLE-exports LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) -list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable) -set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) +list(APPEND GTSAM_UNSTABLE_EXPORTED_TARGETS gtsam_unstable) +set(GTSAM_UNSTABLE_EXPORTED_TARGETS "${GTSAM_UNSTABLE_EXPORTED_TARGETS}" PARENT_SCOPE) # Build examples add_subdirectory(examples) 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 index 5fc1c05eb..c92a13daf 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -23,7 +23,6 @@ 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; @@ -32,12 +31,10 @@ Vector ProjectionFactorRollingShutter::evaluateError( 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); + 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_); @@ -45,29 +42,23 @@ Vector ProjectionFactorRollingShutter::evaluateError( } } 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); + 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 (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()); + << 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 +} // namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 5827a538c..c92653c13 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -17,41 +17,47 @@ #pragma once -#include -#include -#include #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. + * 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 { +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 + 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 + 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) + 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; @@ -66,72 +72,72 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, - boost::optional body_P_sensor = - boost::none) + 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) { - } + verboseCheirality_(false) {} /** * Constructor with exception-handling flags - * @param measured is the 2-dimensional pixel location of point in the image (the measurement) + * @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) + * @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) + 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) { - } + verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~ProjectionFactorRollingShutter() { - } + virtual ~ProjectionFactorRollingShutter() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast < gtsam::NonlinearFactor - > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -139,8 +145,9 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3>& world_P_body_key_pairs, const std::vector& alphas, const std::vector>& Ks, - const std::vector body_P_sensors) { + 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()); @@ -156,20 +172,24 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor>& world_P_body_key_pairs, const std::vector& alphas, - const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { + 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++) { @@ -179,39 +199,37 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor> calibration() const { + 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 (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_; - } + 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_; - } + 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 { + 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; + << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; std::cout << " pose2 key: " - << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; + << 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 = "); @@ -222,32 +240,40 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor* e = - dynamic_cast*>(&p); + 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++){ + 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; + if (!(key1own == key1e) || !(key2own == key2e)) { + keyPairsEqual = false; + break; } } - }else{ keyPairsEqual = false; } + } 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; + 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; } + } else { + extrinsicCalibrationEqual = false; + } - return e && Base::equals(p, tol) && K_all_ == e->calibration() - && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) && K_all_ == e->calibration() && + alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -291,9 +317,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactormeasured_.size(); - E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian) + 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; + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; Eigen::Matrix Ei; for (size_t i = 0; i < numViews; i++) { // for each camera/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); + 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: - const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); - const Pose3& body_P_cam = body_P_sensors_[i]; - const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); + 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); typename Base::Camera camera(w_P_cam, K_all_[i]); // get jacobians and error vector for current measurement Point2 reprojectionError_i = camera.reprojectionError( *this->result_, this->measured_.at(i), dProject_dPoseCam, Ei); 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) + 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); @@ -338,37 +370,40 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor > 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 + 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 < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector < Vector > gs(nrUniqueKeys); + 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"); + 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 < RegularHessianFactor - > (this->keys_, Gs, gs, 0.0); + 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"); + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); } } // compute Jacobian given triangulated 3D Point @@ -384,21 +419,23 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor( Fs, E, P, b, nonuniqueKeys, this->keys_); - return boost::make_shared < RegularHessianFactor - > (this->keys_, augmentedHessianUniqueKeys); + return boost::make_shared>( + this->keys_, augmentedHessianUniqueKeys); } /** @@ -408,38 +445,38 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor linearizeDamped( const Values& values, const double lambda = 0.0) const { - // depending on flag set on construction we may linearize to different linear factors + // 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"); + "SmartProjectionPoseFactorRollingShutter: unknown linearization " + "mode"); } } /// linearize - boost::shared_ptr linearize(const Values& values) const - override { + boost::shared_ptr linearize( + const Values& values) const override { return this->linearizeDamped(values); } private: /// Serialization function friend class boost::serialization::access; - template + template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(K_all_); } - }; // end of class declaration /// traits -template -struct traits > : -public Testable > { -}; +template +struct traits> + : public Testable> {}; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index 943e350d4..161c9aa55 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -16,34 +16,33 @@ * @date July 2021 */ -#include +#include #include -#include -#include +#include #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)); +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::X; 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)); @@ -51,74 +50,84 @@ 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)); +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) { +TEST(ProjectionFactorRollingShutter, Constructor) { ProjectionFactorRollingShutter factor(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K); } /* ************************************************************************* */ -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); +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 (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 + { // 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 +TEST(ProjectionFactorRollingShutter, EqualsWithTransform) { + { // factors are equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); CHECK(assert_equal(factor1, factor2)); } - { // factors are NOT equal + { // 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 + 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); + poseKey1, poseKey2, pointKey, K, + body_P_sensor2); CHECK(!assert_equal(factor1, factor2)); } } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Error ) { +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); + 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)); + 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 @@ -134,11 +143,12 @@ TEST( ProjectionFactorRollingShutter, Error ) { // 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); + 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)); + 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 @@ -153,15 +163,16 @@ TEST( ProjectionFactorRollingShutter, Error ) { { // 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 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 + 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); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the error Vector actualError(factor.evaluateError(pose1, pose2, point)); @@ -175,19 +186,20 @@ TEST( ProjectionFactorRollingShutter, Error ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { +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 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 + 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); + 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)); @@ -200,18 +212,19 @@ TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Jacobian ) { +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 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 + 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); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -222,22 +235,25 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { 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); + 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); + 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); + 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)); @@ -245,19 +261,20 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { +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 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 + 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); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -268,22 +285,25 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { 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); + 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); + 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); + 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)); @@ -291,41 +311,48 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, cheirality ) { +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 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 + 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 + 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); + 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); + { // 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)); + 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 + 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)); + 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 { @@ -333,7 +360,8 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -344,22 +372,25 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { 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); + 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); + 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); + 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)); @@ -368,8 +399,9 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { #endif } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index adf49e8cb..31a301ee6 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -19,7 +19,7 @@ #include "gtsam/slam/tests/smartFactorScenarios.h" #include #include -#include + #include #include #include @@ -40,8 +40,8 @@ static const double sigma = 0.1; static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -49,8 +49,8 @@ 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 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); @@ -65,52 +65,39 @@ static double interp_factor3 = 0.5; 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); +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); -} - -/* ************************************************************************* */ -// default Cal3_S2 poses with rolling shutter effect -namespace sphericalCameraRS { -typedef SphericalCamera Camera; -typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS_spherical; -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); -static EmptyCal::shared_ptr emptyK; -Camera cam1(interp_pose1, emptyK); -Camera cam2(interp_pose2, emptyK); -Camera cam3(interp_pose3, emptyK); -} +} // namespace vanillaPoseRS LevenbergMarquardtParams lmParams; -typedef SmartProjectionPoseFactorRollingShutter< PinholePose > SmartFactorRS; +typedef SmartProjectionPoseFactorRollingShutter> + SmartFactorRS; /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { SmartProjectionParams params; params.setRankTolerance(rankTol); SmartFactorRS factor1(model, params); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, add) { +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 ) { +TEST(SmartProjectionPoseFactorRollingShutter, Equals) { using namespace vanillaPose; // create fake measurements @@ -119,10 +106,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { 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> 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); @@ -141,57 +128,67 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { // 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); + 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 + { // 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(assert_equal(*factor1, *factor2)); - EXPECT(assert_equal(*factor1, *factor3)); + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); } - { // create slightly different factors (different keys) and show equal returns false + { // 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(measurement2, x2, x2, interp_factor2, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different extrinsics) and show equal returns false + { // 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(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(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different interp factors) and show equal returns false + { // 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(measurement2, x2, x3, interp_factor1, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + 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 +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 ) { +TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { using namespace vanillaPoseRS; // Project two landmarks into two cameras @@ -203,7 +200,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { 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 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); @@ -215,41 +212,56 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { // 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 + 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); + 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); + 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)); + 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); + 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)); + 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 ) { +TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { // also includes non-identical extrinsic calibration using namespace vanillaPoseRS; @@ -261,9 +273,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { 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); + 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 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); @@ -271,7 +284,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { // Perform triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid + EXPECT(point.valid()); // check triangulated point is valid Point3 landmarkNoisy = *point; // Check Jacobians @@ -279,32 +292,48 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { 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); + 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); + 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)); + 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); + 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)); + 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 + double actualError = factor.error(values); // from smart factor NonlinearFactorGraph nfg; nfg.add(factor1); nfg.add(factor2); @@ -314,8 +343,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { - +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -325,10 +353,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { 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> 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); @@ -359,20 +387,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { 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/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 + 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 + // 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))); + 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); @@ -381,11 +411,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { } /* *************************************************************************/ -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 +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 @@ -438,7 +469,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { perturbedDelta.insert(x2, delta); double expectedError = 2500; - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + // 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; @@ -464,8 +496,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { Values values; values.insert(x1, pose1); values.insert(x2, pose2); - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values); + 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); @@ -473,7 +505,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -493,7 +525,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - double excludeLandmarksFutherThanDist = 1e10; //very large + double excludeLandmarksFutherThanDist = 1e10; // very large SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); @@ -501,13 +533,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -524,7 +556,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { 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 + // 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 @@ -535,7 +568,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_landmarkDistance) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -563,13 +597,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -586,10 +620,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista 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 + // 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 + // 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(); @@ -597,7 +633,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_dynamicOutlierRejection) { using namespace vanillaPoseRS; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -609,7 +646,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie 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 + measurements_lmk4.at(0) = + measurements_lmk4.at(0) + Point2(10, 10); // add outlier // create inputs std::vector> key_pairs; @@ -623,7 +661,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie interp_factors.push_back(interp_factor3); double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = + 3; // max 3 pixel of average reprojection error SmartProjectionParams params; params.setRankTolerance(1.0); @@ -655,12 +694,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie 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 + 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 + // 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 @@ -671,8 +713,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) { - +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1; @@ -698,10 +740,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // 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))); + 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; @@ -710,8 +757,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -729,46 +776,52 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli 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); + // 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); + 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); + 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; + 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); + 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 + 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 + // -- 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)); @@ -786,9 +839,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -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 +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; @@ -798,7 +853,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // create redundant measurements: Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement // create inputs std::vector> key_pairs; @@ -814,17 +870,23 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli interp_factors.push_back(interp_factor1); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK); + 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 + // 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))); + 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; @@ -833,8 +895,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -843,62 +905,74 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli TriangulationResult point = smartFactor1->point(); EXPECT(point.valid()); // check triangulated point is valid - // Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians + // 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); + 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); + // 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); + 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); + 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); + 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; + 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); + 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 + 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 + // -- 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)); @@ -917,8 +991,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) { - +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_measurementsFromSamePose) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -928,27 +1002,32 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF 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> 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) + // 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 + 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 + 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); + 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); @@ -971,20 +1050,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF 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/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 + 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 + // 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))); + 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); @@ -995,11 +1076,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF #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) +//| -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 ) { - +TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; // Default cameras for simple derivatives @@ -1022,14 +1103,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { size_t nrTests = 1000; - for(size_t i = 0; iadd(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); + 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); + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); Values values; values.insert(x1, pose1); @@ -1039,7 +1120,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { gttoc_(SF_RS_LINEARIZE); } - for(size_t i = 0; iadd(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1055,6 +1136,21 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { } #endif +#include +/* ************************************************************************* */ +// spherical Camera with rolling shutter effect +namespace sphericalCameraRS { +typedef SphericalCamera Camera; +typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS_spherical; +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); +static EmptyCal::shared_ptr emptyK; +Camera cam1(interp_pose1, emptyK); +Camera cam2(interp_pose2, emptyK); +Camera cam3(interp_pose3, emptyK); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCameras ) { @@ -1084,48 +1180,48 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCame new SmartFactorRS_spherical(model,params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, emptyK); - SmartFactorRS_spherical::shared_ptr smartFactor2( - new SmartFactorRS_spherical(model,params)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, emptyK); - - SmartFactorRS_spherical::shared_ptr smartFactor3( - new SmartFactorRS_spherical(model,params)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, emptyK); - - 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)); +// SmartFactorRS_spherical::shared_ptr smartFactor2( +// new SmartFactorRS_spherical(model,params)); +// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, emptyK); +// +// SmartFactorRS_spherical::shared_ptr smartFactor3( +// new SmartFactorRS_spherical(model,params)); +// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, emptyK); +// +// 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)); } /* ************************************************************************* */ @@ -1134,4 +1230,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bdda15acb..b703f5900 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" @@ -88,7 +92,7 @@ set_target_properties(gtsam_py PROPERTIES set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) # Symlink all tests .py files to build folder. -create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" +copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" "${GTSAM_MODULE_PATH}") # Common directory for data/datasets stored with the package. @@ -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" @@ -144,11 +148,11 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable) # Symlink all tests .py files to build folder. - create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" + copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" "${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,5 +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 -s "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests" - DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}) + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} + 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/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index c7fe1e202..36c1e003d 100644 --- a/python/gtsam/examples/CustomFactorExample.py +++ b/python/gtsam/examples/CustomFactorExample.py @@ -9,15 +9,17 @@ CustomFactor demo that simulates a 1-D sensor fusion task. Author: Fan Jiang, Frank Dellaert """ +from functools import partial +from typing import List, Optional + import gtsam import numpy as np -from typing import List, Optional -from functools import partial +I = np.eye(1) -def simulate_car(): - # Simulate a car for one second +def simulate_car() -> List[float]: + """Simulate a car for one second""" x0 = 0 dt = 0.25 # 4 Hz, typical GPS v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast @@ -26,46 +28,9 @@ def simulate_car(): return x -x = simulate_car() -print(f"Simulated car trajectory: {x}") - -# %% -add_noise = True # set this to False to run with "perfect" measurements - -# GPS measurements -sigma_gps = 3.0 # assume GPS is +/- 3m -g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0) - for k in range(5)] - -# Odometry measurements -sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz -o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0) - for k in range(4)] - -# Landmark measurements: -sigma_lm = 1 # assume landmark measurement is accurate up to 1m - -# Assume first landmark is at x=5, we measure it at time k=0 -lm_0 = 5.0 -z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0) - -# Assume other landmark is at x=28, we measure it at time k=3 -lm_3 = 28.0 -z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0) - -unknown = [gtsam.symbol('x', k) for k in range(5)] - -print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown))) - -# We now can use nonlinear factor graphs -factor_graph = gtsam.NonlinearFactorGraph() - -# Add factors for GPS measurements -I = np.eye(1) -gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) - - -def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): +def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, + values: gtsam.Values, + jacobians: Optional[List[np.ndarray]]) -> float: """GPS Factor error function :param measurement: GPS measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -82,36 +47,9 @@ def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobia return error -# Add the GPS factors -for k in range(5): - gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]]))) - factor_graph.add(gf) - -# New Values container -v = gtsam.Values() - -# Add initial estimates to the Values container -for i in range(5): - v.insert(unknown[i], np.array([0.0])) - -# Initialize optimizer -params = gtsam.GaussNewtonParams() -optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) - -# Optimize the factor graph -result = optimizer.optimize() - -# calculate the error from ground truth -error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) - -print("Result with only GPS") -print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") - -# Adding odometry will improve things a lot -odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) - - -def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): +def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, + values: gtsam.Values, + jacobians: Optional[List[np.ndarray]]) -> float: """Odometry Factor error function :param measurement: Odometry measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -130,25 +68,9 @@ def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobi return error -for k in range(4): - odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]]))) - factor_graph.add(odof) - -params = gtsam.GaussNewtonParams() -optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) - -result = optimizer.optimize() - -error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) - -print("Result with GPS+Odometry") -print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") - -# This is great, but GPS noise is still apparent, so now we add the two landmarks -lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) - - -def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): +def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, + values: gtsam.Values, + jacobians: Optional[List[np.ndarray]]) -> float: """Landmark Factor error function :param measurement: Landmark measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -165,15 +87,120 @@ def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobian return error -factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0])))) -factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3])))) +def main(): + """Main runner.""" -params = gtsam.GaussNewtonParams() -optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + x = simulate_car() + print(f"Simulated car trajectory: {x}") -result = optimizer.optimize() + add_noise = True # set this to False to run with "perfect" measurements -error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) + # GPS measurements + sigma_gps = 3.0 # assume GPS is +/- 3m + g = [ + x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0) + for k in range(5) + ] -print("Result with GPS+Odometry+Landmark") -print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") + # Odometry measurements + sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz + o = [ + x[k + 1] - x[k] + + (np.random.normal(scale=sigma_odo) if add_noise else 0) + for k in range(4) + ] + + # Landmark measurements: + sigma_lm = 1 # assume landmark measurement is accurate up to 1m + + # Assume first landmark is at x=5, we measure it at time k=0 + lm_0 = 5.0 + z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0) + + # Assume other landmark is at x=28, we measure it at time k=3 + lm_3 = 28.0 + z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0) + + unknown = [gtsam.symbol('x', k) for k in range(5)] + + print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown))) + + # We now can use nonlinear factor graphs + factor_graph = gtsam.NonlinearFactorGraph() + + # Add factors for GPS measurements + gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) + + # Add the GPS factors + for k in range(5): + gf = gtsam.CustomFactor(gps_model, [unknown[k]], + partial(error_gps, np.array([g[k]]))) + factor_graph.add(gf) + + # New Values container + v = gtsam.Values() + + # Add initial estimates to the Values container + for i in range(5): + v.insert(unknown[i], np.array([0.0])) + + # Initialize optimizer + params = gtsam.GaussNewtonParams() + optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + + # Optimize the factor graph + result = optimizer.optimize() + + # calculate the error from ground truth + error = np.array([(result.atVector(unknown[k]) - x[k])[0] + for k in range(5)]) + + print("Result with only GPS") + print(result, np.round(error, 2), + f"\nJ(X)={0.5 * np.sum(np.square(error))}") + + # Adding odometry will improve things a lot + odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) + + for k in range(4): + odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], + partial(error_odom, np.array([o[k]]))) + factor_graph.add(odof) + + params = gtsam.GaussNewtonParams() + optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + + result = optimizer.optimize() + + error = np.array([(result.atVector(unknown[k]) - x[k])[0] + for k in range(5)]) + + print("Result with GPS+Odometry") + print(result, np.round(error, 2), + f"\nJ(X)={0.5 * np.sum(np.square(error))}") + + # This is great, but GPS noise is still apparent, so now we add the two landmarks + lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) + + factor_graph.add( + gtsam.CustomFactor(lm_model, [unknown[0]], + partial(error_lm, np.array([lm_0 + z_0])))) + factor_graph.add( + gtsam.CustomFactor(lm_model, [unknown[3]], + partial(error_lm, np.array([lm_3 + z_3])))) + + params = gtsam.GaussNewtonParams() + optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + + result = optimizer.optimize() + + error = np.array([(result.atVector(unknown[k]) - x[k])[0] + for k in range(5)]) + + print("Result with GPS+Odometry+Landmark") + print(result, np.round(error, 2), + f"\nJ(X)={0.5 * np.sum(np.square(error))}") + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/GPSFactorExample.py b/python/gtsam/examples/GPSFactorExample.py index 0bc0d1bf3..8eb663cb4 100644 --- a/python/gtsam/examples/GPSFactorExample.py +++ b/python/gtsam/examples/GPSFactorExample.py @@ -13,13 +13,8 @@ Author: Mandy Xie from __future__ import print_function -import numpy as np - import gtsam -import matplotlib.pyplot as plt -import gtsam.utils.plot as gtsam_plot - # ENU Origin is where the plane was in hold next to runway lat0 = 33.86998 lon0 = -84.30626 @@ -29,28 +24,34 @@ h0 = 274 GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25) -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() -# Add a prior on the first point, setting it to the origin -# A prior factor consists of a mean and a noise model (covariance matrix) -priorMean = gtsam.Pose3() # prior at origin -graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) +def main(): + """Main runner.""" + # Create an empty nonlinear factor graph + graph = gtsam.NonlinearFactorGraph() -# Add GPS factors -gps = gtsam.Point3(lat0, lon0, h0) -graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) + # Add a prior on the first point, setting it to the origin + # A prior factor consists of a mean and a noise model (covariance matrix) + priorMean = gtsam.Pose3() # prior at origin + graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) -# Create the data structure to hold the initialEstimate estimate to the solution -# For illustrative purposes, these have been deliberately set to incorrect values -initial = gtsam.Values() -initial.insert(1, gtsam.Pose3()) -print("\nInitial Estimate:\n{}".format(initial)) + # Add GPS factors + gps = gtsam.Point3(lat0, lon0, h0) + graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) + print("\nFactor Graph:\n{}".format(graph)) -# optimize using Levenberg-Marquardt optimization -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) + # Create the data structure to hold the initialEstimate estimate to the solution + # For illustrative purposes, these have been deliberately set to incorrect values + initial = gtsam.Values() + initial.insert(1, gtsam.Pose3()) + print("\nInitial Estimate:\n{}".format(initial)) + # optimize using Levenberg-Marquardt optimization + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + print("\nFinal Result:\n{}".format(result)) + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py new file mode 100644 index 000000000..8b6b4fdf0 --- /dev/null +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -0,0 +1,366 @@ +""" +Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE + +Author: Varun Agrawal +""" +import argparse +from typing import List, Tuple + +import gtsam +import numpy as np +from gtsam import ISAM2, Pose3, noiseModel +from gtsam.symbol_shorthand import B, V, X + +GRAVITY = 9.8 + + +class KittiCalibration: + """Class to hold KITTI calibration info.""" + def __init__(self, body_ptx: float, body_pty: float, body_ptz: float, + body_prx: float, body_pry: float, body_prz: float, + accelerometer_sigma: float, gyroscope_sigma: float, + integration_sigma: float, accelerometer_bias_sigma: float, + gyroscope_bias_sigma: float, average_delta_t: float): + self.bodyTimu = Pose3(gtsam.Rot3.RzRyRx(body_prx, body_pry, body_prz), + gtsam.Point3(body_ptx, body_pty, body_ptz)) + self.accelerometer_sigma = accelerometer_sigma + self.gyroscope_sigma = gyroscope_sigma + self.integration_sigma = integration_sigma + self.accelerometer_bias_sigma = accelerometer_bias_sigma + self.gyroscope_bias_sigma = gyroscope_bias_sigma + self.average_delta_t = average_delta_t + + +class ImuMeasurement: + """An instance of an IMU measurement.""" + def __init__(self, time: float, dt: float, accelerometer: gtsam.Point3, + gyroscope: gtsam.Point3): + self.time = time + self.dt = dt + self.accelerometer = accelerometer + self.gyroscope = gyroscope + + +class GpsMeasurement: + """An instance of a GPS measurement.""" + def __init__(self, time: float, position: gtsam.Point3): + self.time = time + self.position = position + + +def loadImuData(imu_data_file: str) -> List[ImuMeasurement]: + """Helper to load the IMU data.""" + # Read IMU data + # Time dt accelX accelY accelZ omegaX omegaY omegaZ + imu_data_file = gtsam.findExampleDataFile(imu_data_file) + imu_measurements = [] + + print("-- Reading IMU measurements from file") + with open(imu_data_file, encoding='UTF-8') as imu_data: + data = imu_data.readlines() + for i in range(1, len(data)): # ignore the first line + time, dt, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z = map( + float, data[i].split(' ')) + imu_measurement = ImuMeasurement( + time, dt, np.asarray([acc_x, acc_y, acc_z]), + np.asarray([gyro_x, gyro_y, gyro_z])) + imu_measurements.append(imu_measurement) + + return imu_measurements + + +def loadGpsData(gps_data_file: str) -> List[GpsMeasurement]: + """Helper to load the GPS data.""" + # Read GPS data + # Time,X,Y,Z + gps_data_file = gtsam.findExampleDataFile(gps_data_file) + gps_measurements = [] + + print("-- Reading GPS measurements from file") + with open(gps_data_file, encoding='UTF-8') as gps_data: + data = gps_data.readlines() + for i in range(1, len(data)): + time, x, y, z = map(float, data[i].split(',')) + gps_measurement = GpsMeasurement(time, np.asarray([x, y, z])) + gps_measurements.append(gps_measurement) + + return gps_measurements + + +def loadKittiData( + imu_data_file: str = "KittiEquivBiasedImu.txt", + gps_data_file: str = "KittiGps_converted.txt", + imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt" +) -> Tuple[KittiCalibration, List[ImuMeasurement], List[GpsMeasurement]]: + """ + Load the KITTI Dataset. + """ + # Read IMU metadata and compute relative sensor pose transforms + # BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma + # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma + # AverageDeltaT + imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) + with open(imu_metadata_file, encoding='UTF-8') as imu_metadata: + print("-- Reading sensor metadata") + line = imu_metadata.readline() # Ignore the first line + line = imu_metadata.readline().strip() + data = list(map(float, line.split(' '))) + kitti_calibration = KittiCalibration(*data) + print("IMU metadata:", data) + + imu_measurements = loadImuData(imu_data_file) + gps_measurements = loadGpsData(gps_data_file) + + return kitti_calibration, imu_measurements, gps_measurements + + +def getImuParams(kitti_calibration: KittiCalibration): + """Get the IMU parameters from the KITTI calibration data.""" + w_coriolis = np.zeros(3) + + # Set IMU preintegration parameters + measured_acc_cov = np.eye(3) * np.power( + kitti_calibration.accelerometer_sigma, 2) + measured_omega_cov = np.eye(3) * np.power( + kitti_calibration.gyroscope_sigma, 2) + # error committed in integrating position from velocities + integration_error_cov = np.eye(3) * np.power( + kitti_calibration.integration_sigma, 2) + + imu_params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY) + # acc white noise in continuous + imu_params.setAccelerometerCovariance(measured_acc_cov) + # integration uncertainty continuous + imu_params.setIntegrationCovariance(integration_error_cov) + # gyro white noise in continuous + imu_params.setGyroscopeCovariance(measured_omega_cov) + imu_params.setOmegaCoriolis(w_coriolis) + + return imu_params + + +def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, + gps_measurements: List[GpsMeasurement]): + """Write the results from `isam` to `output_filename`.""" + # Save results to file + print("Writing results to file...") + with open(output_filename, 'w', encoding='UTF-8') as fp_out: + fp_out.write( + "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n") + + result = isam.calculateEstimate() + for i in range(first_gps_pose, len(gps_measurements)): + pose_key = X(i) + vel_key = V(i) + bias_key = B(i) + + pose = result.atPose3(pose_key) + velocity = result.atVector(vel_key) + bias = result.atConstantBias(bias_key) + + pose_quat = pose.rotation().toQuaternion() + gps = gps_measurements[i].position + + print(f"State at #{i}") + print(f"Pose:\n{pose}") + print(f"Velocity:\n{velocity}") + print(f"Bias:\n{bias}") + + fp_out.write("{},{},{},{},{},{},{},{},{},{},{}\n".format( + gps_measurements[i].time, pose.x(), pose.y(), pose.z(), + pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), + gps[0], gps[1], gps[2])) + + +def parse_args() -> argparse.Namespace: + """Parse command line arguments.""" + parser = argparse.ArgumentParser() + parser.add_argument("--output_filename", + default="IMUKittiExampleGPSResults.csv") + return parser.parse_args() + + +def optimize(gps_measurements: List[GpsMeasurement], + imu_measurements: List[ImuMeasurement], + sigma_init_x: gtsam.noiseModel.Diagonal, + sigma_init_v: gtsam.noiseModel.Diagonal, + sigma_init_b: gtsam.noiseModel.Diagonal, + noise_model_gps: gtsam.noiseModel.Diagonal, + kitti_calibration: KittiCalibration, first_gps_pose: int, + gps_skip: int) -> gtsam.ISAM2: + """Run ISAM2 optimization on the measurements.""" + # Set initial conditions for the estimated trajectory + # initial pose is the reference frame (navigation frame) + current_pose_global = Pose3(gtsam.Rot3(), + gps_measurements[first_gps_pose].position) + + # the vehicle is stationary at the beginning at position 0,0,0 + current_velocity_global = np.zeros(3) + current_bias = gtsam.imuBias.ConstantBias() # init with zero bias + + imu_params = getImuParams(kitti_calibration) + + # Set ISAM2 parameters and create ISAM2 solver object + isam_params = gtsam.ISAM2Params() + isam_params.setFactorization("CHOLESKY") + isam_params.setRelinearizeSkip(10) + + isam = gtsam.ISAM2(isam_params) + + # Create the factor graph and values object that will store new factors and + # values to add to the incremental graph + new_factors = gtsam.NonlinearFactorGraph() + # values storing the initial estimates of new nodes in the factor graph + new_values = gtsam.Values() + + # Main loop: + # (1) we read the measurements + # (2) we create the corresponding factors in the graph + # (3) we solve the graph to obtain and optimal estimate of robot trajectory + print("-- Starting main loop: inference is performed at each time step, " + "but we plot trajectory every 10 steps") + + j = 0 + included_imu_measurement_count = 0 + + for i in range(first_gps_pose, len(gps_measurements)): + # At each non=IMU measurement we initialize a new node in the graph + current_pose_key = X(i) + current_vel_key = V(i) + current_bias_key = B(i) + t = gps_measurements[i].time + + if i == first_gps_pose: + # Create initial estimate and prior on initial pose, velocity, and biases + new_values.insert(current_pose_key, current_pose_global) + new_values.insert(current_vel_key, current_velocity_global) + new_values.insert(current_bias_key, current_bias) + + new_factors.addPriorPose3(current_pose_key, current_pose_global, + sigma_init_x) + new_factors.addPriorVector(current_vel_key, + current_velocity_global, sigma_init_v) + new_factors.addPriorConstantBias(current_bias_key, current_bias, + sigma_init_b) + else: + t_previous = gps_measurements[i - 1].time + + # Summarize IMU data between the previous GPS measurement and now + current_summarized_measurement = gtsam.PreintegratedImuMeasurements( + imu_params, current_bias) + + while (j < len(imu_measurements) + and imu_measurements[j].time <= t): + if imu_measurements[j].time >= t_previous: + current_summarized_measurement.integrateMeasurement( + imu_measurements[j].accelerometer, + imu_measurements[j].gyroscope, imu_measurements[j].dt) + included_imu_measurement_count += 1 + j += 1 + + # Create IMU factor + previous_pose_key = X(i - 1) + previous_vel_key = V(i - 1) + previous_bias_key = B(i - 1) + + new_factors.push_back( + gtsam.ImuFactor(previous_pose_key, previous_vel_key, + current_pose_key, current_vel_key, + previous_bias_key, + current_summarized_measurement)) + + # Bias evolution as given in the IMU metadata + sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas( + np.asarray([ + np.sqrt(included_imu_measurement_count) * + kitti_calibration.accelerometer_bias_sigma + ] * 3 + [ + np.sqrt(included_imu_measurement_count) * + kitti_calibration.gyroscope_bias_sigma + ] * 3)) + + new_factors.push_back( + gtsam.BetweenFactorConstantBias(previous_bias_key, + current_bias_key, + gtsam.imuBias.ConstantBias(), + sigma_between_b)) + + # Create GPS factor + gps_pose = Pose3(current_pose_global.rotation(), + gps_measurements[i].position) + if (i % gps_skip) == 0: + new_factors.addPriorPose3(current_pose_key, gps_pose, + noise_model_gps) + new_values.insert(current_pose_key, gps_pose) + + print(f"############ POSE INCLUDED AT TIME {t} ############") + print(gps_pose.translation(), "\n") + else: + new_values.insert(current_pose_key, current_pose_global) + + # Add initial values for velocity and bias based on the previous + # estimates + new_values.insert(current_vel_key, current_velocity_global) + new_values.insert(current_bias_key, current_bias) + + # Update solver + # ======================================================================= + # We accumulate 2*GPSskip GPS measurements before updating the solver at + # first so that the heading becomes observable. + if i > (first_gps_pose + 2 * gps_skip): + print(f"############ NEW FACTORS AT TIME {t:.6f} ############") + new_factors.print() + + isam.update(new_factors, new_values) + + # Reset the newFactors and newValues list + new_factors.resize(0) + new_values.clear() + + # Extract the result/current estimates + result = isam.calculateEstimate() + + current_pose_global = result.atPose3(current_pose_key) + current_velocity_global = result.atVector(current_vel_key) + current_bias = result.atConstantBias(current_bias_key) + + print(f"############ POSE AT TIME {t} ############") + current_pose_global.print() + print("\n") + + return isam + + +def main(): + """Main runner.""" + args = parse_args() + kitti_calibration, imu_measurements, gps_measurements = loadKittiData() + + if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8): + raise ValueError( + "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" + ) + + # Configure different variables + first_gps_pose = 1 + gps_skip = 10 + + # Configure noise models + noise_model_gps = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0] + [1.0 / 0.07] * 3)) + + sigma_init_x = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0, 1, 1, 1])) + sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) + sigma_init_b = noiseModel.Diagonal.Sigmas( + np.asarray([0.1] * 3 + [5.00e-05] * 3)) + + isam = optimize(gps_measurements, imu_measurements, sigma_init_x, + sigma_init_v, sigma_init_b, noise_model_gps, + kitti_calibration, first_gps_pose, gps_skip) + + save_results(isam, args.output_filename, first_gps_pose, gps_measurements) + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index bb707a8f5..c86a4e216 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -10,6 +10,8 @@ A script validating and demonstrating the ImuFactor inference. Author: Frank Dellaert, Varun Agrawal """ +# pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order + from __future__ import print_function import argparse @@ -25,14 +27,34 @@ from mpl_toolkits.mplot3d import Axes3D from PreintegrationExample import POSES_FIG, PreintegrationExample BIAS_KEY = B(0) - +GRAVITY = 9.81 np.set_printoptions(precision=3, suppress=True) -class ImuFactorExample(PreintegrationExample): +def parse_args() -> argparse.Namespace: + """Parse command line arguments.""" + 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 navigation time in seconds") + parser.add_argument("--compute_covariances", + default=False, + action='store_true') + parser.add_argument("--verbose", default=False, action='store_true') + args = parser.parse_args() + return args - def __init__(self, twist_scenario="sick_twist"): + +class ImuFactorExample(PreintegrationExample): + """Class to run example of the Imu Factor.""" + def __init__(self, twist_scenario: str = "sick_twist"): self.velocity = np.array([2, 0, 0]) self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) @@ -42,32 +64,92 @@ 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]) bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) + params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY) + + # Some arbitrary noise sigmas + gyro_sigma = 1e-3 + accel_sigma = 1e-3 + I_3x3 = np.eye(3) + params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3) + params.setAccelerometerCovariance(accel_sigma**2 * I_3x3) + params.setIntegrationCovariance(1e-7**2 * I_3x3) + dt = 1e-2 super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario], - bias, dt) + bias, params, dt) - def addPrior(self, i, graph): + def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph): + """Add a prior on the navigation state at time `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 run(self, T=12, compute_covariances=False, verbose=True): + def optimize(self, graph: gtsam.NonlinearFactorGraph, + initial: gtsam.Values): + """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, + values: gtsam.Values, + title: str = "Estimated Trajectory", + fignum: int = POSES_FIG + 1, + show: bool = False): + """ + Plot poses in values. + + Args: + values: The values object with the poses to plot. + title: The title of the plot. + fignum: The matplotlib figure number. + POSES_FIG is a value from the PreintegrationExample which we simply increment to generate a new figure. + show: Flag indicating whether to display the figure. + """ + i = 0 + while values.exists(X(i)): + pose_i = values.atPose3(X(i)) + plot_pose3(fignum, pose_i, 1) + i += 1 + plt.title(title) + + gtsam.utils.plot.set_axes_equal(fignum) + + print("Bias Values", values.atConstantBias(BIAS_KEY)) + + plt.ioff() + + if show: + plt.show() + + def run(self, + T: int = 12, + compute_covariances: bool = False, + verbose: bool = True): + """ + Main runner. + + Args: + T: Total trajectory time. + compute_covariances: Flag indicating whether to compute marginal covariances. + verbose: Flag indicating if printing should be verbose. + """ 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 +173,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 +189,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 +229,12 @@ 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, show=True) 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") - parser.add_argument("--compute_covariances", - default=False, action='store_true') - parser.add_argument("--verbose", default=False, action='store_true') - args = parser.parse_args() + args = 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/OdometryExample.py b/python/gtsam/examples/OdometryExample.py index 8b519ce9a..210aeb808 100644 --- a/python/gtsam/examples/OdometryExample.py +++ b/python/gtsam/examples/OdometryExample.py @@ -13,57 +13,60 @@ Author: Frank Dellaert from __future__ import print_function -import numpy as np - import gtsam - -import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt +import numpy as np # Create noise models ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() -# Add a prior on the first pose, setting it to the origin -# A prior factor consists of a mean and a noise model (covariance matrix) -priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin -graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) +def main(): + """Main runner""" + # Create an empty nonlinear factor graph + graph = gtsam.NonlinearFactorGraph() -# Add odometry factors -odometry = gtsam.Pose2(2.0, 0.0, 0.0) -# For simplicity, we will use the same noise model for each odometry factor -# Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) + # Add a prior on the first pose, setting it to the origin + # A prior factor consists of a mean and a noise model (covariance matrix) + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin + graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) -# Create the data structure to hold the initialEstimate estimate to the solution -# For illustrative purposes, these have been deliberately set to incorrect values -initial = gtsam.Values() -initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) -initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) -initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) -print("\nInitial Estimate:\n{}".format(initial)) + # Add odometry factors + odometry = gtsam.Pose2(2.0, 0.0, 0.0) + # For simplicity, we will use the same noise model for each odometry factor + # Create odometry (Between) factors between consecutive poses + graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) + graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) + print("\nFactor Graph:\n{}".format(graph)) -# optimize using Levenberg-Marquardt optimization -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) + # Create the data structure to hold the initialEstimate estimate to the solution + # For illustrative purposes, these have been deliberately set to incorrect values + initial = gtsam.Values() + initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) + print("\nInitial Estimate:\n{}".format(initial)) -# 5. Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for i in range(1, 4): - print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) - -fig = plt.figure(0) -for i in range(1, 4): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) -plt.axis('equal') -plt.show() + # optimize using Levenberg-Marquardt optimization + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + print("\nFinal Result:\n{}".format(result)) + # 5. Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + for i in range(1, 4): + print("X{} covariance:\n{}\n".format(i, + marginals.marginalCovariance(i))) + + for i in range(1, 4): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, + marginals.marginalCovariance(i)) + plt.axis('equal') + plt.show() +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py index 5ffdf048d..d2ee92c95 100644 --- a/python/gtsam/examples/PlanarSLAMExample.py +++ b/python/gtsam/examples/PlanarSLAMExample.py @@ -13,69 +13,85 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) from __future__ import print_function -import numpy as np - import gtsam -from gtsam.symbol_shorthand import X, L +import numpy as np +from gtsam.symbol_shorthand import L, X # Create noise models PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2])) -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() -# Create the keys corresponding to unknown variables in the factor graph -X1 = X(1) -X2 = X(2) -X3 = X(3) -L1 = L(4) -L2 = L(5) +def main(): + """Main runner""" -# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model -graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) + # Create an empty nonlinear factor graph + graph = gtsam.NonlinearFactorGraph() -# Add odometry factors between X1,X2 and X2,X3, respectively -graph.add(gtsam.BetweenFactorPose2( - X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) + # Create the keys corresponding to unknown variables in the factor graph + X1 = X(1) + X2 = X(2) + X3 = X(3) + L1 = L(4) + L2 = L(5) -# Add Range-Bearing measurements to two different landmarks L1 and L2 -graph.add(gtsam.BearingRangeFactor2D( - X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE)) -graph.add(gtsam.BearingRangeFactor2D( - X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) -graph.add(gtsam.BearingRangeFactor2D( - X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) + # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model + graph.add( + gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) -# Print graph -print("Factor Graph:\n{}".format(graph)) + # Add odometry factors between X1,X2 and X2,X3, respectively + graph.add( + gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), + ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), + ODOMETRY_NOISE)) -# Create (deliberately inaccurate) initial estimate -initial_estimate = gtsam.Values() -initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) -initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) -initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) -initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) -initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) + # Add Range-Bearing measurements to two different landmarks L1 and L2 + graph.add( + gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45), + np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE)) + graph.add( + gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, + MEASUREMENT_NOISE)) + graph.add( + gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, + MEASUREMENT_NOISE)) -# Print -print("Initial Estimate:\n{}".format(initial_estimate)) + # Print graph + print("Factor Graph:\n{}".format(graph)) -# Optimize using Levenberg-Marquardt optimization. The optimizer -# accepts an optional set of configuration parameters, controlling -# things like convergence criteria, the type of linear system solver -# to use, and the amount of information displayed during optimization. -# Here we will use the default set of parameters. See the -# documentation for the full set of parameters. -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) + # Create (deliberately inaccurate) initial estimate + initial_estimate = gtsam.Values() + initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) + initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) + initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) + initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) + initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) -# Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]: - print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key))) + # Print + print("Initial Estimate:\n{}".format(initial_estimate)) + + # Optimize using Levenberg-Marquardt optimization. The optimizer + # accepts an optional set of configuration parameters, controlling + # things like convergence criteria, the type of linear system solver + # to use, and the amount of information displayed during optimization. + # Here we will use the default set of parameters. See the + # documentation for the full set of parameters. + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, + params) + result = optimizer.optimize() + print("\nFinal Result:\n{}".format(result)) + + # Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), + (L2, "L2")]: + print("{} covariance:\n{}\n".format(s, + marginals.marginalCovariance(key))) + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py new file mode 100644 index 000000000..c70dbfa6f --- /dev/null +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -0,0 +1,178 @@ +""" +GTSAM Copyright 2010-2018, 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 + +Pose SLAM example using iSAM2 in the 2D plane. +Author: Jerred Chen, Yusuf Ali +Modeled after: + - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) + - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) +""" + +import math + +import matplotlib.pyplot as plt +import numpy as np + +import gtsam +import gtsam.utils.plot as gtsam_plot + +def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, + key: int): + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.""" + + # Print the current estimates computed using iSAM2. + print("*"*50 + f"\nInference after State {key+1}:\n") + print(current_estimate) + + # Compute the marginals for all states in the graph. + marginals = gtsam.Marginals(graph, current_estimate) + + # Plot the newly updated iSAM2 inference. + fig = plt.figure(0) + axes = fig.gca() + plt.cla() + + i = 1 + while current_estimate.exists(i): + gtsam_plot.plot_pose2(0, current_estimate.atPose2(i), 0.5, marginals.marginalCovariance(i)) + i += 1 + + plt.axis('equal') + axes.set_xlim(-1, 5) + axes.set_ylim(-1, 3) + plt.pause(1) + +def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, + key: int, xy_tol=0.6, theta_tol=17) -> int: + """Simple brute force approach which iterates through previous states + and checks for loop closure. + + Args: + odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame. + current_estimate: The current estimates computed by iSAM2. + key: Key corresponding to the current state estimate of the robot. + xy_tol: Optional argument for the x-y measurement tolerance, in meters. + theta_tol: Optional argument for the theta measurement tolerance, in degrees. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + prev_est = current_estimate.atPose2(key+1) + rotated_odom = prev_est.rotation().matrix() @ odom[:2] + curr_xy = np.array([prev_est.x() + rotated_odom[0], + prev_est.y() + rotated_odom[1]]) + curr_theta = prev_est.theta() + odom[2] + for k in range(1, key+1): + pose_xy = np.array([current_estimate.atPose2(k).x(), + current_estimate.atPose2(k).y()]) + pose_theta = current_estimate.atPose2(k).theta() + if (abs(pose_xy - curr_xy) <= xy_tol).all() and \ + (abs(pose_theta - curr_theta) <= theta_tol*np.pi/180): + return k + +def Pose2SLAM_ISAM2_example(): + """Perform 2D SLAM given the ground truth changes in pose as well as + simple loop closure detection.""" + plt.ion() + + # Declare the 2D translational standard deviations of the prior factor's Gaussian model, in meters. + prior_xy_sigma = 0.3 + + # Declare the 2D rotational standard deviation of the prior factor's Gaussian model, in degrees. + prior_theta_sigma = 5 + + # Declare the 2D translational standard deviations of the odometry factor's Gaussian model, in meters. + odometry_xy_sigma = 0.2 + + # Declare the 2D rotational standard deviation of the odometry factor's Gaussian model, in degrees. + odometry_theta_sigma = 5 + + # Although this example only uses linear measurements and Gaussian noise models, it is important + # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example + # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma, + prior_xy_sigma, + prior_theta_sigma*np.pi/180])) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma, + odometry_xy_sigma, + odometry_theta_sigma*np.pi/180])) + + # Create a Nonlinear factor graph as well as the data structure to hold state estimates. + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + + # Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # update calls are required to perform the relinearization. + parameters = gtsam.ISAM2Params() + parameters.setRelinearizeThreshold(0.1) + parameters.setRelinearizeSkip(1) + isam = gtsam.ISAM2(parameters) + + # Create the ground truth odometry measurements of the robot during the trajectory. + true_odometry = [(2, 0, 0), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2)] + + # Corrupt the odometry measurements with gaussian noise to create noisy odometry measurements. + odometry_measurements = [np.random.multivariate_normal(true_odom, ODOMETRY_NOISE.covariance()) + for true_odom in true_odometry] + + # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate + # iSAM2 incremental optimization. + graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) + initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + + # Initialize the current estimate which is used during the incremental inference loop. + current_estimate = initial_estimate + + for i in range(len(true_odometry)): + + # Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise. + noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i] + + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. + loop = determine_loop_closure(odometry_measurements[i], current_estimate, i, xy_tol=0.8, theta_tol=25) + + # Add a binary factor in between two existing states if loop closure is detected. + # Otherwise, add a binary factor between a newly observed state and the previous state. + if loop: + graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, + gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) + else: + graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, + gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) + + # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. + computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, + noisy_odom_y, + noisy_odom_theta)) + initial_estimate.insert(i + 2, computed_estimate) + + # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. + isam.update(graph, initial_estimate) + current_estimate = isam.calculateEstimate() + + # Report all current state estimates from the iSAM2 optimzation. + report_on_progress(graph, current_estimate, i) + initial_estimate.clear() + + # Print the final covariance matrix for each pose after completing inference on the trajectory. + marginals = gtsam.Marginals(graph, current_estimate) + i = 1 + for i in range(1, len(true_odometry)+1): + print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") + + plt.ioff() + plt.show() + + +if __name__ == "__main__": + Pose2SLAM_ISAM2_example() diff --git a/python/gtsam/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py index 2ec190d73..300a70fbd 100644 --- a/python/gtsam/examples/Pose2SLAMExample.py +++ b/python/gtsam/examples/Pose2SLAMExample.py @@ -15,82 +15,88 @@ from __future__ import print_function import math -import numpy as np - import gtsam - -import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt -def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) +def main(): + """Main runner.""" + # Create noise models + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(gtsam.Point3(0.3, 0.3, 0.1)) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas( + gtsam.Point3(0.2, 0.2, 0.1)) -# Create noise models -PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) -ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) + # 1. Create a factor graph container and add factors to it + graph = gtsam.NonlinearFactorGraph() -# 1. Create a factor graph container and add factors to it -graph = gtsam.NonlinearFactorGraph() + # 2a. Add a prior on the first pose, setting it to the origin + # A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) + graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) -# 2a. Add a prior on the first pose, setting it to the origin -# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) -graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) + # 2b. Add odometry factors + # Create odometry (Between) factors between consecutive poses + graph.add( + gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) -# 2b. Add odometry factors -# Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) + # 2c. Add the loop closure constraint + # This factor encodes the fact that we have returned to the same pose. In real + # systems, these constraints may be identified in many ways, such as appearance-based + # techniques with camera images. We will use another Between Factor to enforce this constraint: + graph.add( + gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + print("\nFactor Graph:\n{}".format(graph)) # print -# 2c. Add the loop closure constraint -# This factor encodes the fact that we have returned to the same pose. In real -# systems, these constraints may be identified in many ways, such as appearance-based -# techniques with camera images. We will use another Between Factor to enforce this constraint: -graph.add(gtsam.BetweenFactorPose2( - 5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) # print + # 3. Create the data structure to hold the initial_estimate estimate to the + # solution. For illustrative purposes, these have been deliberately set to incorrect values + initial_estimate = gtsam.Values() + initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) + initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) + initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) + print("\nInitial Estimate:\n{}".format(initial_estimate)) # print -# 3. Create the data structure to hold the initial_estimate estimate to the -# solution. For illustrative purposes, these have been deliberately set to incorrect values -initial_estimate = gtsam.Values() -initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) -initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) -initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) -initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) -initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) -print("\nInitial Estimate:\n{}".format(initial_estimate)) # print + # 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer + # The optimizer accepts an optional set of configuration parameters, + # controlling things like convergence criteria, the type of linear + # system solver to use, and the amount of information displayed during + # optimization. We will set a few parameters as a demonstration. + parameters = gtsam.GaussNewtonParams() -# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer -# The optimizer accepts an optional set of configuration parameters, -# controlling things like convergence criteria, the type of linear -# system solver to use, and the amount of information displayed during -# optimization. We will set a few parameters as a demonstration. -parameters = gtsam.GaussNewtonParams() + # Stop iterating once the change in error between steps is less than this value + parameters.setRelativeErrorTol(1e-5) + # Do not perform more than N iteration steps + parameters.setMaxIterations(100) + # Create the optimizer ... + optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) + # ... and optimize + result = optimizer.optimize() + print("Final Result:\n{}".format(result)) -# Stop iterating once the change in error between steps is less than this value -parameters.setRelativeErrorTol(1e-5) -# Do not perform more than N iteration steps -parameters.setMaxIterations(100) -# Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) -# ... and optimize -result = optimizer.optimize() -print("Final Result:\n{}".format(result)) + # 5. Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + for i in range(1, 6): + print("X{} covariance:\n{}\n".format(i, + marginals.marginalCovariance(i))) -# 5. Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for i in range(1, 6): - print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) + for i in range(1, 6): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, + marginals.marginalCovariance(i)) -fig = plt.figure(0) -for i in range(1, 6): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) + plt.axis('equal') + plt.show() -plt.axis('equal') -plt.show() + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose2SLAMExample_g2o.py b/python/gtsam/examples/Pose2SLAMExample_g2o.py index 97fb46c5f..cf029c049 100644 --- a/python/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/python/gtsam/examples/Pose2SLAMExample_g2o.py @@ -12,77 +12,86 @@ and does the optimization. Output is written on a file, in g2o format # pylint: disable=invalid-name, E1101 from __future__ import print_function + import argparse -import math -import numpy as np -import matplotlib.pyplot as plt import gtsam +import matplotlib.pyplot as plt from gtsam.utils import plot -def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) +def main(): + """Main runner.""" + + parser = argparse.ArgumentParser( + description="A 2D Pose SLAM example that reads input from g2o, " + "converts it to a factor graph and does the optimization. " + "Output is written on a file, in g2o format") + parser.add_argument('-i', '--input', help='input file g2o format') + parser.add_argument( + '-o', + '--output', + help="the path to the output file with optimized graph") + parser.add_argument('-m', + '--maxiter', + type=int, + help="maximum number of iterations for optimizer") + parser.add_argument('-k', + '--kernel', + choices=['none', 'huber', 'tukey'], + default="none", + help="Type of kernel used") + parser.add_argument("-p", + "--plot", + action="store_true", + help="Flag to plot results") + args = parser.parse_args() + + g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\ + else args.input + + maxIterations = 100 if args.maxiter is None else args.maxiter + + is3D = False + + graph, initial = gtsam.readG2o(g2oFile, is3D) + + assert args.kernel == "none", "Supplied kernel type is not yet implemented" + + # Add prior on the pose having index (key) = 0 + priorModel = gtsam.noiseModel.Diagonal.Variances(gtsam.Point3(1e-6, 1e-6, 1e-8)) + graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) + + params = gtsam.GaussNewtonParams() + params.setVerbosity("Termination") + params.setMaxIterations(maxIterations) + # parameters.setRelativeErrorTol(1e-5) + # Create the optimizer ... + optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) + # ... and optimize + result = optimizer.optimize() + + print("Optimization complete") + print("initial error = ", graph.error(initial)) + print("final error = ", graph.error(result)) + + if args.output is None: + print("\nFactor Graph:\n{}".format(graph)) + print("\nInitial Estimate:\n{}".format(initial)) + print("Final Result:\n{}".format(result)) + else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) + gtsam.writeG2o(graphNoKernel, result, outputFile) + print("Done!") + + if args.plot: + resultPoses = gtsam.utilities.extractPose2(result) + for i in range(resultPoses.shape[0]): + plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) + plt.show() -parser = argparse.ArgumentParser( - description="A 2D Pose SLAM example that reads input from g2o, " - "converts it to a factor graph and does the optimization. " - "Output is written on a file, in g2o format") -parser.add_argument('-i', '--input', help='input file g2o format') -parser.add_argument('-o', '--output', - help="the path to the output file with optimized graph") -parser.add_argument('-m', '--maxiter', type=int, - help="maximum number of iterations for optimizer") -parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'], - default="none", help="Type of kernel used") -parser.add_argument("-p", "--plot", action="store_true", - help="Flag to plot results") -args = parser.parse_args() - -g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\ - else args.input - -maxIterations = 100 if args.maxiter is None else args.maxiter - -is3D = False - -graph, initial = gtsam.readG2o(g2oFile, is3D) - -assert args.kernel == "none", "Supplied kernel type is not yet implemented" - -# Add prior on the pose having index (key) = 0 -priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) -graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) - -params = gtsam.GaussNewtonParams() -params.setVerbosity("Termination") -params.setMaxIterations(maxIterations) -# parameters.setRelativeErrorTol(1e-5) -# Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) -# ... and optimize -result = optimizer.optimize() - -print("Optimization complete") -print("initial error = ", graph.error(initial)) -print("final error = ", graph.error(result)) - - -if args.output is None: - print("\nFactor Graph:\n{}".format(graph)) - print("\nInitial Estimate:\n{}".format(initial)) - print("Final Result:\n{}".format(result)) -else: - outputFile = args.output - print("Writing results to file: ", outputFile) - graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) - gtsam.writeG2o(graphNoKernel, result, outputFile) - print ("Done!") - -if args.plot: - resultPoses = gtsam.utilities.extractPose2(result) - for i in range(resultPoses.shape[0]): - plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) - plt.show() +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose2SLAMExample_lago.py b/python/gtsam/examples/Pose2SLAMExample_lago.py new file mode 100644 index 000000000..d8cddde0b --- /dev/null +++ b/python/gtsam/examples/Pose2SLAMExample_lago.py @@ -0,0 +1,67 @@ +""" +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 + +A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem +using LAGO (Linear Approximation for Graph Optimization). +Output is written to a file, in g2o format + +Reference: +L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate +approximation for planar pose graph optimization, IJRR, 2014. + +L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation +for graph-based simultaneous localization and mapping, RSS, 2011. + +Author: Luca Carlone (C++), John Lambert (Python) +""" + +import argparse +from argparse import Namespace + +import numpy as np + +import gtsam +from gtsam import Point3, Pose2, PriorFactorPose2, Values + + +def run(args: Namespace) -> None: + """Run LAGO on input data stored in g2o file.""" + g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None else args.input + + graph = gtsam.NonlinearFactorGraph() + graph, initial = gtsam.readG2o(g2oFile) + + # Add prior on the pose having index (key) = 0 + priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8)) + graph.add(PriorFactorPose2(0, Pose2(), priorModel)) + print(graph) + + print("Computing LAGO estimate") + estimateLago: Values = gtsam.lago.initialize(graph) + print("done!") + + if args.output is None: + estimateLago.print("estimateLago") + else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel = gtsam.NonlinearFactorGraph() + graphNoKernel, initial2 = gtsam.readG2o(g2oFile) + gtsam.writeG2o(graphNoKernel, estimateLago, outputFile) + print("Done! ") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="A 2D Pose SLAM example that reads input from g2o, " + "converts it to a factor graph and does the optimization. " + "Output is written on a file, in g2o format" + ) + parser.add_argument("-i", "--input", help="input file g2o format") + parser.add_argument("-o", "--output", help="the path to the output file with optimized graph") + args = parser.parse_args() + run(args) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py new file mode 100644 index 000000000..59b9fb2ac --- /dev/null +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -0,0 +1,208 @@ +""" +GTSAM Copyright 2010-2018, 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 + +Pose SLAM example using iSAM2 in 3D space. +Author: Jerred Chen +Modeled after: + - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) + - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) +""" + +from typing import List + +import matplotlib.pyplot as plt +import numpy as np + +import gtsam +import gtsam.utils.plot as gtsam_plot + +def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, + key: int): + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.""" + + # Print the current estimates computed using iSAM2. + print("*"*50 + f"\nInference after State {key+1}:\n") + print(current_estimate) + + # Compute the marginals for all states in the graph. + marginals = gtsam.Marginals(graph, current_estimate) + + # Plot the newly updated iSAM2 inference. + fig = plt.figure(0) + axes = fig.gca(projection='3d') + plt.cla() + + i = 1 + while current_estimate.exists(i): + gtsam_plot.plot_pose3(0, current_estimate.atPose3(i), 10, + marginals.marginalCovariance(i)) + i += 1 + + axes.set_xlim3d(-30, 45) + axes.set_ylim3d(-30, 45) + axes.set_zlim3d(-30, 45) + plt.pause(1) + +def create_poses() -> List[gtsam.Pose3]: + """Creates ground truth poses of the robot.""" + P0 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + P1 = np.array([[0, -1, 0, 15], + [1, 0, 0, 15], + [0, 0, 1, 20], + [0, 0, 0, 1]]) + P2 = np.array([[np.cos(np.pi/4), 0, np.sin(np.pi/4), 30], + [0, 1, 0, 30], + [-np.sin(np.pi/4), 0, np.cos(np.pi/4), 30], + [0, 0, 0, 1]]) + P3 = np.array([[0, 1, 0, 30], + [0, 0, -1, 0], + [-1, 0, 0, -15], + [0, 0, 0, 1]]) + P4 = np.array([[-1, 0, 0, 0], + [0, -1, 0, -10], + [0, 0, 1, -10], + [0, 0, 0, 1]]) + P5 = P0[:] + + return [gtsam.Pose3(P0), gtsam.Pose3(P1), gtsam.Pose3(P2), + gtsam.Pose3(P3), gtsam.Pose3(P4), gtsam.Pose3(P5)] + +def determine_loop_closure(odom_tf: gtsam.Pose3, current_estimate: gtsam.Values, + key: int, xyz_tol=0.6, rot_tol=17) -> int: + """Simple brute force approach which iterates through previous states + and checks for loop closure. + + Args: + odom_tf: The noisy odometry transformation measurement in the body frame. + current_estimate: The current estimates computed by iSAM2. + key: Key corresponding to the current state estimate of the robot. + xyz_tol: Optional argument for the translational tolerance, in meters. + rot_tol: Optional argument for the rotational tolerance, in degrees. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + prev_est = current_estimate.atPose3(key+1) + curr_est = prev_est.compose(odom_tf) + for k in range(1, key+1): + pose = current_estimate.atPose3(k) + if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol*np.pi/180).all() and \ + (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): + return k + +def Pose3_ISAM2_example(): + """Perform 3D SLAM given ground truth poses as well as simple + loop closure detection.""" + plt.ion() + + # Declare the 3D translational standard deviations of the prior factor's Gaussian model, in meters. + prior_xyz_sigma = 0.3 + + # Declare the 3D rotational standard deviations of the prior factor's Gaussian model, in degrees. + prior_rpy_sigma = 5 + + # Declare the 3D translational standard deviations of the odometry factor's Gaussian model, in meters. + odometry_xyz_sigma = 0.2 + + # Declare the 3D rotational standard deviations of the odometry factor's Gaussian model, in degrees. + odometry_rpy_sigma = 5 + + # Although this example only uses linear measurements and Gaussian noise models, it is important + # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example + # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_rpy_sigma*np.pi/180, + prior_rpy_sigma*np.pi/180, + prior_rpy_sigma*np.pi/180, + prior_xyz_sigma, + prior_xyz_sigma, + prior_xyz_sigma])) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_rpy_sigma*np.pi/180, + odometry_rpy_sigma*np.pi/180, + odometry_rpy_sigma*np.pi/180, + odometry_xyz_sigma, + odometry_xyz_sigma, + odometry_xyz_sigma])) + + # Create a Nonlinear factor graph as well as the data structure to hold state estimates. + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + + # Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # update calls are required to perform the relinearization. + parameters = gtsam.ISAM2Params() + parameters.setRelinearizeThreshold(0.1) + parameters.setRelinearizeSkip(1) + isam = gtsam.ISAM2(parameters) + + # Create the ground truth poses of the robot trajectory. + true_poses = create_poses() + + # Create the ground truth odometry transformations, xyz translations, and roll-pitch-yaw rotations + # between each robot pose in the trajectory. + odometry_tf = [true_poses[i-1].transformPoseTo(true_poses[i]) for i in range(1, len(true_poses))] + odometry_xyz = [(odometry_tf[i].x(), odometry_tf[i].y(), odometry_tf[i].z()) for i in range(len(odometry_tf))] + odometry_rpy = [odometry_tf[i].rotation().rpy() for i in range(len(odometry_tf))] + + # Corrupt xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements. + noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), \ + ODOMETRY_NOISE.covariance()) for i in range(len(odometry_tf))] + + # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate + # iSAM2 incremental optimization. + graph.push_back(gtsam.PriorFactorPose3(1, true_poses[0], PRIOR_NOISE)) + initial_estimate.insert(1, true_poses[0].compose(gtsam.Pose3( + gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + + # Initialize the current estimate which is used during the incremental inference loop. + current_estimate = initial_estimate + for i in range(len(odometry_tf)): + + # Obtain the noisy translation and rotation that is received by the robot and corrupted by gaussian noise. + noisy_odometry = noisy_measurements[i] + + # Compute the noisy odometry transformation according to the xyz translation and roll-pitch-yaw rotation. + noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1)) + + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. + loop = determine_loop_closure(noisy_tf, current_estimate, i, xyz_tol=18, rot_tol=30) + + # Add a binary factor in between two existing states if loop closure is detected. + # Otherwise, add a binary factor between a newly observed state and the previous state. + if loop: + graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, noisy_tf, ODOMETRY_NOISE)) + else: + graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, noisy_tf, ODOMETRY_NOISE)) + + # Compute and insert the initialization estimate for the current pose using a noisy odometry measurement. + noisy_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) + initial_estimate.insert(i + 2, noisy_estimate) + + # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. + isam.update(graph, initial_estimate) + current_estimate = isam.calculateEstimate() + + # Report all current state estimates from the iSAM2 optimization. + report_on_progress(graph, current_estimate, i) + initial_estimate.clear() + + # Print the final covariance matrix for each pose after completing inference. + marginals = gtsam.Marginals(graph, current_estimate) + i = 1 + while current_estimate.exists(i): + print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") + i += 1 + + plt.ioff() + plt.show() + +if __name__ == '__main__': + Pose3_ISAM2_example() diff --git a/python/gtsam/examples/Pose3SLAMExample_g2o.py b/python/gtsam/examples/Pose3SLAMExample_g2o.py index 501a75dc1..dcdfc34a3 100644 --- a/python/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/python/gtsam/examples/Pose3SLAMExample_g2o.py @@ -8,13 +8,14 @@ # pylint: disable=invalid-name, E1101 from __future__ import print_function + import argparse -import numpy as np -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D import gtsam +import matplotlib.pyplot as plt +import numpy as np from gtsam.utils import plot +from mpl_toolkits.mplot3d import Axes3D def vector6(x, y, z, a, b, c): @@ -22,50 +23,62 @@ def vector6(x, y, z, a, b, c): return np.array([x, y, z, a, b, c], dtype=float) -parser = argparse.ArgumentParser( - description="A 3D Pose SLAM example that reads input from g2o, and " - "initializes Pose3") -parser.add_argument('-i', '--input', help='input file g2o format') -parser.add_argument('-o', '--output', - help="the path to the output file with optimized graph") -parser.add_argument("-p", "--plot", action="store_true", - help="Flag to plot results") -args = parser.parse_args() +def main(): + """Main runner.""" -g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ - else args.input + parser = argparse.ArgumentParser( + description="A 3D Pose SLAM example that reads input from g2o, and " + "initializes Pose3") + parser.add_argument('-i', '--input', help='input file g2o format') + parser.add_argument( + '-o', + '--output', + help="the path to the output file with optimized graph") + parser.add_argument("-p", + "--plot", + action="store_true", + help="Flag to plot results") + args = parser.parse_args() -is3D = True -graph, initial = gtsam.readG2o(g2oFile, is3D) + g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ + else args.input -# Add Prior on the first key -priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, - 1e-4, 1e-4, 1e-4)) + is3D = True + graph, initial = gtsam.readG2o(g2oFile, is3D) -print("Adding prior to g2o file ") -firstKey = initial.keys()[0] -graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) + # Add Prior on the first key + priorModel = gtsam.noiseModel.Diagonal.Variances( + vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) -params = gtsam.GaussNewtonParams() -params.setVerbosity("Termination") # this will show info about stopping conds -optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) -result = optimizer.optimize() -print("Optimization complete") + print("Adding prior to g2o file ") + firstKey = initial.keys()[0] + graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) -print("initial error = ", graph.error(initial)) -print("final error = ", graph.error(result)) + params = gtsam.GaussNewtonParams() + params.setVerbosity( + "Termination") # this will show info about stopping conds + optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) + result = optimizer.optimize() + print("Optimization complete") -if args.output is None: - print("Final Result:\n{}".format(result)) -else: - outputFile = args.output - print("Writing results to file: ", outputFile) - graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) - gtsam.writeG2o(graphNoKernel, result, outputFile) - print ("Done!") + print("initial error = ", graph.error(initial)) + print("final error = ", graph.error(result)) -if args.plot: - resultPoses = gtsam.utilities.allPose3s(result) - for i in range(resultPoses.size()): - plot.plot_pose3(1, resultPoses.atPose3(i)) - plt.show() + if args.output is None: + print("Final Result:\n{}".format(result)) + else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) + gtsam.writeG2o(graphNoKernel, result, outputFile) + print("Done!") + + if args.plot: + resultPoses = gtsam.utilities.allPose3s(result) + for i in range(resultPoses.size()): + plot.plot_pose3(1, resultPoses.atPose3(i)) + plt.show() + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py index 2b2c5f991..a96da0774 100644 --- a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py +++ b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -13,23 +13,29 @@ Author: Luca Carlone, Frank Dellaert (python port) from __future__ import print_function +import gtsam import numpy as np -import gtsam -# Read graph from file -g2oFile = gtsam.findExampleDataFile("pose3example.txt") +def main(): + """Main runner.""" + # Read graph from file + g2oFile = gtsam.findExampleDataFile("pose3example.txt") -is3D = True -graph, initial = gtsam.readG2o(g2oFile, is3D) + is3D = True + graph, initial = gtsam.readG2o(g2oFile, is3D) -# Add prior on the first key. TODO: assumes first key ios z -priorModel = gtsam.noiseModel.Diagonal.Variances( - np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) -firstKey = initial.keys()[0] -graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) + # Add prior on the first key. TODO: assumes first key ios z + priorModel = gtsam.noiseModel.Diagonal.Variances( + np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) + firstKey = initial.keys()[0] + graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) -# Initializing Pose3 - chordal relaxation" -initialization = gtsam.InitializePose3.initialize(graph) + # Initializing Pose3 - chordal relaxation + initialization = gtsam.InitializePose3.initialize(graph) -print(initialization) + print(initialization) + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index 458248c30..611c536c7 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -5,10 +5,14 @@ All Rights Reserved See LICENSE for the license information -A script validating the Preintegration of IMU measurements +A script validating the Preintegration of IMU measurements. + +Authors: Frank Dellaert, Varun Agrawal. """ -import math +# pylint: disable=invalid-name,unused-import,wrong-import-order + +from typing import Optional, Sequence import gtsam import matplotlib.pyplot as plt @@ -18,25 +22,28 @@ from mpl_toolkits.mplot3d import Axes3D IMU_FIG = 1 POSES_FIG = 2 +GRAVITY = 10 -class PreintegrationExample(object): - +class PreintegrationExample: + """Base class for all preintegration examples.""" @staticmethod - def defaultParams(g): + def defaultParams(g: float): """Create default parameters with Z *up* and realistic noise parameters""" params = gtsam.PreintegrationParams.MakeSharedU(g) - kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW + kGyroSigma = np.radians(0.5) / 60 # 0.5 degree ARW kAccelSigma = 0.1 / 60 # 10 cm VRW - params.setGyroscopeCovariance( - kGyroSigma ** 2 * np.identity(3, float)) - params.setAccelerometerCovariance( - kAccelSigma ** 2 * np.identity(3, float)) - params.setIntegrationCovariance( - 0.0000001 ** 2 * np.identity(3, float)) + params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float)) + params.setAccelerometerCovariance(kAccelSigma**2 * + np.identity(3, float)) + params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float)) return params - def __init__(self, twist=None, bias=None, dt=1e-2): + def __init__(self, + twist: Optional[np.ndarray] = None, + bias: Optional[gtsam.imuBias.ConstantBias] = None, + params: Optional[gtsam.PreintegrationParams] = None, + dt: float = 1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting @@ -48,7 +55,7 @@ class PreintegrationExample(object): else: # default = loop with forward velocity 2m/s, while pitching up # with angular velocity 30 degree/sec (negative in FLU) - W = np.array([0, -math.radians(30), 0]) + W = np.array([0, -np.radians(30), 0]) V = np.array([2, 0, 0]) self.scenario = gtsam.ConstantTwistScenario(W, V) @@ -58,9 +65,11 @@ class PreintegrationExample(object): self.labels = list('xyz') self.colors = list('rgb') - # Create runner - self.g = 10 # simple gravity constant - self.params = self.defaultParams(self.g) + if params: + self.params = params + else: + # Default params with simple gravity constant + self.params = self.defaultParams(g=GRAVITY) if bias is not None: self.actualBias = bias @@ -69,13 +78,22 @@ class PreintegrationExample(object): gyroBias = np.array([0, 0, 0]) self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias) - self.runner = gtsam.ScenarioRunner( - self.scenario, self.params, self.dt, self.actualBias) + # Create runner + self.runner = gtsam.ScenarioRunner(self.scenario, self.params, self.dt, + self.actualBias) fig, self.axes = plt.subplots(4, 3) fig.set_tight_layout(True) - def plotImu(self, t, measuredOmega, measuredAcc): + def plotImu(self, t: float, measuredOmega: Sequence, + measuredAcc: Sequence): + """ + Plot IMU measurements. + Args: + t: The time at which the measurement was recoreded. + measuredOmega: Measured angular velocity. + measuredAcc: Measured linear acceleration. + """ plt.figure(IMU_FIG) # plot angular velocity @@ -108,12 +126,21 @@ class PreintegrationExample(object): ax.scatter(t, measuredAcc[i], color=color, marker='.') ax.set_xlabel('specific force ' + label) - def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01): - # plot ground truth pose, as well as prediction from integrated IMU measurements + def plotGroundTruthPose(self, + t: float, + scale: float = 0.3, + time_interval: float = 0.01): + """ + Plot ground truth pose, as well as prediction from integrated IMU measurements. + Args: + t: Time at which the pose was obtained. + scale: The scaling factor for the pose axes. + time_interval: The time to wait before showing the plot. + """ actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, scale) - t = actualPose.translation() - self.maxDim = max([max(np.abs(t)), self.maxDim]) + translation = actualPose.translation() + self.maxDim = max([max(np.abs(translation)), self.maxDim]) ax = plt.gca() ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim) @@ -121,8 +148,8 @@ class PreintegrationExample(object): plt.pause(time_interval) - def run(self, T=12): - # simulate the loop + def run(self, T: int = 12): + """Simulate the loop.""" for i, t in enumerate(np.arange(0, T, self.dt)): measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) 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_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_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_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_lago.py b/python/gtsam/tests/test_lago.py new file mode 100644 index 000000000..8ed5dd943 --- /dev/null +++ b/python/gtsam/tests/test_lago.py @@ -0,0 +1,38 @@ +""" +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 + +Author: John Lambert (Python) +""" + +import unittest + +import numpy as np + +import gtsam +from gtsam import Point3, Pose2, PriorFactorPose2, Values + + +class TestLago(unittest.TestCase): + """Test selected LAGO methods.""" + + def test_initialize(self) -> None: + """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file.""" + g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") + + graph = gtsam.NonlinearFactorGraph() + graph, initial = gtsam.readG2o(g2oFile) + + # Add prior on the pose having index (key) = 0 + priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8)) + graph.add(PriorFactorPose2(0, Pose2(), priorModel)) + + estimateLago: Values = gtsam.lago.initialize(graph) + assert isinstance(estimateLago, Values) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index f324da63a..7ea393077 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -23,7 +23,10 @@ def set_axes_equal(fignum: int) -> None: 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(), @@ -339,7 +342,11 @@ def plot_pose3( """ # get figure object fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + 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]) diff --git a/tests/testLie.cpp b/tests/testLie.cpp index 0ef12198b..fe1173f22 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -129,6 +129,46 @@ TEST( testProduct, Logmap ) { EXPECT(assert_equal(numericH, actH, tol)); } +/* ************************************************************************* */ +Product interpolate_proxy(const Product& x, const Product& y, double t) { + return interpolate(x, y, t); +} + +TEST(Lie, Interpolate) { + Product x(Point2(1, 2), Pose2(3, 4, 5)); + Product y(Point2(6, 7), Pose2(8, 9, 0)); + + double t; + Matrix actH1, numericH1, actH2, numericH2; + + t = 0.0; + interpolate(x, y, t, actH1, actH2); + numericH1 = numericalDerivative31( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH1, actH1, tol)); + numericH2 = numericalDerivative32( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH2, actH2, tol)); + + t = 0.5; + interpolate(x, y, t, actH1, actH2); + numericH1 = numericalDerivative31( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH1, actH1, tol)); + numericH2 = numericalDerivative32( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH2, actH2, tol)); + + t = 1.0; + interpolate(x, y, t, actH1, actH2); + numericH1 = numericalDerivative31( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH1, actH1, tol)); + numericH2 = numericalDerivative32( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/wrap/.github/workflows/macos-ci.yml b/wrap/.github/workflows/macos-ci.yml index 3910d28d8..8119a3acb 100644 --- a/wrap/.github/workflows/macos-ci.yml +++ b/wrap/.github/workflows/macos-ci.yml @@ -27,10 +27,12 @@ jobs: - name: Python Dependencies run: | + pip3 install -U pip setuptools pip3 install -r requirements.txt - name: Build and Test run: | + # Build cmake . cd tests # Use Pytest to run all the tests. 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/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake index f341c2f98..2149c7195 100644 --- a/wrap/cmake/PybindWrap.cmake +++ b/wrap/cmake/PybindWrap.cmake @@ -192,9 +192,9 @@ function(install_python_files source_files dest_directory) endfunction() # ~~~ -# https://stackoverflow.com/questions/13959434/cmake-out-of-source-build-python-files +# Copy over the directory from source_folder to dest_foler # ~~~ -function(create_symlinks source_folder dest_folder) +function(copy_directory source_folder dest_folder) if(${source_folder} STREQUAL ${dest_folder}) return() endif() @@ -215,31 +215,13 @@ function(create_symlinks source_folder dest_folder) # Create REAL folder file(MAKE_DIRECTORY "${dest_folder}") - # Delete symlink if it exists + # Delete if it exists file(REMOVE "${dest_folder}/${path_file}") - # Get OS dependent path to use in `execute_process` - file(TO_NATIVE_PATH "${dest_folder}/${path_file}" link) + # Get OS dependent path to use in copy file(TO_NATIVE_PATH "${source_folder}/${path_file}" target) - # cmake-format: off - if(UNIX) - set(command ln -s ${target} ${link}) - else() - set(command cmd.exe /c mklink ${link} ${target}) - endif() - # cmake-format: on - - execute_process( - COMMAND ${command} - RESULT_VARIABLE result - ERROR_VARIABLE output) - - if(NOT ${result} EQUAL 0) - message( - FATAL_ERROR - "Could not create symbolic link for: ${target} --> ${output}") - endif() + file(COPY ${target} DESTINATION ${dest_folder}) endforeach(path_file) -endfunction(create_symlinks) +endfunction(copy_directory) diff --git a/wrap/gtwrap/interface_parser/classes.py b/wrap/gtwrap/interface_parser/classes.py index 11317962d..54beb86c1 100644 --- a/wrap/gtwrap/interface_parser/classes.py +++ b/wrap/gtwrap/interface_parser/classes.py @@ -62,6 +62,10 @@ class Method: self.parent = parent + def to_cpp(self) -> str: + """Generate the C++ code for wrapping.""" + return self.name + def __repr__(self) -> str: return "Method: {} {} {}({}){}".format( self.template, @@ -84,7 +88,8 @@ class StaticMethod: ``` """ rule = ( - STATIC # + Optional(Template.rule("template")) # + + STATIC # + ReturnType.rule("return_type") # + IDENT("name") # + LPAREN # @@ -92,16 +97,18 @@ class StaticMethod: + RPAREN # + SEMI_COLON # BR ).setParseAction( - lambda t: StaticMethod(t.name, t.return_type, t.args_list)) + lambda t: StaticMethod(t.name, t.return_type, t.args_list, t.template)) def __init__(self, name: str, return_type: ReturnType, args: ArgumentList, + template: Union[Template, Any] = None, parent: Union["Class", Any] = ''): self.name = name self.return_type = return_type self.args = args + self.template = template self.parent = parent @@ -119,24 +126,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: @@ -218,8 +228,8 @@ class Class: Rule for all the members within a class. """ rule = ZeroOrMore(Constructor.rule # - ^ StaticMethod.rule # ^ Method.rule # + ^ StaticMethod.rule # ^ Variable.rule # ^ Operator.rule # ^ Enum.rule # @@ -260,17 +270,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/interface_parser/type.py b/wrap/gtwrap/interface_parser/type.py index 49315cc56..e94db4ff2 100644 --- a/wrap/gtwrap/interface_parser/type.py +++ b/wrap/gtwrap/interface_parser/type.py @@ -158,6 +158,8 @@ class Type: """ Parsed datatype, can be either a fundamental type or a custom datatype. E.g. void, double, size_t, Matrix. + Think of this as a high-level type which encodes the typename and other + characteristics of the type. The type can optionally be a raw pointer, shared pointer or reference. Can also be optionally qualified with a `const`, e.g. `const int`. @@ -240,6 +242,9 @@ class Type: or self.typename.name in ["Matrix", "Vector"]) else "", typename=typename)) + def get_typename(self): + """Convenience method to get the typename of this type.""" + return self.typename.name class TemplatedType: """ diff --git a/wrap/gtwrap/matlab_wrapper/mixins.py b/wrap/gtwrap/matlab_wrapper/mixins.py index 217801ff3..2d7c75b39 100644 --- a/wrap/gtwrap/matlab_wrapper/mixins.py +++ b/wrap/gtwrap/matlab_wrapper/mixins.py @@ -112,7 +112,7 @@ class FormatMixin: if separator == "::": # C++ templates = [] - for idx in range(len(type_name.instantiations)): + for idx, _ in enumerate(type_name.instantiations): template = '{}'.format( self._format_type_name(type_name.instantiations[idx], include_namespace=include_namespace, @@ -124,7 +124,7 @@ class FormatMixin: formatted_type_name += '<{}>'.format(','.join(templates)) else: - for idx in range(len(type_name.instantiations)): + for idx, _ in enumerate(type_name.instantiations): formatted_type_name += '{}'.format( self._format_type_name(type_name.instantiations[idx], separator=separator, diff --git a/wrap/gtwrap/pybind_wrapper.py b/wrap/gtwrap/pybind_wrapper.py index 40571263a..809c69b56 100755 --- a/wrap/gtwrap/pybind_wrapper.py +++ b/wrap/gtwrap/pybind_wrapper.py @@ -119,8 +119,11 @@ class PybindWrapper: if py_method in self.python_keywords: py_method = py_method + "_" - is_method = isinstance(method, instantiator.InstantiatedMethod) - is_static = isinstance(method, parser.StaticMethod) + is_method = isinstance( + method, (parser.Method, instantiator.InstantiatedMethod)) + is_static = isinstance( + method, + (parser.StaticMethod, instantiator.InstantiatedStaticMethod)) return_void = method.return_type.is_void() args_names = method.args.names() py_args_names = self._py_args_names(method.args) diff --git a/wrap/gtwrap/template_instantiator.py b/wrap/gtwrap/template_instantiator.py deleted file mode 100644 index 0cda93d5d..000000000 --- a/wrap/gtwrap/template_instantiator.py +++ /dev/null @@ -1,644 +0,0 @@ -"""Code to help instantiate templated classes, methods and functions.""" - -# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, unused-variable - -import itertools -from copy import deepcopy -from typing import Any, Iterable, List, Sequence - -import gtwrap.interface_parser as parser - - -def instantiate_type(ctype: parser.Type, - template_typenames: List[str], - instantiations: List[parser.Typename], - cpp_typename: parser.Typename, - instantiated_class=None): - """ - Instantiate template typename for @p ctype. - - Args: - instiated_class (InstantiatedClass): - - @return If ctype's name is in the @p template_typenames, return the - corresponding type to replace in @p instantiations. - If ctype name is `This`, return the new typename @p `cpp_typename`. - Otherwise, return the original ctype. - """ - # make a deep copy so that there is no overwriting of original template params - ctype = deepcopy(ctype) - - # Check if the return type has template parameters - if ctype.typename.instantiations: - for idx, instantiation in enumerate(ctype.typename.instantiations): - if instantiation.name in template_typenames: - template_idx = template_typenames.index(instantiation.name) - ctype.typename.instantiations[ - idx] = instantiations[ # type: ignore - template_idx] - - return ctype - - str_arg_typename = str(ctype.typename) - - if str_arg_typename in template_typenames: - idx = template_typenames.index(str_arg_typename) - return parser.Type( - typename=instantiations[idx], - 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, - ) - elif str_arg_typename == 'This': - 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) - - return parser.Type( - typename=cpp_typename, - 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, - ) - else: - return ctype - - -def instantiate_args_list(args_list, template_typenames, instantiations, - cpp_typename): - """ - Instantiate template typenames in an argument list. - Type with name `This` will be replaced by @p `cpp_typename`. - - @param[in] args_list A list of `parser.Argument` to instantiate. - @param[in] template_typenames List of template typenames to instantiate, - e.g. ['T', 'U', 'V']. - @param[in] instantiations List of specific types to instantiate, each - associated with each template typename. Each type is a parser.Typename, - including its name and full namespaces. - @param[in] cpp_typename Full-namespace cpp class name of this instantiation - to replace for arguments of type named `This`. - @return A new list of parser.Argument which types are replaced with their - instantiations. - """ - instantiated_args = [] - for arg in args_list: - new_type = instantiate_type(arg.ctype, template_typenames, - instantiations, cpp_typename) - instantiated_args.append( - parser.Argument(name=arg.name, ctype=new_type, - default=arg.default)) - return instantiated_args - - -def instantiate_return_type(return_type, - template_typenames, - instantiations, - cpp_typename, - instantiated_class=None): - """Instantiate the return type.""" - new_type1 = instantiate_type(return_type.type1, - template_typenames, - instantiations, - cpp_typename, - instantiated_class=instantiated_class) - if return_type.type2: - new_type2 = instantiate_type(return_type.type2, - template_typenames, - instantiations, - cpp_typename, - instantiated_class=instantiated_class) - else: - new_type2 = '' - return parser.ReturnType(new_type1, new_type2) - - -def instantiate_name(original_name, instantiations): - """ - Concatenate instantiated types with an @p original name to form a new - instantiated name. - TODO(duy): To avoid conflicts, we should include the instantiation's - namespaces, but I find that too verbose. - """ - instantiated_names = [] - for inst in instantiations: - # Ensure the first character of the type is capitalized - name = inst.instantiated_name() - # Using `capitalize` on the complete name causes other caps to be lower case - instantiated_names.append(name.replace(name[0], name[0].capitalize())) - - return "{}{}".format(original_name, "".join(instantiated_names)) - - -class InstantiatedGlobalFunction(parser.GlobalFunction): - """ - Instantiate global functions. - - E.g. - template - T add(const T& x, const T& y); - """ - def __init__(self, original, instantiations=(), new_name=''): - self.original = original - self.instantiations = instantiations - self.template = '' - self.parent = original.parent - - if not original.template: - self.name = original.name - self.return_type = original.return_type - self.args = original.args - else: - self.name = instantiate_name( - original.name, instantiations) if not new_name else new_name - self.return_type = instantiate_return_type( - original.return_type, - self.original.template.typenames, - self.instantiations, - # Keyword type name `This` should already be replaced in the - # previous class template instantiation round. - cpp_typename='', - ) - instantiated_args = instantiate_args_list( - original.args.list(), - self.original.template.typenames, - self.instantiations, - # Keyword type name `This` should already be replaced in the - # previous class template instantiation round. - cpp_typename='', - ) - self.args = parser.ArgumentList(instantiated_args) - - super().__init__(self.name, - self.return_type, - self.args, - self.template, - parent=self.parent) - - def to_cpp(self): - """Generate the C++ code for wrapping.""" - if self.original.template: - instantiated_names = [ - inst.instantiated_name() for inst in self.instantiations - ] - ret = "{}<{}>".format(self.original.name, - ",".join(instantiated_names)) - else: - ret = self.original.name - return ret - - def __repr__(self): - return "Instantiated {}".format( - super(InstantiatedGlobalFunction, self).__repr__()) - - -class InstantiatedMethod(parser.Method): - """ - Instantiate method with template parameters. - - E.g. - class A { - template - void func(X x, Y y); - } - """ - def __init__(self, - original: parser.Method, - instantiations: Iterable[parser.Typename] = ()): - self.original = original - self.instantiations = instantiations - self.template: Any = '' - self.is_const = original.is_const - self.parent = original.parent - - # Check for typenames if templated. - # This way, we can gracefully handle both templated and non-templated methods. - typenames: Sequence = self.original.template.typenames if self.original.template else [] - self.name = instantiate_name(original.name, self.instantiations) - self.return_type = instantiate_return_type( - original.return_type, - typenames, - self.instantiations, - # Keyword type name `This` should already be replaced in the - # previous class template instantiation round. - cpp_typename='', - ) - - instantiated_args = instantiate_args_list( - original.args.list(), - typenames, - self.instantiations, - # Keyword type name `This` should already be replaced in the - # previous class template instantiation round. - cpp_typename='', - ) - self.args = parser.ArgumentList(instantiated_args) - - super().__init__(self.template, - self.name, - self.return_type, - self.args, - self.is_const, - parent=self.parent) - - def to_cpp(self): - """Generate the C++ code for wrapping.""" - if self.original.template: - # to_cpp will handle all the namespacing and templating - instantiation_list = [x.to_cpp() for x in self.instantiations] - # now can simply combine the instantiations, separated by commas - ret = "{}<{}>".format(self.original.name, - ",".join(instantiation_list)) - else: - ret = self.original.name - return ret - - def __repr__(self): - return "Instantiated {}".format( - super(InstantiatedMethod, self).__repr__()) - - -class InstantiatedClass(parser.Class): - """ - Instantiate the class defined in the interface file. - """ - def __init__(self, original: parser.Class, instantiations=(), new_name=''): - """ - Template - Instantiations: [T1, U1] - """ - self.original = original - self.instantiations = instantiations - - self.template = None - self.is_virtual = original.is_virtual - self.parent_class = original.parent_class - self.parent = original.parent - - # If the class is templated, check if the number of provided instantiations - # match the number of templates, else it's only a partial instantiation which is bad. - if original.template: - assert len(original.template.typenames) == len( - instantiations), "Typenames and instantiations mismatch!" - - # Get the instantiated name of the class. E.g. FuncDouble - self.name = instantiate_name( - original.name, instantiations) if not new_name else new_name - - # Check for typenames if templated. - # By passing in typenames, we can gracefully handle both templated and non-templated classes - # This will allow the `This` keyword to be used in both templated and non-templated classes. - typenames = self.original.template.typenames if self.original.template else [] - - # Instantiate the constructors, static methods, properties, respectively. - self.ctors = self.instantiate_ctors(typenames) - self.static_methods = self.instantiate_static_methods(typenames) - self.properties = self.instantiate_properties(typenames) - - # Instantiate all operator overloads - self.operators = self.instantiate_operators(typenames) - - # Set enums - self.enums = original.enums - - # Instantiate all instance methods - instantiated_methods = \ - self.instantiate_class_templates_in_methods(typenames) - - # Second instantiation round to instantiate templated methods. - # This is done in case both the class and the method are templated. - self.methods = [] - for method in instantiated_methods: - if not method.template: - self.methods.append(InstantiatedMethod(method, ())) - else: - instantiations = [] - # Get all combinations of template parameters - for instantiations in itertools.product( - *method.template.instantiations): - self.methods.append( - InstantiatedMethod(method, instantiations)) - - super().__init__( - self.template, - self.is_virtual, - self.name, - [self.parent_class], - self.ctors, - self.methods, - self.static_methods, - self.properties, - self.operators, - self.enums, - parent=self.parent, - ) - - def __repr__(self): - return "{virtual}Class {cpp_class} : {parent_class}\n"\ - "{ctors}\n{static_methods}\n{methods}\n{operators}".format( - virtual="virtual " if self.is_virtual else '', - cpp_class=self.to_cpp(), - parent_class=self.parent, - ctors="\n".join([repr(ctor) for ctor in self.ctors]), - static_methods="\n".join([repr(m) - for m in self.static_methods]), - methods="\n".join([repr(m) for m in self.methods]), - operators="\n".join([repr(op) for op in self.operators]) - ) - - def instantiate_ctors(self, typenames): - """ - Instantiate the class constructors. - - Args: - typenames: List of template types to instantiate. - - Return: List of constructors instantiated with provided template args. - """ - instantiated_ctors = [] - - for ctor in self.original.ctors: - instantiated_args = instantiate_args_list( - ctor.args.list(), - typenames, - self.instantiations, - self.cpp_typename(), - ) - instantiated_ctors.append( - parser.Constructor( - name=self.name, - args=parser.ArgumentList(instantiated_args), - parent=self, - )) - return instantiated_ctors - - def instantiate_static_methods(self, typenames): - """ - Instantiate static methods in the class. - - Args: - typenames: List of template types to instantiate. - - Return: List of static methods instantiated with provided template args. - """ - instantiated_static_methods = [] - for static_method in self.original.static_methods: - instantiated_args = instantiate_args_list( - static_method.args.list(), typenames, self.instantiations, - self.cpp_typename()) - instantiated_static_methods.append( - parser.StaticMethod( - name=static_method.name, - return_type=instantiate_return_type( - static_method.return_type, - typenames, - self.instantiations, - self.cpp_typename(), - instantiated_class=self), - args=parser.ArgumentList(instantiated_args), - parent=self, - )) - return instantiated_static_methods - - def instantiate_class_templates_in_methods(self, typenames): - """ - This function only instantiates the class-level templates in the methods. - Template methods are instantiated in InstantiatedMethod in the second - round. - - E.g. - ``` - template - class Greeter{ - void sayHello(T& name); - }; - - Args: - typenames: List of template types to instantiate. - - Return: List of methods instantiated with provided template args on the class. - """ - class_instantiated_methods = [] - for method in self.original.methods: - instantiated_args = instantiate_args_list( - method.args.list(), - typenames, - self.instantiations, - self.cpp_typename(), - ) - class_instantiated_methods.append( - parser.Method( - template=method.template, - name=method.name, - return_type=instantiate_return_type( - method.return_type, - typenames, - self.instantiations, - self.cpp_typename(), - ), - args=parser.ArgumentList(instantiated_args), - is_const=method.is_const, - parent=self, - )) - return class_instantiated_methods - - def instantiate_operators(self, typenames): - """ - Instantiate the class-level template in the operator overload. - - Args: - typenames: List of template types to instantiate. - - Return: List of methods instantiated with provided template args on the class. - """ - instantiated_operators = [] - for operator in self.original.operators: - instantiated_args = instantiate_args_list( - operator.args.list(), - typenames, - self.instantiations, - self.cpp_typename(), - ) - instantiated_operators.append( - parser.Operator( - name=operator.name, - operator=operator.operator, - return_type=instantiate_return_type( - operator.return_type, - typenames, - self.instantiations, - self.cpp_typename(), - ), - args=parser.ArgumentList(instantiated_args), - is_const=operator.is_const, - parent=self, - )) - return instantiated_operators - - def instantiate_properties(self, typenames): - """ - Instantiate the class properties. - - Args: - typenames: List of template types to instantiate. - - Return: List of properties instantiated with provided template args. - """ - instantiated_properties = instantiate_args_list( - self.original.properties, - typenames, - self.instantiations, - self.cpp_typename(), - ) - return instantiated_properties - - def cpp_typename(self): - """ - Return a parser.Typename including namespaces and cpp name of this - class. - """ - if self.original.template: - name = "{}<{}>".format( - self.original.name, - ", ".join([inst.to_cpp() for inst in self.instantiations])) - else: - name = self.original.name - namespaces_name = self.namespaces() - namespaces_name.append(name) - return parser.Typename(namespaces_name) - - def to_cpp(self): - """Generate the C++ code for wrapping.""" - return self.cpp_typename().to_cpp() - - -class InstantiatedDeclaration(parser.ForwardDeclaration): - """ - Instantiate typedefs of forward declarations. - This is useful when we wish to typedef a templated class - which is not defined in the current project. - - E.g. - class FactorFromAnotherMother; - - typedef FactorFromAnotherMother FactorWeCanUse; - """ - def __init__(self, original, instantiations=(), new_name=''): - super().__init__(original.typename, - original.parent_type, - original.is_virtual, - parent=original.parent) - - self.original = original - self.instantiations = instantiations - self.parent = original.parent - - self.name = instantiate_name( - original.name, instantiations) if not new_name else new_name - - def to_cpp(self): - """Generate the C++ code for wrapping.""" - instantiated_names = [ - inst.qualified_name() for inst in self.instantiations - ] - name = "{}<{}>".format(self.original.name, - ",".join(instantiated_names)) - namespaces_name = self.namespaces() - namespaces_name.append(name) - # Leverage Typename to generate the fully qualified C++ name - return parser.Typename(namespaces_name).to_cpp() - - def __repr__(self): - return "Instantiated {}".format( - super(InstantiatedDeclaration, self).__repr__()) - - -def instantiate_namespace(namespace): - """ - Instantiate the classes and other elements in the `namespace` content and - assign it back to the namespace content attribute. - - @param[in/out] namespace The namespace whose content will be replaced with - the instantiated content. - """ - instantiated_content = [] - typedef_content = [] - - for element in namespace.content: - if isinstance(element, parser.Class): - original_class = element - if not original_class.template: - instantiated_content.append( - InstantiatedClass(original_class, [])) - else: - # This case is for when the templates have enumerated instantiations. - - # Use itertools to get all possible combinations of instantiations - # Works even if one template does not have an instantiation list - for instantiations in itertools.product( - *original_class.template.instantiations): - instantiated_content.append( - InstantiatedClass(original_class, - list(instantiations))) - - elif isinstance(element, parser.GlobalFunction): - original_func = element - if not original_func.template: - instantiated_content.append( - InstantiatedGlobalFunction(original_func, [])) - else: - # Use itertools to get all possible combinations of instantiations - # Works even if one template does not have an instantiation list - for instantiations in itertools.product( - *original_func.template.instantiations): - instantiated_content.append( - InstantiatedGlobalFunction(original_func, - list(instantiations))) - - elif isinstance(element, parser.TypedefTemplateInstantiation): - # This is for the case where `typedef` statements are used - # to specify the template parameters. - typedef_inst = element - top_level = namespace.top_level() - original_element = top_level.find_class_or_function( - typedef_inst.typename) - - # Check if element is a typedef'd class, function or - # forward declaration from another project. - if isinstance(original_element, parser.Class): - typedef_content.append( - InstantiatedClass(original_element, - typedef_inst.typename.instantiations, - typedef_inst.new_name)) - elif isinstance(original_element, parser.GlobalFunction): - typedef_content.append( - InstantiatedGlobalFunction( - original_element, typedef_inst.typename.instantiations, - typedef_inst.new_name)) - elif isinstance(original_element, parser.ForwardDeclaration): - typedef_content.append( - InstantiatedDeclaration( - original_element, typedef_inst.typename.instantiations, - typedef_inst.new_name)) - - elif isinstance(element, parser.Namespace): - element = instantiate_namespace(element) - instantiated_content.append(element) - else: - instantiated_content.append(element) - - instantiated_content.extend(typedef_content) - namespace.content = instantiated_content - - return namespace diff --git a/wrap/gtwrap/template_instantiator/__init__.py b/wrap/gtwrap/template_instantiator/__init__.py new file mode 100644 index 000000000..6a30bb3c3 --- /dev/null +++ b/wrap/gtwrap/template_instantiator/__init__.py @@ -0,0 +1,14 @@ +"""Code to help instantiate templated classes, methods and functions.""" + +# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, unused-variable. unused-argument, too-many-branches + +from typing import Iterable, Sequence, Union + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.classes import * +from gtwrap.template_instantiator.constructor import * +from gtwrap.template_instantiator.declaration import * +from gtwrap.template_instantiator.function import * +from gtwrap.template_instantiator.helpers import * +from gtwrap.template_instantiator.method import * +from gtwrap.template_instantiator.namespace import * diff --git a/wrap/gtwrap/template_instantiator/classes.py b/wrap/gtwrap/template_instantiator/classes.py new file mode 100644 index 000000000..af366f80f --- /dev/null +++ b/wrap/gtwrap/template_instantiator/classes.py @@ -0,0 +1,206 @@ +"""Instantiate a class and its members.""" + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.constructor import InstantiatedConstructor +from gtwrap.template_instantiator.helpers import (InstantiationHelper, + instantiate_args_list, + instantiate_name, + instantiate_return_type) +from gtwrap.template_instantiator.method import (InstantiatedMethod, + InstantiatedStaticMethod) + + +class InstantiatedClass(parser.Class): + """ + Instantiate the class defined in the interface file. + """ + def __init__(self, original: parser.Class, instantiations=(), new_name=''): + """ + Template + Instantiations: [T1, U1] + """ + self.original = original + self.instantiations = instantiations + + self.template = None + self.is_virtual = original.is_virtual + self.parent_class = original.parent_class + self.parent = original.parent + + # If the class is templated, check if the number of provided instantiations + # match the number of templates, else it's only a partial instantiation which is bad. + if original.template: + assert len(original.template.typenames) == len( + instantiations), "Typenames and instantiations mismatch!" + + # Get the instantiated name of the class. E.g. FuncDouble + self.name = instantiate_name( + original.name, instantiations) if not new_name else new_name + + # Check for typenames if templated. + # By passing in typenames, we can gracefully handle both templated and non-templated classes + # This will allow the `This` keyword to be used in both templated and non-templated classes. + typenames = self.original.template.typenames if self.original.template else [] + + # Instantiate the constructors, static methods, properties, respectively. + self.ctors = self.instantiate_ctors(typenames) + self.static_methods = self.instantiate_static_methods(typenames) + self.properties = self.instantiate_properties(typenames) + + # Instantiate all operator overloads + self.operators = self.instantiate_operators(typenames) + + # Set enums + self.enums = original.enums + + # Instantiate all instance methods + self.methods = self.instantiate_methods(typenames) + + super().__init__( + self.template, + self.is_virtual, + self.name, + [self.parent_class], + self.ctors, + self.methods, + self.static_methods, + self.properties, + self.operators, + self.enums, + parent=self.parent, + ) + + def __repr__(self): + return "{virtual}Class {cpp_class} : {parent_class}\n"\ + "{ctors}\n{static_methods}\n{methods}\n{operators}".format( + virtual="virtual " if self.is_virtual else '', + cpp_class=self.to_cpp(), + parent_class=self.parent, + ctors="\n".join([repr(ctor) for ctor in self.ctors]), + static_methods="\n".join([repr(m) + for m in self.static_methods]), + methods="\n".join([repr(m) for m in self.methods]), + operators="\n".join([repr(op) for op in self.operators]) + ) + + def instantiate_ctors(self, typenames): + """ + Instantiate the class constructors. + + Args: + typenames: List of template types to instantiate. + + Return: List of constructors instantiated with provided template args. + """ + + helper = InstantiationHelper( + instantiation_type=InstantiatedConstructor) + + instantiated_ctors = helper.multilevel_instantiation( + self.original.ctors, typenames, self) + + return instantiated_ctors + + def instantiate_static_methods(self, typenames): + """ + Instantiate static methods in the class. + + Args: + typenames: List of template types to instantiate. + + Return: List of static methods instantiated with provided template args. + """ + helper = InstantiationHelper( + instantiation_type=InstantiatedStaticMethod) + + instantiated_static_methods = helper.multilevel_instantiation( + self.original.static_methods, typenames, self) + + return instantiated_static_methods + + def instantiate_methods(self, typenames): + """ + Instantiate regular methods in the class. + + Args: + typenames: List of template types to instantiate. + + Return: List of methods instantiated with provided template args. + """ + instantiated_methods = [] + + helper = InstantiationHelper(instantiation_type=InstantiatedMethod) + + instantiated_methods = helper.multilevel_instantiation( + self.original.methods, typenames, self) + + return instantiated_methods + + def instantiate_operators(self, typenames): + """ + Instantiate the class-level template in the operator overload. + + Args: + typenames: List of template types to instantiate. + + Return: List of methods instantiated with provided template args on the class. + """ + instantiated_operators = [] + for operator in self.original.operators: + instantiated_args = instantiate_args_list( + operator.args.list(), + typenames, + self.instantiations, + self.cpp_typename(), + ) + instantiated_operators.append( + parser.Operator( + name=operator.name, + operator=operator.operator, + return_type=instantiate_return_type( + operator.return_type, + typenames, + self.instantiations, + self.cpp_typename(), + ), + args=parser.ArgumentList(instantiated_args), + is_const=operator.is_const, + parent=self, + )) + return instantiated_operators + + def instantiate_properties(self, typenames): + """ + Instantiate the class properties. + + Args: + typenames: List of template types to instantiate. + + Return: List of properties instantiated with provided template args. + """ + instantiated_properties = instantiate_args_list( + self.original.properties, + typenames, + self.instantiations, + self.cpp_typename(), + ) + return instantiated_properties + + def cpp_typename(self): + """ + Return a parser.Typename including namespaces and cpp name of this + class. + """ + if self.original.template: + name = "{}<{}>".format( + self.original.name, + ", ".join([inst.to_cpp() for inst in self.instantiations])) + else: + name = self.original.name + namespaces_name = self.namespaces() + namespaces_name.append(name) + return parser.Typename(namespaces_name) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + return self.cpp_typename().to_cpp() diff --git a/wrap/gtwrap/template_instantiator/constructor.py b/wrap/gtwrap/template_instantiator/constructor.py new file mode 100644 index 000000000..1ea7d22d5 --- /dev/null +++ b/wrap/gtwrap/template_instantiator/constructor.py @@ -0,0 +1,64 @@ +"""Class constructor instantiator.""" + +# pylint: disable=unused-argument + +from typing import Iterable, List + +import gtwrap.interface_parser as parser + + +class InstantiatedConstructor(parser.Constructor): + """ + Instantiate constructor with template parameters. + + E.g. + class A { + template + A(X x, Y y); + } + """ + def __init__(self, + original: parser.Constructor, + instantiations: Iterable[parser.Typename] = ()): + self.original = original + self.instantiations = instantiations + self.name = original.name + self.args = original.args + self.template = original.template + self.parent = original.parent + + super().__init__(self.name, + self.args, + self.template, + parent=self.parent) + + @classmethod + def construct(cls, original: parser.Constructor, typenames: List[str], + class_instantiations: List[parser.Typename], + method_instantiations: List[parser.Typename], + instantiated_args: List[parser.Argument], + parent: 'InstantiatedClass'): + """Class method to construct object as required by InstantiationHelper.""" + method = parser.Constructor( + name=parent.name, + args=parser.ArgumentList(instantiated_args), + template=original.template, + parent=parent, + ) + return InstantiatedConstructor(method, + instantiations=method_instantiations) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + if self.original.template: + # to_cpp will handle all the namespacing and templating + instantiation_list = [x.to_cpp() for x in self.instantiations] + # now can simply combine the instantiations, separated by commas + ret = "{}<{}>".format(self.original.name, + ",".join(instantiation_list)) + else: + ret = self.original.name + return ret + + def __repr__(self): + return "Instantiated {}".format(super().__repr__()) diff --git a/wrap/gtwrap/template_instantiator/declaration.py b/wrap/gtwrap/template_instantiator/declaration.py new file mode 100644 index 000000000..4fa6b75d8 --- /dev/null +++ b/wrap/gtwrap/template_instantiator/declaration.py @@ -0,0 +1,45 @@ +"""Instantiate a forward declaration.""" + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.helpers import instantiate_name + + +class InstantiatedDeclaration(parser.ForwardDeclaration): + """ + Instantiate typedefs of forward declarations. + This is useful when we wish to typedef a templated class + which is not defined in the current project. + + E.g. + class FactorFromAnotherMother; + + typedef FactorFromAnotherMother FactorWeCanUse; + """ + def __init__(self, original, instantiations=(), new_name=''): + super().__init__(original.typename, + original.parent_type, + original.is_virtual, + parent=original.parent) + + self.original = original + self.instantiations = instantiations + self.parent = original.parent + + self.name = instantiate_name( + original.name, instantiations) if not new_name else new_name + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + instantiated_names = [ + inst.qualified_name() for inst in self.instantiations + ] + name = "{}<{}>".format(self.original.name, + ",".join(instantiated_names)) + namespaces_name = self.namespaces() + namespaces_name.append(name) + # Leverage Typename to generate the fully qualified C++ name + return parser.Typename(namespaces_name).to_cpp() + + def __repr__(self): + return "Instantiated {}".format( + super(InstantiatedDeclaration, self).__repr__()) diff --git a/wrap/gtwrap/template_instantiator/function.py b/wrap/gtwrap/template_instantiator/function.py new file mode 100644 index 000000000..3ad5da3f4 --- /dev/null +++ b/wrap/gtwrap/template_instantiator/function.py @@ -0,0 +1,68 @@ +"""Instantiate global function.""" + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.helpers import (instantiate_args_list, + instantiate_name, + instantiate_return_type) + + +class InstantiatedGlobalFunction(parser.GlobalFunction): + """ + Instantiate global functions. + + E.g. + template + T add(const T& x, const T& y); + """ + def __init__(self, original, instantiations=(), new_name=''): + self.original = original + self.instantiations = instantiations + self.template = '' + self.parent = original.parent + + if not original.template: + self.name = original.name + self.return_type = original.return_type + self.args = original.args + else: + self.name = instantiate_name( + original.name, instantiations) if not new_name else new_name + self.return_type = instantiate_return_type( + original.return_type, + self.original.template.typenames, + self.instantiations, + # Keyword type name `This` should already be replaced in the + # previous class template instantiation round. + cpp_typename='', + ) + instantiated_args = instantiate_args_list( + original.args.list(), + self.original.template.typenames, + self.instantiations, + # Keyword type name `This` should already be replaced in the + # previous class template instantiation round. + cpp_typename='', + ) + self.args = parser.ArgumentList(instantiated_args) + + super().__init__(self.name, + self.return_type, + self.args, + self.template, + parent=self.parent) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + if self.original.template: + instantiated_names = [ + inst.instantiated_name() for inst in self.instantiations + ] + ret = "{}<{}>".format(self.original.name, + ",".join(instantiated_names)) + else: + ret = self.original.name + return ret + + def __repr__(self): + return "Instantiated {}".format( + super(InstantiatedGlobalFunction, self).__repr__()) diff --git a/wrap/gtwrap/template_instantiator/helpers.py b/wrap/gtwrap/template_instantiator/helpers.py new file mode 100644 index 000000000..b55515dba --- /dev/null +++ b/wrap/gtwrap/template_instantiator/helpers.py @@ -0,0 +1,293 @@ +"""Various helpers for instantiation.""" + +import itertools +from copy import deepcopy +from typing import List, Sequence, Union + +import gtwrap.interface_parser as parser + +ClassMembers = Union[parser.Constructor, parser.Method, parser.StaticMethod, + parser.GlobalFunction, parser.Operator, parser.Variable, + parser.Enum] +InstantiatedMembers = Union['InstantiatedConstructor', 'InstantiatedMethod', + 'InstantiatedStaticMethod', + 'InstantiatedGlobalFunction'] + + +def is_scoped_template(template_typenames: Sequence[str], + str_arg_typename: str): + """ + Check if the template given by `str_arg_typename` is a scoped template e.g. T::Value, + and if so, return what template from `template_typenames` and + the corresponding index matches the scoped template correctly. + """ + for idx, template in enumerate(template_typenames): + if "::" in str_arg_typename and \ + template in str_arg_typename.split("::"): + return template, idx + return False, -1 + + +def instantiate_type( + ctype: parser.Type, + template_typenames: Sequence[str], + instantiations: Sequence[parser.Typename], + cpp_typename: parser.Typename, + instantiated_class: 'InstantiatedClass' = None) -> parser.Type: + """ + Instantiate template typename for `ctype`. + + Args: + ctype: The original argument type. + template_typenames: List of strings representing the templates. + instantiations: List of the instantiations of the templates in `template_typenames`. + cpp_typename: Full-namespace cpp class name of this instantiation + to replace for arguments of type named `This`. + instiated_class: The instantiated class which, if provided, + will be used for instantiating `This`. + + Returns: + If `ctype`'s name is in the `template_typenames`, return the + corresponding type to replace in `instantiations`. + If ctype name is `This`, return the new typename `cpp_typename`. + Otherwise, return the original ctype. + """ + # make a deep copy so that there is no overwriting of original template params + ctype = deepcopy(ctype) + + # Check if the return type has template parameters + if ctype.typename.instantiations: + for idx, instantiation in enumerate(ctype.typename.instantiations): + if instantiation.name in template_typenames: + template_idx = template_typenames.index(instantiation.name) + ctype.typename.instantiations[ + idx] = instantiations[ # type: ignore + template_idx] + + return ctype + + str_arg_typename = str(ctype.typename) + + # 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], + 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, + ) + + # 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) + cpp_typename = parser.Typename( + namespaces_name, + instantiations=instantiated_class.instantiations) + + return parser.Type( + typename=cpp_typename, + 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, + ) + + # 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 + + +def instantiate_args_list( + args_list: Sequence[parser.Argument], + template_typenames: Sequence[parser.template.Typename], + instantiations: Sequence, cpp_typename: parser.Typename): + """ + Instantiate template typenames in an argument list. + Type with name `This` will be replaced by @p `cpp_typename`. + + @param[in] args_list A list of `parser.Argument` to instantiate. + @param[in] template_typenames List of template typenames to instantiate, + e.g. ['T', 'U', 'V']. + @param[in] instantiations List of specific types to instantiate, each + associated with each template typename. Each type is a parser.Typename, + including its name and full namespaces. + @param[in] cpp_typename Full-namespace cpp class name of this instantiation + to replace for arguments of type named `This`. + @return A new list of parser.Argument which types are replaced with their + instantiations. + """ + instantiated_args = [] + for arg in args_list: + new_type = instantiate_type(arg.ctype, template_typenames, + instantiations, cpp_typename) + instantiated_args.append( + parser.Argument(name=arg.name, ctype=new_type, + default=arg.default)) + return instantiated_args + + +def instantiate_return_type( + return_type: parser.ReturnType, + template_typenames: Sequence[parser.template.Typename], + instantiations: Sequence[parser.Typename], + cpp_typename: parser.Typename, + instantiated_class: 'InstantiatedClass' = None): + """Instantiate the return type.""" + new_type1 = instantiate_type(return_type.type1, + template_typenames, + instantiations, + cpp_typename, + instantiated_class=instantiated_class) + if return_type.type2: + new_type2 = instantiate_type(return_type.type2, + template_typenames, + instantiations, + cpp_typename, + instantiated_class=instantiated_class) + else: + new_type2 = '' + return parser.ReturnType(new_type1, new_type2) + + +def instantiate_name(original_name: str, + instantiations: Sequence[parser.Typename]): + """ + Concatenate instantiated types with `original_name` to form a new + instantiated name. + + NOTE: To avoid conflicts, we should include the instantiation's + namespaces, but that is too verbose. + """ + instantiated_names = [] + for inst in instantiations: + # Ensure the first character of the type is capitalized + name = inst.instantiated_name() + # Using `capitalize` on the complete name causes other caps to be lower case + instantiated_names.append(name.replace(name[0], name[0].capitalize())) + + return "{}{}".format(original_name, "".join(instantiated_names)) + + +class InstantiationHelper: + """ + Helper class for instantiation templates. + Requires that `instantiation_type` defines a class method called + `construct` to generate the appropriate object type. + + Signature for `construct` should be + ``` + construct(method, + typenames, + class_instantiations, + method_instantiations, + instantiated_args, + parent=parent) + ``` + """ + def __init__(self, instantiation_type: InstantiatedMembers): + self.instantiation_type = instantiation_type + + def instantiate(self, instantiated_methods: List[InstantiatedMembers], + method: ClassMembers, typenames: Sequence[str], + class_instantiations: Sequence[parser.Typename], + method_instantiations: Sequence[parser.Typename], + parent: 'InstantiatedClass'): + """ + Instantiate both the class and method level templates. + """ + instantiations = class_instantiations + method_instantiations + + instantiated_args = instantiate_args_list(method.args.list(), + typenames, instantiations, + parent.cpp_typename()) + + instantiated_methods.append( + self.instantiation_type.construct(method, + typenames, + class_instantiations, + method_instantiations, + instantiated_args, + parent=parent)) + + return instantiated_methods + + def multilevel_instantiation(self, methods_list: Sequence[ClassMembers], + typenames: Sequence[str], + parent: 'InstantiatedClass'): + """ + Helper to instantiate methods at both the class and method level. + + Args: + methods_list: The list of methods in the class to instantiated. + typenames: List of class level template parameters, e.g. ['T']. + parent: The instantiated class to which `methods_list` belongs. + """ + instantiated_methods = [] + + for method in methods_list: + # We creare a copy since we will modify the typenames list. + method_typenames = deepcopy(typenames) + + if isinstance(method.template, parser.template.Template): + method_typenames.extend(method.template.typenames) + + # Get all combinations of template args + for instantiations in itertools.product( + *method.template.instantiations): + + instantiated_methods = self.instantiate( + instantiated_methods, + method, + typenames=method_typenames, + class_instantiations=parent.instantiations, + method_instantiations=list(instantiations), + parent=parent) + + else: + # If no constructor level templates, just use the class templates + instantiated_methods = self.instantiate( + instantiated_methods, + method, + typenames=method_typenames, + class_instantiations=parent.instantiations, + method_instantiations=[], + parent=parent) + + return instantiated_methods diff --git a/wrap/gtwrap/template_instantiator/method.py b/wrap/gtwrap/template_instantiator/method.py new file mode 100644 index 000000000..cd0a09c90 --- /dev/null +++ b/wrap/gtwrap/template_instantiator/method.py @@ -0,0 +1,124 @@ +"""Class method and static method instantiators.""" + +from typing import Iterable + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.helpers import (instantiate_name, + instantiate_return_type) + + +class InstantiatedMethod(parser.Method): + """ + Instantiate method with template parameters. + + E.g. + class A { + template + void func(X x, Y y); + } + """ + def __init__(self, + original: parser.Method, + instantiations: Iterable[parser.Typename] = ()): + self.original = original + self.instantiations = instantiations + self.template = original.template + self.is_const = original.is_const + self.parent = original.parent + + self.name = instantiate_name(original.name, self.instantiations) + self.return_type = original.return_type + self.args = original.args + + super().__init__(self.template, + self.name, + self.return_type, + self.args, + self.is_const, + parent=self.parent) + + @classmethod + def construct(cls, original, typenames, class_instantiations, + method_instantiations, instantiated_args, parent): + """Class method to construct object as required by InstantiationHelper.""" + method = parser.Method( + template=original.template, + name=original.name, + return_type=instantiate_return_type( + original.return_type, typenames, + class_instantiations + method_instantiations, + parent.cpp_typename()), + args=parser.ArgumentList(instantiated_args), + is_const=original.is_const, + parent=parent, + ) + return InstantiatedMethod(method, instantiations=method_instantiations) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + if self.original.template: + # to_cpp will handle all the namespacing and templating + instantiation_list = [x.to_cpp() for x in self.instantiations] + # now can simply combine the instantiations, separated by commas + ret = "{}<{}>".format(self.original.name, + ",".join(instantiation_list)) + else: + ret = self.original.name + return ret + + def __repr__(self): + return "Instantiated {}".format(super().__repr__()) + + +class InstantiatedStaticMethod(parser.StaticMethod): + """ + Instantiate static method with template parameters. + """ + def __init__(self, + original: parser.StaticMethod, + instantiations: Iterable[parser.Typename] = ()): + self.original = original + self.instantiations = instantiations + + self.name = instantiate_name(original.name, self.instantiations) + self.return_type = original.return_type + self.args = original.args + self.template = original.template + self.parent = original.parent + + super().__init__(self.name, self.return_type, self.args, self.template, + self.parent) + + @classmethod + def construct(cls, original, typenames, class_instantiations, + method_instantiations, instantiated_args, parent): + """Class method to construct object as required by InstantiationHelper.""" + method = parser.StaticMethod( + name=original.name, + return_type=instantiate_return_type(original.return_type, + typenames, + class_instantiations + + method_instantiations, + parent.cpp_typename(), + instantiated_class=parent), + args=parser.ArgumentList(instantiated_args), + template=original.template, + parent=parent, + ) + return InstantiatedStaticMethod(method, + instantiations=method_instantiations) + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + if self.original.template: + # to_cpp will handle all the namespacing and templating + instantiation_list = [x.to_cpp() for x in self.instantiations] + # now can simply combine the instantiations, separated by commas + ret = "{}<{}>".format(self.original.name, + ",".join(instantiation_list)) + else: + ret = self.original.name + return ret + + def __repr__(self): + return "Instantiated {}".format(super().__repr__()) diff --git a/wrap/gtwrap/template_instantiator/namespace.py b/wrap/gtwrap/template_instantiator/namespace.py new file mode 100644 index 000000000..32ba0b95d --- /dev/null +++ b/wrap/gtwrap/template_instantiator/namespace.py @@ -0,0 +1,88 @@ +"""Instantiate a namespace.""" + +import itertools + +import gtwrap.interface_parser as parser +from gtwrap.template_instantiator.classes import InstantiatedClass +from gtwrap.template_instantiator.declaration import InstantiatedDeclaration +from gtwrap.template_instantiator.function import InstantiatedGlobalFunction + + +def instantiate_namespace(namespace): + """ + Instantiate the classes and other elements in the `namespace` content and + assign it back to the namespace content attribute. + + @param[in/out] namespace The namespace whose content will be replaced with + the instantiated content. + """ + instantiated_content = [] + typedef_content = [] + + for element in namespace.content: + if isinstance(element, parser.Class): + original_class = element + if not original_class.template: + instantiated_content.append( + InstantiatedClass(original_class, [])) + else: + # This case is for when the templates have enumerated instantiations. + + # Use itertools to get all possible combinations of instantiations + # Works even if one template does not have an instantiation list + for instantiations in itertools.product( + *original_class.template.instantiations): + instantiated_content.append( + InstantiatedClass(original_class, + list(instantiations))) + + elif isinstance(element, parser.GlobalFunction): + original_func = element + if not original_func.template: + instantiated_content.append( + InstantiatedGlobalFunction(original_func, [])) + else: + # Use itertools to get all possible combinations of instantiations + # Works even if one template does not have an instantiation list + for instantiations in itertools.product( + *original_func.template.instantiations): + instantiated_content.append( + InstantiatedGlobalFunction(original_func, + list(instantiations))) + + elif isinstance(element, parser.TypedefTemplateInstantiation): + # This is for the case where `typedef` statements are used + # to specify the template parameters. + typedef_inst = element + top_level = namespace.top_level() + original_element = top_level.find_class_or_function( + typedef_inst.typename) + + # Check if element is a typedef'd class, function or + # forward declaration from another project. + if isinstance(original_element, parser.Class): + typedef_content.append( + InstantiatedClass(original_element, + typedef_inst.typename.instantiations, + typedef_inst.new_name)) + elif isinstance(original_element, parser.GlobalFunction): + typedef_content.append( + InstantiatedGlobalFunction( + original_element, typedef_inst.typename.instantiations, + typedef_inst.new_name)) + elif isinstance(original_element, parser.ForwardDeclaration): + typedef_content.append( + InstantiatedDeclaration( + original_element, typedef_inst.typename.instantiations, + typedef_inst.new_name)) + + elif isinstance(element, parser.Namespace): + element = instantiate_namespace(element) + instantiated_content.append(element) + else: + instantiated_content.append(element) + + instantiated_content.extend(typedef_content) + namespace.content = instantiated_content + + return namespace diff --git a/wrap/requirements.txt b/wrap/requirements.txt index a7181b271..0aac9302e 100644 --- a/wrap/requirements.txt +++ b/wrap/requirements.txt @@ -1,2 +1,2 @@ -pyparsing -pytest +pyparsing==2.4.7 +pytest>=6.2.4 diff --git a/wrap/tests/expected/matlab/FunDouble.m b/wrap/tests/expected/matlab/FunDouble.m index ca0efcacf..78609c7f6 100644 --- a/wrap/tests/expected/matlab/FunDouble.m +++ b/wrap/tests/expected/matlab/FunDouble.m @@ -7,6 +7,7 @@ % %-------Static Methods------- %staticMethodWithThis() : returns Fun +%templatedStaticMethodInt(int m) : returns double % %-------Serialization Interface------- %string_serialize() : returns string @@ -69,5 +70,16 @@ classdef FunDouble < handle error('Arguments do not match any overload of function FunDouble.staticMethodWithThis'); end + function varargout = TemplatedStaticMethodInt(varargin) + % TEMPLATEDSTATICMETHODINT usage: templatedStaticMethodInt(int m) : returns double + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 1 && isa(varargin{1},'numeric') + varargout{1} = class_wrapper(10, varargin{:}); + return + end + + error('Arguments do not match any overload of function FunDouble.templatedStaticMethodInt'); + end + end end diff --git a/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m b/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m index 1a00572e0..863d30ee8 100644 --- a/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m +++ b/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m @@ -9,7 +9,7 @@ classdef MultipleTemplatesIntDouble < handle function obj = MultipleTemplatesIntDouble(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(49, my_ptr); + class_wrapper(50, my_ptr); else error('Arguments do not match any overload of MultipleTemplatesIntDouble constructor'); end @@ -17,7 +17,7 @@ classdef MultipleTemplatesIntDouble < handle end function delete(obj) - class_wrapper(50, obj.ptr_MultipleTemplatesIntDouble); + class_wrapper(51, obj.ptr_MultipleTemplatesIntDouble); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m b/wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m index 6239b1bd1..b7f1fdac5 100644 --- a/wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m +++ b/wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m @@ -9,7 +9,7 @@ classdef MultipleTemplatesIntFloat < handle function obj = MultipleTemplatesIntFloat(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(51, my_ptr); + class_wrapper(52, my_ptr); else error('Arguments do not match any overload of MultipleTemplatesIntFloat constructor'); end @@ -17,7 +17,7 @@ classdef MultipleTemplatesIntFloat < handle end function delete(obj) - class_wrapper(52, obj.ptr_MultipleTemplatesIntFloat); + class_wrapper(53, obj.ptr_MultipleTemplatesIntFloat); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/matlab/MyFactorPosePoint2.m b/wrap/tests/expected/matlab/MyFactorPosePoint2.m index 2dd4b5428..7634ae2cb 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(63, 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(64, 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(65, 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(66, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/wrap/tests/expected/matlab/MyVector12.m b/wrap/tests/expected/matlab/MyVector12.m index 00a8f1965..291d0d71b 100644 --- a/wrap/tests/expected/matlab/MyVector12.m +++ b/wrap/tests/expected/matlab/MyVector12.m @@ -12,9 +12,9 @@ classdef MyVector12 < handle function obj = MyVector12(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(46, my_ptr); + class_wrapper(47, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(47); + my_ptr = class_wrapper(48); else error('Arguments do not match any overload of MyVector12 constructor'); end @@ -22,7 +22,7 @@ classdef MyVector12 < handle end function delete(obj) - class_wrapper(48, obj.ptr_MyVector12); + class_wrapper(49, obj.ptr_MyVector12); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/matlab/MyVector3.m b/wrap/tests/expected/matlab/MyVector3.m index 5d4a80cd8..3051dc2e2 100644 --- a/wrap/tests/expected/matlab/MyVector3.m +++ b/wrap/tests/expected/matlab/MyVector3.m @@ -12,9 +12,9 @@ classdef MyVector3 < handle function obj = MyVector3(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(43, my_ptr); + class_wrapper(44, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(44); + my_ptr = class_wrapper(45); else error('Arguments do not match any overload of MyVector3 constructor'); end @@ -22,7 +22,7 @@ classdef MyVector3 < handle end function delete(obj) - class_wrapper(45, obj.ptr_MyVector3); + class_wrapper(46, obj.ptr_MyVector3); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/matlab/PrimitiveRefDouble.m b/wrap/tests/expected/matlab/PrimitiveRefDouble.m index 14f04a825..dd0a6d2da 100644 --- a/wrap/tests/expected/matlab/PrimitiveRefDouble.m +++ b/wrap/tests/expected/matlab/PrimitiveRefDouble.m @@ -19,9 +19,9 @@ classdef PrimitiveRefDouble < handle function obj = PrimitiveRefDouble(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(39, my_ptr); + class_wrapper(40, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(40); + my_ptr = class_wrapper(41); else error('Arguments do not match any overload of PrimitiveRefDouble constructor'); end @@ -29,7 +29,7 @@ classdef PrimitiveRefDouble < handle end function delete(obj) - class_wrapper(41, obj.ptr_PrimitiveRefDouble); + class_wrapper(42, obj.ptr_PrimitiveRefDouble); end function display(obj), obj.print(''); end @@ -43,7 +43,7 @@ classdef PrimitiveRefDouble < handle % BRUTAL usage: Brutal(double t) : returns PrimitiveRefdouble % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(42, varargin{:}); + varargout{1} = class_wrapper(43, varargin{:}); return end diff --git a/wrap/tests/expected/matlab/Test.m b/wrap/tests/expected/matlab/Test.m index c39882a93..8569120c5 100644 --- a/wrap/tests/expected/matlab/Test.m +++ b/wrap/tests/expected/matlab/Test.m @@ -40,11 +40,11 @@ classdef Test < handle function obj = Test(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(10, my_ptr); + class_wrapper(11, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(11); + my_ptr = class_wrapper(12); elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') - my_ptr = class_wrapper(12, varargin{1}, varargin{2}); + my_ptr = class_wrapper(13, varargin{1}, varargin{2}); else error('Arguments do not match any overload of Test constructor'); end @@ -52,7 +52,7 @@ classdef Test < handle end function delete(obj) - class_wrapper(13, obj.ptr_Test); + class_wrapper(14, obj.ptr_Test); end function display(obj), obj.print(''); end @@ -63,7 +63,7 @@ classdef Test < handle % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - class_wrapper(14, this, varargin{:}); + class_wrapper(15, this, varargin{:}); return end error('Arguments do not match any overload of function Test.arg_EigenConstRef'); @@ -73,7 +73,7 @@ classdef Test < handle % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - [ varargout{1} varargout{2} ] = class_wrapper(15, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(16, this, varargin{:}); return end error('Arguments do not match any overload of function Test.create_MixedPtrs'); @@ -83,7 +83,7 @@ classdef Test < handle % CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - [ varargout{1} varargout{2} ] = class_wrapper(16, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(17, this, varargin{:}); return end error('Arguments do not match any overload of function Test.create_ptrs'); @@ -93,7 +93,7 @@ classdef Test < handle % GET_CONTAINER usage: get_container() : returns std.vectorTest % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - varargout{1} = class_wrapper(17, this, varargin{:}); + varargout{1} = class_wrapper(18, this, varargin{:}); return end error('Arguments do not match any overload of function Test.get_container'); @@ -103,7 +103,7 @@ classdef Test < handle % LAMBDA usage: lambda() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(18, this, varargin{:}); + class_wrapper(19, this, varargin{:}); return end error('Arguments do not match any overload of function Test.lambda'); @@ -113,7 +113,7 @@ classdef Test < handle % PRINT usage: print() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(19, this, varargin{:}); + class_wrapper(20, this, varargin{:}); return end error('Arguments do not match any overload of function Test.print'); @@ -123,7 +123,7 @@ classdef Test < handle % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = class_wrapper(20, this, varargin{:}); + varargout{1} = class_wrapper(21, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Point2Ptr'); @@ -133,7 +133,7 @@ classdef Test < handle % RETURN_TEST usage: return_Test(Test value) : returns Test % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(21, this, varargin{:}); + varargout{1} = class_wrapper(22, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Test'); @@ -143,7 +143,7 @@ classdef Test < handle % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(22, this, varargin{:}); + varargout{1} = class_wrapper(23, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_TestPtr'); @@ -153,7 +153,7 @@ classdef Test < handle % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = class_wrapper(23, this, varargin{:}); + varargout{1} = class_wrapper(24, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_bool'); @@ -163,7 +163,7 @@ classdef Test < handle % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(24, this, varargin{:}); + varargout{1} = class_wrapper(25, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_double'); @@ -173,7 +173,7 @@ classdef Test < handle % RETURN_FIELD usage: return_field(Test t) : returns bool % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(25, this, varargin{:}); + varargout{1} = class_wrapper(26, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_field'); @@ -183,7 +183,7 @@ classdef Test < handle % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = class_wrapper(26, this, varargin{:}); + varargout{1} = class_wrapper(27, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_int'); @@ -193,7 +193,7 @@ classdef Test < handle % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(27, this, varargin{:}); + varargout{1} = class_wrapper(28, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix1'); @@ -203,7 +203,7 @@ classdef Test < handle % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(28, this, varargin{:}); + varargout{1} = class_wrapper(29, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix2'); @@ -213,13 +213,13 @@ classdef Test < handle % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double') - [ varargout{1} varargout{2} ] = class_wrapper(29, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:}); return end % RETURN_PAIR usage: return_pair(Vector v) : returns pair< Vector, Matrix > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_pair'); @@ -229,7 +229,7 @@ classdef Test < handle % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') - [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(32, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_ptrs'); @@ -239,7 +239,7 @@ classdef Test < handle % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = class_wrapper(32, this, varargin{:}); + varargout{1} = class_wrapper(33, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_size_t'); @@ -249,7 +249,7 @@ classdef Test < handle % RETURN_STRING usage: return_string(string value) : returns string % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'char') - varargout{1} = class_wrapper(33, this, varargin{:}); + varargout{1} = class_wrapper(34, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_string'); @@ -259,7 +259,7 @@ classdef Test < handle % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - varargout{1} = class_wrapper(34, this, varargin{:}); + varargout{1} = class_wrapper(35, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_vector1'); @@ -269,19 +269,13 @@ classdef Test < handle % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - varargout{1} = class_wrapper(35, this, varargin{:}); + varargout{1} = class_wrapper(36, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_vector2'); end function varargout = set_container(this, varargin) - % SET_CONTAINER usage: set_container(vector container) : returns void - % Doxygen can be found at https://gtsam.org/doxygen/ - if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') - class_wrapper(36, this, varargin{:}); - return - end % SET_CONTAINER usage: set_container(vector container) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') @@ -294,6 +288,12 @@ classdef Test < handle class_wrapper(38, this, varargin{:}); return end + % SET_CONTAINER usage: set_container(vector container) : returns void + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') + class_wrapper(39, this, varargin{:}); + return + end error('Arguments do not match any overload of function Test.set_container'); end diff --git a/wrap/tests/expected/matlab/class_wrapper.cpp b/wrap/tests/expected/matlab/class_wrapper.cpp index fab9c1450..df6cb3307 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; @@ -238,7 +246,14 @@ void FunDouble_staticMethodWithThis_9(int nargout, mxArray *out[], int nargin, c out[0] = wrap_shared_ptr(boost::make_shared>(Fun::staticMethodWithThis()),"Fundouble", false); } -void Test_collectorInsertAndMakeBase_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void FunDouble_templatedStaticMethodInt_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("FunDouble.templatedStaticMethodInt",nargout,nargin,1); + int m = unwrap< int >(in[0]); + out[0] = wrap< double >(Fun::templatedStaticMethodInt(m)); +} + +void Test_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -247,7 +262,7 @@ void Test_collectorInsertAndMakeBase_10(int nargout, mxArray *out[], int nargin, collector_Test.insert(self); } -void Test_constructor_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -258,7 +273,7 @@ void Test_constructor_11(int nargout, mxArray *out[], int nargin, const mxArray *reinterpret_cast (mxGetData(out[0])) = self; } -void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -271,7 +286,7 @@ void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *reinterpret_cast (mxGetData(out[0])) = self; } -void Test_deconstructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_Test",nargout,nargin,1); @@ -284,7 +299,7 @@ void Test_deconstructor_13(int nargout, mxArray *out[], int nargin, const mxArra } } -void Test_arg_EigenConstRef_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_arg_EigenConstRef_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("arg_EigenConstRef",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -292,7 +307,7 @@ void Test_arg_EigenConstRef_14(int nargout, mxArray *out[], int nargin, const mx obj->arg_EigenConstRef(value); } -void Test_create_MixedPtrs_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_create_MixedPtrs_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_MixedPtrs",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -301,7 +316,7 @@ void Test_create_MixedPtrs_15(int nargout, mxArray *out[], int nargin, const mxA out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_create_ptrs_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_create_ptrs_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -310,28 +325,28 @@ void Test_create_ptrs_16(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_get_container_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_get_container_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("get_container",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); out[0] = wrap_shared_ptr(boost::make_shared>(obj->get_container()),"std.vectorTest", false); } -void Test_lambda_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_lambda_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("lambda",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->lambda(); } -void Test_print_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_print_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->print(); } -void Test_return_Point2Ptr_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Point2Ptr_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Point2Ptr",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -342,7 +357,7 @@ void Test_return_Point2Ptr_20(int nargout, mxArray *out[], int nargin, const mxA } } -void Test_return_Test_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Test_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Test",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -350,7 +365,7 @@ void Test_return_Test_21(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap_shared_ptr(boost::make_shared(obj->return_Test(value)),"Test", false); } -void Test_return_TestPtr_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_TestPtr_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -358,7 +373,7 @@ void Test_return_TestPtr_22(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); } -void Test_return_bool_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_bool_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -366,7 +381,7 @@ void Test_return_bool_23(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_bool(value)); } -void Test_return_double_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_double_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -374,7 +389,7 @@ void Test_return_double_24(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< double >(obj->return_double(value)); } -void Test_return_field_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_field_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -382,7 +397,7 @@ void Test_return_field_25(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_field(t)); } -void Test_return_int_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_int_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -390,7 +405,7 @@ void Test_return_int_26(int nargout, mxArray *out[], int nargin, const mxArray * out[0] = wrap< int >(obj->return_int(value)); } -void Test_return_matrix1_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix1_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -398,7 +413,7 @@ void Test_return_matrix1_27(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix1(value)); } -void Test_return_matrix2_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix2_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -406,7 +421,7 @@ void Test_return_matrix2_28(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix2(value)); } -void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -417,7 +432,7 @@ void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -427,7 +442,7 @@ void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_ptrs_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_ptrs_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -438,7 +453,7 @@ void Test_return_ptrs_31(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_return_size_t_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_size_t_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -446,7 +461,7 @@ void Test_return_size_t_32(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< size_t >(obj->return_size_t(value)); } -void Test_return_string_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_string_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -454,7 +469,7 @@ void Test_return_string_33(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< string >(obj->return_string(value)); } -void Test_return_vector1_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector1_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -462,7 +477,7 @@ void Test_return_vector1_34(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector1(value)); } -void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector2_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -470,14 +485,6 @@ void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector2(value)); } -void Test_set_container_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("set_container",nargout,nargin-1,1); - auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); - boost::shared_ptr> container = unwrap_shared_ptr< std::vector >(in[1], "ptr_stdvectorTest"); - obj->set_container(*container); -} - void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("set_container",nargout,nargin-1,1); @@ -494,7 +501,15 @@ void Test_set_container_38(int nargout, mxArray *out[], int nargin, const mxArra obj->set_container(*container); } -void PrimitiveRefDouble_collectorInsertAndMakeBase_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_set_container_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("set_container",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + boost::shared_ptr> container = unwrap_shared_ptr< std::vector >(in[1], "ptr_stdvectorTest"); + obj->set_container(*container); +} + +void PrimitiveRefDouble_collectorInsertAndMakeBase_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -503,7 +518,7 @@ void PrimitiveRefDouble_collectorInsertAndMakeBase_39(int nargout, mxArray *out[ collector_PrimitiveRefDouble.insert(self); } -void PrimitiveRefDouble_constructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_constructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -514,7 +529,7 @@ void PrimitiveRefDouble_constructor_40(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void PrimitiveRefDouble_deconstructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_deconstructor_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1); @@ -527,14 +542,14 @@ void PrimitiveRefDouble_deconstructor_41(int nargout, mxArray *out[], int nargin } } -void PrimitiveRefDouble_Brutal_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_Brutal_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("PrimitiveRefDouble.Brutal",nargout,nargin,1); double t = unwrap< double >(in[0]); out[0] = wrap_shared_ptr(boost::make_shared>(PrimitiveRef::Brutal(t)),"PrimitiveRefdouble", false); } -void MyVector3_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_collectorInsertAndMakeBase_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -543,7 +558,7 @@ void MyVector3_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int na collector_MyVector3.insert(self); } -void MyVector3_constructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_constructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -554,7 +569,7 @@ void MyVector3_constructor_44(int nargout, mxArray *out[], int nargin, const mxA *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector3_deconstructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_deconstructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector3",nargout,nargin,1); @@ -567,7 +582,7 @@ void MyVector3_deconstructor_45(int nargout, mxArray *out[], int nargin, const m } } -void MyVector12_collectorInsertAndMakeBase_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_collectorInsertAndMakeBase_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -576,7 +591,7 @@ void MyVector12_collectorInsertAndMakeBase_46(int nargout, mxArray *out[], int n collector_MyVector12.insert(self); } -void MyVector12_constructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_constructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -587,7 +602,7 @@ void MyVector12_constructor_47(int nargout, mxArray *out[], int nargin, const mx *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector12_deconstructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_deconstructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector12",nargout,nargin,1); @@ -600,7 +615,7 @@ void MyVector12_deconstructor_48(int nargout, mxArray *out[], int nargin, const } } -void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -609,7 +624,7 @@ void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(int nargout, mxArr collector_MultipleTemplatesIntDouble.insert(self); } -void MultipleTemplatesIntDouble_deconstructor_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_deconstructor_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1); @@ -622,7 +637,7 @@ void MultipleTemplatesIntDouble_deconstructor_50(int nargout, mxArray *out[], in } } -void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -631,7 +646,7 @@ void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(int nargout, mxArra collector_MultipleTemplatesIntFloat.insert(self); } -void MultipleTemplatesIntFloat_deconstructor_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_deconstructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1); @@ -644,7 +659,7 @@ void MultipleTemplatesIntFloat_deconstructor_52(int nargout, mxArray *out[], int } } -void ForwardKinematics_collectorInsertAndMakeBase_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_collectorInsertAndMakeBase_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -653,7 +668,7 @@ void ForwardKinematics_collectorInsertAndMakeBase_53(int nargout, mxArray *out[] collector_ForwardKinematics.insert(self); } -void ForwardKinematics_constructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_constructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -669,7 +684,7 @@ void ForwardKinematics_constructor_54(int nargout, mxArray *out[], int nargin, c *reinterpret_cast (mxGetData(out[0])) = self; } -void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_deconstructor_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_ForwardKinematics",nargout,nargin,1); @@ -682,7 +697,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_57(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_58(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_59(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_60(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_61(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_62(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_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -691,7 +775,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_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -706,7 +790,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_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -719,7 +803,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_66(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -771,85 +855,85 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) FunDouble_staticMethodWithThis_9(nargout, out, nargin-1, in+1); break; case 10: - Test_collectorInsertAndMakeBase_10(nargout, out, nargin-1, in+1); + FunDouble_templatedStaticMethodInt_10(nargout, out, nargin-1, in+1); break; case 11: - Test_constructor_11(nargout, out, nargin-1, in+1); + Test_collectorInsertAndMakeBase_11(nargout, out, nargin-1, in+1); break; case 12: Test_constructor_12(nargout, out, nargin-1, in+1); break; case 13: - Test_deconstructor_13(nargout, out, nargin-1, in+1); + Test_constructor_13(nargout, out, nargin-1, in+1); break; case 14: - Test_arg_EigenConstRef_14(nargout, out, nargin-1, in+1); + Test_deconstructor_14(nargout, out, nargin-1, in+1); break; case 15: - Test_create_MixedPtrs_15(nargout, out, nargin-1, in+1); + Test_arg_EigenConstRef_15(nargout, out, nargin-1, in+1); break; case 16: - Test_create_ptrs_16(nargout, out, nargin-1, in+1); + Test_create_MixedPtrs_16(nargout, out, nargin-1, in+1); break; case 17: - Test_get_container_17(nargout, out, nargin-1, in+1); + Test_create_ptrs_17(nargout, out, nargin-1, in+1); break; case 18: - Test_lambda_18(nargout, out, nargin-1, in+1); + Test_get_container_18(nargout, out, nargin-1, in+1); break; case 19: - Test_print_19(nargout, out, nargin-1, in+1); + Test_lambda_19(nargout, out, nargin-1, in+1); break; case 20: - Test_return_Point2Ptr_20(nargout, out, nargin-1, in+1); + Test_print_20(nargout, out, nargin-1, in+1); break; case 21: - Test_return_Test_21(nargout, out, nargin-1, in+1); + Test_return_Point2Ptr_21(nargout, out, nargin-1, in+1); break; case 22: - Test_return_TestPtr_22(nargout, out, nargin-1, in+1); + Test_return_Test_22(nargout, out, nargin-1, in+1); break; case 23: - Test_return_bool_23(nargout, out, nargin-1, in+1); + Test_return_TestPtr_23(nargout, out, nargin-1, in+1); break; case 24: - Test_return_double_24(nargout, out, nargin-1, in+1); + Test_return_bool_24(nargout, out, nargin-1, in+1); break; case 25: - Test_return_field_25(nargout, out, nargin-1, in+1); + Test_return_double_25(nargout, out, nargin-1, in+1); break; case 26: - Test_return_int_26(nargout, out, nargin-1, in+1); + Test_return_field_26(nargout, out, nargin-1, in+1); break; case 27: - Test_return_matrix1_27(nargout, out, nargin-1, in+1); + Test_return_int_27(nargout, out, nargin-1, in+1); break; case 28: - Test_return_matrix2_28(nargout, out, nargin-1, in+1); + Test_return_matrix1_28(nargout, out, nargin-1, in+1); break; case 29: - Test_return_pair_29(nargout, out, nargin-1, in+1); + Test_return_matrix2_29(nargout, out, nargin-1, in+1); break; case 30: Test_return_pair_30(nargout, out, nargin-1, in+1); break; case 31: - Test_return_ptrs_31(nargout, out, nargin-1, in+1); + Test_return_pair_31(nargout, out, nargin-1, in+1); break; case 32: - Test_return_size_t_32(nargout, out, nargin-1, in+1); + Test_return_ptrs_32(nargout, out, nargin-1, in+1); break; case 33: - Test_return_string_33(nargout, out, nargin-1, in+1); + Test_return_size_t_33(nargout, out, nargin-1, in+1); break; case 34: - Test_return_vector1_34(nargout, out, nargin-1, in+1); + Test_return_string_34(nargout, out, nargin-1, in+1); break; case 35: - Test_return_vector2_35(nargout, out, nargin-1, in+1); + Test_return_vector1_35(nargout, out, nargin-1, in+1); break; case 36: - Test_set_container_36(nargout, out, nargin-1, in+1); + Test_return_vector2_36(nargout, out, nargin-1, in+1); break; case 37: Test_set_container_37(nargout, out, nargin-1, in+1); @@ -858,67 +942,88 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) Test_set_container_38(nargout, out, nargin-1, in+1); break; case 39: - PrimitiveRefDouble_collectorInsertAndMakeBase_39(nargout, out, nargin-1, in+1); + Test_set_container_39(nargout, out, nargin-1, in+1); break; case 40: - PrimitiveRefDouble_constructor_40(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_collectorInsertAndMakeBase_40(nargout, out, nargin-1, in+1); break; case 41: - PrimitiveRefDouble_deconstructor_41(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_constructor_41(nargout, out, nargin-1, in+1); break; case 42: - PrimitiveRefDouble_Brutal_42(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_deconstructor_42(nargout, out, nargin-1, in+1); break; case 43: - MyVector3_collectorInsertAndMakeBase_43(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_Brutal_43(nargout, out, nargin-1, in+1); break; case 44: - MyVector3_constructor_44(nargout, out, nargin-1, in+1); + MyVector3_collectorInsertAndMakeBase_44(nargout, out, nargin-1, in+1); break; case 45: - MyVector3_deconstructor_45(nargout, out, nargin-1, in+1); + MyVector3_constructor_45(nargout, out, nargin-1, in+1); break; case 46: - MyVector12_collectorInsertAndMakeBase_46(nargout, out, nargin-1, in+1); + MyVector3_deconstructor_46(nargout, out, nargin-1, in+1); break; case 47: - MyVector12_constructor_47(nargout, out, nargin-1, in+1); + MyVector12_collectorInsertAndMakeBase_47(nargout, out, nargin-1, in+1); break; case 48: - MyVector12_deconstructor_48(nargout, out, nargin-1, in+1); + MyVector12_constructor_48(nargout, out, nargin-1, in+1); break; case 49: - MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(nargout, out, nargin-1, in+1); + MyVector12_deconstructor_49(nargout, out, nargin-1, in+1); break; case 50: - MultipleTemplatesIntDouble_deconstructor_50(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(nargout, out, nargin-1, in+1); break; case 51: - MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_deconstructor_51(nargout, out, nargin-1, in+1); break; case 52: - MultipleTemplatesIntFloat_deconstructor_52(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(nargout, out, nargin-1, in+1); break; case 53: - ForwardKinematics_collectorInsertAndMakeBase_53(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_deconstructor_53(nargout, out, nargin-1, in+1); break; case 54: - ForwardKinematics_constructor_54(nargout, out, nargin-1, in+1); + ForwardKinematics_collectorInsertAndMakeBase_54(nargout, out, nargin-1, in+1); break; case 55: - ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1); + ForwardKinematics_constructor_55(nargout, out, nargin-1, in+1); break; case 56: - MyFactorPosePoint2_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); + ForwardKinematics_deconstructor_56(nargout, out, nargin-1, in+1); break; case 57: - MyFactorPosePoint2_constructor_57(nargout, out, nargin-1, in+1); + TemplatedConstructor_collectorInsertAndMakeBase_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_constructor_61(nargout, out, nargin-1, in+1); + break; + case 62: + TemplatedConstructor_deconstructor_62(nargout, out, nargin-1, in+1); + break; + case 63: + MyFactorPosePoint2_collectorInsertAndMakeBase_63(nargout, out, nargin-1, in+1); + break; + case 64: + MyFactorPosePoint2_constructor_64(nargout, out, nargin-1, in+1); + break; + case 65: + MyFactorPosePoint2_deconstructor_65(nargout, out, nargin-1, in+1); + break; + case 66: + MyFactorPosePoint2_print_66(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..a54d9135b 100644 --- a/wrap/tests/expected/python/class_pybind.cpp +++ b/wrap/tests/expected/python/class_pybind.cpp @@ -31,7 +31,8 @@ PYBIND11_MODULE(class_py, m_) { py::class_, std::shared_ptr>>(m_, "FunDouble") .def("templatedMethodString",[](Fun* self, double d, string t){return self->templatedMethod(d, t);}, py::arg("d"), py::arg("t")) .def("multiTemplatedMethodStringSize_t",[](Fun* self, double d, string t, size_t u){return self->multiTemplatedMethod(d, t, u);}, py::arg("d"), py::arg("t"), py::arg("u")) - .def_static("staticMethodWithThis",[](){return Fun::staticMethodWithThis();}); + .def_static("staticMethodWithThis",[](){return Fun::staticMethodWithThis();}) + .def_static("templatedStaticMethodInt",[](const int& m){return Fun::templatedStaticMethod(m);}, py::arg("m")); py::class_>(m_, "Test") .def(py::init<>()) @@ -86,6 +87,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..40a565506 100644 --- a/wrap/tests/fixtures/class.i +++ b/wrap/tests/fixtures/class.i @@ -7,8 +7,12 @@ class FunRange { template class Fun { + static This staticMethodWithThis(); + template + static double templatedStaticMethod(const T& m); + template This templatedMethod(double d, T t); @@ -118,5 +122,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..49165328c 100644 --- a/wrap/tests/test_interface_parser.py +++ b/wrap/tests/test_interface_parser.py @@ -18,11 +18,13 @@ import unittest sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from gtwrap.interface_parser import ( - ArgumentList, Class, Constructor, Enum, Enumerator, ForwardDeclaration, - GlobalFunction, Include, Method, Module, Namespace, Operator, ReturnType, - StaticMethod, TemplatedType, Type, TypedefTemplateInstantiation, Typename, - Variable) +from gtwrap.interface_parser import (ArgumentList, Class, Constructor, Enum, + Enumerator, ForwardDeclaration, + GlobalFunction, Include, Method, Module, + Namespace, Operator, ReturnType, + StaticMethod, TemplatedType, Type, + TypedefTemplateInstantiation, Typename, + Variable) class TestInterfaceParser(unittest.TestCase): @@ -266,6 +268,11 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual("char", return_type.type1.typename.name) self.assertEqual("int", return_type.type2.typename.name) + return_type = ReturnType.rule.parseString("pair")[0] + self.assertEqual("Test", return_type.type1.typename.name) + self.assertEqual("Test", return_type.type2.typename.name) + self.assertTrue(return_type.type2.is_shared_ptr) + def test_method(self): """Test for a class method.""" ret = Method.rule.parseString("int f();")[0] @@ -283,6 +290,13 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual("f", ret.name) self.assertEqual(3, len(ret.args)) + ret = Method.rule.parseString( + "pair create_MixedPtrs();")[0] + self.assertEqual("create_MixedPtrs", ret.name) + self.assertEqual(0, len(ret.args)) + self.assertEqual("First", ret.return_type.type1.typename.name) + self.assertEqual("Second", ret.return_type.type2.typename.name) + def test_static_method(self): """Test for static methods.""" ret = StaticMethod.rule.parseString("static int f();")[0] @@ -314,6 +328,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..34940d62e 100644 --- a/wrap/tests/test_matlab_wrapper.py +++ b/wrap/tests/test_matlab_wrapper.py @@ -42,17 +42,16 @@ class TestWrap(unittest.TestCase): # Create the `actual/matlab` directory os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True) - def compare_and_diff(self, file): + def compare_and_diff(self, file, actual): """ Compute the comparison between the expected and actual file, and assert if diff is zero. """ - output = osp.join(self.MATLAB_ACTUAL_DIR, file) expected = osp.join(self.MATLAB_TEST_DIR, file) - success = filecmp.cmp(output, expected) + success = filecmp.cmp(actual, expected) + if not success: - print("Differ in file: {}".format(file)) - os.system("diff {} {}".format(output, expected)) + os.system("diff {} {}".format(actual, expected)) self.assertTrue(success, "Mismatch for file {0}".format(file)) def test_geometry(self): @@ -77,7 +76,8 @@ class TestWrap(unittest.TestCase): self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam'))) for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_functions(self): """Test interface file with function info.""" @@ -99,7 +99,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_class(self): """Test interface file with only class info.""" @@ -121,7 +122,26 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) + + 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: + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_inheritance(self): """Test interface file with class inheritance definitions.""" @@ -140,7 +160,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_namespaces(self): """ @@ -164,7 +185,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_special_cases(self): """ @@ -186,7 +208,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) def test_multiple_files(self): """ @@ -211,7 +234,8 @@ class TestWrap(unittest.TestCase): ] for file in files: - self.compare_and_diff(file) + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) if __name__ == '__main__': 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') diff --git a/wrap/tests/test_template_instantiator.py b/wrap/tests/test_template_instantiator.py new file mode 100644 index 000000000..4faf01aa9 --- /dev/null +++ b/wrap/tests/test_template_instantiator.py @@ -0,0 +1,606 @@ +""" +GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Tests for template_instantiator. + +Author: Varun Agrawal +""" + +# pylint: disable=import-error,wrong-import-position + +import os +import sys +import unittest + +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from gtwrap.interface_parser import (Argument, ArgumentList, Class, + Constructor, ForwardDeclaration, + GlobalFunction, Include, Method, + Namespace, ReturnType, StaticMethod, + Typename) +from gtwrap.template_instantiator import ( + InstantiatedClass, InstantiatedConstructor, InstantiatedDeclaration, + InstantiatedGlobalFunction, InstantiatedMethod, InstantiatedStaticMethod, + InstantiationHelper, instantiate_args_list, instantiate_name, + instantiate_namespace, instantiate_return_type, instantiate_type, + is_scoped_template) + + +class TestInstantiationHelper(unittest.TestCase): + """Tests for the InstantiationHelper class.""" + def test_constructor(self): + """Test constructor.""" + helper = InstantiationHelper(InstantiatedClass) + self.assertEqual(helper.instantiation_type, InstantiatedClass) + helper = InstantiationHelper(InstantiatedConstructor) + self.assertEqual(helper.instantiation_type, InstantiatedConstructor) + helper = InstantiationHelper(InstantiatedDeclaration) + self.assertEqual(helper.instantiation_type, InstantiatedDeclaration) + helper = InstantiationHelper(InstantiatedGlobalFunction) + self.assertEqual(helper.instantiation_type, InstantiatedGlobalFunction) + helper = InstantiationHelper(InstantiatedMethod) + self.assertEqual(helper.instantiation_type, InstantiatedMethod) + helper = InstantiationHelper(InstantiatedStaticMethod) + self.assertEqual(helper.instantiation_type, InstantiatedStaticMethod) + + def test_instantiate(self): + """Test instantiate method.""" + method = Method.rule.parseString(""" + template + double method(const T x, const U& param); + """)[0] + cls = Class.rule.parseString(""" + template + class Foo {}; + """)[0] + typenames = ['T', 'U'] + class_instantiations = [Typename.rule.parseString("string")[0]] + method_instantiations = [Typename.rule.parseString("double")[0]] + + parent = InstantiatedClass(cls, class_instantiations) + + helper = InstantiationHelper(InstantiatedMethod) + instantiated_methods = helper.instantiate([], method, typenames, + class_instantiations, + method_instantiations, + parent) + + self.assertEqual(len(instantiated_methods), 1) + args_list = instantiated_methods[0].args.list() + self.assertEqual(args_list[0].ctype.get_typename(), 'string') + self.assertEqual(args_list[1].ctype.get_typename(), 'double') + + def test_multilevel_instantiation(self): + """ + Test method for multilevel instantiation + i.e. instantiation at both the class and method level. + """ + cls = Class.rule.parseString(""" + template + class Foo { + template + double method1(const T x, const U& param); + + template + V method2(const T x); + }; + """)[0] + + typenames = ['T'] + class_instantiations = [Typename.rule.parseString("string")[0]] + parent = InstantiatedClass(cls, class_instantiations) + + helper = InstantiationHelper(InstantiatedMethod) + + instantiated_methods = helper.multilevel_instantiation( + cls.methods, typenames, parent) + self.assertEqual(len(instantiated_methods), 2) + self.assertEqual( + instantiated_methods[0].args.list()[0].ctype.get_typename(), + 'string') + self.assertEqual( + instantiated_methods[0].args.list()[1].ctype.get_typename(), + 'double') + self.assertEqual( + instantiated_methods[1].args.list()[0].ctype.get_typename(), + 'string') + self.assertEqual( + instantiated_methods[1].return_type.type1.get_typename(), 'int') + + +class TestInstantiatedGlobalFunction(unittest.TestCase): + """Tests for the InstantiatedGlobalFunction class.""" + def setUp(self): + original = GlobalFunction.rule.parseString(""" + template + R function(const T& x); + """)[0] + instantiations = [ + Typename.rule.parseString("int")[0], + Typename.rule.parseString("double")[0] + ] + self.func = InstantiatedGlobalFunction(original, instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.func, InstantiatedGlobalFunction) + self.assertIsInstance(self.func.original, GlobalFunction) + self.assertEqual(self.func.name, "functionIntDouble") + self.assertEqual(len(self.func.args.list()), 1) + self.assertEqual(self.func.args.list()[0].ctype.get_typename(), "int") + self.assertEqual(self.func.return_type.type1.get_typename(), "double") + + def test_to_cpp(self): + """Test to_cpp method.""" + actual = self.func.to_cpp() + self.assertEqual(actual, "function") + + +class TestInstantiatedConstructor(unittest.TestCase): + """Tests for the InstantiatedConstructor class.""" + def setUp(self): + constructor = Constructor.rule.parseString(""" + template + Class(C x, const U& param); + """)[0] + instantiations = [ + Typename.rule.parseString("double")[0], + Typename.rule.parseString("string")[0] + ] + self.constructor = InstantiatedConstructor(constructor, instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.constructor, InstantiatedConstructor) + self.assertIsInstance(self.constructor.original, Constructor) + + def test_construct(self): + """Test the construct classmethod.""" + constructor = Constructor.rule.parseString(""" + template + Class(C x, const U& param); + """)[0] + c = Class.rule.parseString(""" + template + class Class {}; + """)[0] + class_instantiations = [Typename.rule.parseString("double")[0]] + method_instantiations = [Typename.rule.parseString("string")[0]] + typenames = ['C', 'U'] + parent = InstantiatedClass(c, class_instantiations) + instantiated_args = instantiate_args_list( + constructor.args.list(), + typenames, class_instantiations + method_instantiations, + parent.cpp_typename()) + + instantiated_constructor = InstantiatedConstructor.construct( + constructor, typenames, class_instantiations, + method_instantiations, instantiated_args, parent) + self.assertEqual(instantiated_constructor.name, "ClassDouble") + self.assertEqual( + instantiated_constructor.args.list()[0].ctype.get_typename(), + "double") + self.assertEqual( + instantiated_constructor.args.list()[1].ctype.get_typename(), + "string") + + def test_to_cpp(self): + """Test the to_cpp method.""" + actual = self.constructor.to_cpp() + self.assertEqual(actual, "Class") + + +class TestInstantiatedMethod(unittest.TestCase): + """Tests for the InstantiatedMethod class.""" + def setUp(self): + method = Method.rule.parseString(""" + template + double method(const U& param); + """)[0] + instantiations = [Typename.rule.parseString("double")[0]] + self.method = InstantiatedMethod(method, instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.method, InstantiatedMethod) + self.assertIsInstance(self.method.original, Method) + self.assertEqual(self.method.name, "methodDouble") + + def test_construct(self): + """Test the construct classmethod.""" + method = Method.rule.parseString(""" + template + T method(U& param); + """)[0] + method_instantiations = [Typename.rule.parseString("double")[0]] + c = Class.rule.parseString(""" + template + class Class {}; + """)[0] + class_instantiations = [Typename.rule.parseString("string")[0]] + + typenames = ['T', 'U'] + parent = InstantiatedClass(c, class_instantiations) + instantiated_args = instantiate_args_list( + method.args.list(), + typenames, class_instantiations + method_instantiations, + parent.cpp_typename()) + + instantiated_method = InstantiatedMethod.construct( + method, typenames, class_instantiations, method_instantiations, + instantiated_args, parent) + self.assertEqual(instantiated_method.name, "methodDouble") + self.assertEqual( + instantiated_method.args.list()[0].ctype.get_typename(), "double") + self.assertEqual(instantiated_method.return_type.type1.get_typename(), + "string") + + def test_to_cpp(self): + """Test the to_cpp method.""" + actual = self.method.to_cpp() + self.assertEqual(actual, "method") + + +class TestInstantiatedStaticMethod(unittest.TestCase): + """Tests for the InstantiatedStaticMethod class.""" + def setUp(self): + static_method = StaticMethod.rule.parseString(""" + template + static T staticMethod(const U& param); + """)[0] + instantiations = [Typename.rule.parseString("double")[0]] + self.static_method = InstantiatedStaticMethod(static_method, + instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.static_method, InstantiatedStaticMethod) + self.assertIsInstance(self.static_method.original, StaticMethod) + self.assertEqual(self.static_method.name, "staticMethodDouble") + + def test_construct(self): + """Test the construct classmethod.""" + static_method = StaticMethod.rule.parseString(""" + template + static T staticMethod(U& param); + """)[0] + method_instantiations = [Typename.rule.parseString("double")[0]] + c = Class.rule.parseString(""" + template + class Class {}; + """)[0] + class_instantiations = [Typename.rule.parseString("string")[0]] + + typenames = ['T', 'U'] + parent = InstantiatedClass(c, class_instantiations) + instantiated_args = instantiate_args_list( + static_method.args.list(), + typenames, class_instantiations + method_instantiations, + parent.cpp_typename()) + + instantiated_static_method = InstantiatedStaticMethod.construct( + static_method, typenames, class_instantiations, + method_instantiations, instantiated_args, parent) + self.assertEqual(instantiated_static_method.name, "staticMethodDouble") + self.assertEqual( + instantiated_static_method.args.list()[0].ctype.get_typename(), + "double") + self.assertEqual( + instantiated_static_method.return_type.type1.get_typename(), + "string") + + def test_to_cpp(self): + """Test the to_cpp method.""" + actual = self.static_method.to_cpp() + self.assertEqual(actual, "staticMethod") + + +class TestInstantiatedClass(unittest.TestCase): + """Tests for the InstantiatedClass class.""" + def setUp(self): + cl = Class.rule.parseString(""" + template + class Foo { + template + Foo(C& c); + + template + static T staticMethod(const S& s); + + template + T method(const M& m); + + T operator*(T other) const; + + T prop; + }; + """)[0] + class_instantiations = [Typename.rule.parseString('string')[0]] + self.member_instantiations = [ + Typename.rule.parseString('int')[0], + Typename.rule.parseString('char')[0], + Typename.rule.parseString('double')[0], + ] + self.cl = InstantiatedClass(cl, class_instantiations) + self.typenames = self.cl.original.template.typenames + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.cl, InstantiatedClass) + self.assertIsInstance(self.cl.original, Class) + self.assertEqual(self.cl.name, "FooString") + + def test_instantiate_ctors(self): + """Test instantiate_ctors method.""" + ctors = self.cl.instantiate_ctors(self.typenames) + self.assertEqual(len(ctors), 1) + self.assertEqual(ctors[0].name, "FooString") + self.assertEqual(ctors[0].args.list()[0].ctype.get_typename(), "int") + + def test_instantiate_static_methods(self): + """Test instantiate_static_methods method.""" + static_methods = self.cl.instantiate_static_methods(self.typenames) + self.assertEqual(len(static_methods), 1) + self.assertEqual(static_methods[0].name, "staticMethodChar") + self.assertEqual(static_methods[0].args.list()[0].ctype.get_typename(), + "char") + self.assertEqual(static_methods[0].return_type.type1.get_typename(), + "string") + + def test_instantiate_methods(self): + """Test instantiate_methods method.""" + methods = self.cl.instantiate_methods(self.typenames) + self.assertEqual(len(methods), 1) + self.assertEqual(methods[0].name, "methodDouble") + self.assertEqual(methods[0].args.list()[0].ctype.get_typename(), + "double") + self.assertEqual(methods[0].return_type.type1.get_typename(), "string") + + def test_instantiate_operators(self): + """Test instantiate_operators method.""" + operators = self.cl.instantiate_operators(self.typenames) + self.assertEqual(len(operators), 1) + self.assertEqual(operators[0].operator, "*") + self.assertEqual(operators[0].args.list()[0].ctype.get_typename(), + "string") + self.assertEqual(operators[0].return_type.type1.get_typename(), + "string") + + def test_instantiate_properties(self): + """Test instantiate_properties method.""" + properties = self.cl.instantiate_properties(self.typenames) + self.assertEqual(len(properties), 1) + self.assertEqual(properties[0].name, "prop") + self.assertEqual(properties[0].ctype.get_typename(), "string") + + def test_cpp_typename(self): + """Test cpp_typename method.""" + actual = self.cl.cpp_typename() + self.assertEqual(actual.name, "Foo") + + def test_to_cpp(self): + """Test to_cpp method.""" + actual = self.cl.to_cpp() + self.assertEqual(actual, "Foo") + + +class TestInstantiatedDeclaration(unittest.TestCase): + """Tests for the InstantiatedDeclaration class.""" + def setUp(self): + #TODO(Varun) Need to support templated class forward declaration. + forward_declaration = ForwardDeclaration.rule.parseString(""" + class FooBar; + """)[0] + instantiations = [Typename.rule.parseString("double")[0]] + self.declaration = InstantiatedDeclaration( + forward_declaration, instantiations=instantiations) + + def test_constructor(self): + """Test constructor.""" + self.assertIsInstance(self.declaration, InstantiatedDeclaration) + self.assertIsInstance(self.declaration.original, ForwardDeclaration) + self.assertEqual(self.declaration.instantiations[0].name, "double") + + def test_to_cpp(self): + """Test to_cpp method.""" + cpp = self.declaration.to_cpp() + self.assertEqual(cpp, "FooBar") + + +class TestTemplateInstantiator(unittest.TestCase): + """ + Test overall template instantiation and the functions in the module. + """ + def test_scoped_template(self): + """Test is_scoped_template.""" + # Test if not scoped template. + template_typenames = ['T'] + str_arg_typename = "double" + scoped_template, scoped_idx = is_scoped_template( + template_typenames, str_arg_typename) + self.assertFalse(scoped_template) + self.assertEqual(scoped_idx, -1) + + # Check for correct template match. + template_typenames = ['T'] + str_arg_typename = "gtsam::Matrix" + scoped_template, scoped_idx = is_scoped_template( + template_typenames, str_arg_typename) + self.assertFalse(scoped_template) + self.assertEqual(scoped_idx, -1) + + # Test scoped templatte + template_typenames = ['T'] + str_arg_typename = "T::Value" + scoped_template, scoped_idx = is_scoped_template( + template_typenames, str_arg_typename) + self.assertEqual(scoped_template, "T") + self.assertEqual(scoped_idx, 0) + + template_typenames = ['U', 'T'] + str_arg_typename = "T::Value" + scoped_template, scoped_idx = is_scoped_template( + template_typenames, str_arg_typename) + self.assertEqual(scoped_template, "T") + self.assertEqual(scoped_idx, 1) + + def test_instantiate_type(self): + """Test for instantiate_type.""" + arg = Argument.rule.parseString("const T x")[0] + template_typenames = ["T"] + instantiations = [Typename.rule.parseString("double")[0]] + cpp_typename = "ExampleClass" + new_type = instantiate_type(arg.ctype, + template_typenames, + instantiations=instantiations, + cpp_typename=cpp_typename, + instantiated_class=None) + + new_typename = new_type.typename + self.assertEqual(new_typename.name, "double") + self.assertEqual(new_typename.instantiated_name(), "double") + + def test_instantiate_args_list(self): + """Test for instantiate_args_list.""" + args = ArgumentList.rule.parseString("T x, double y, string z")[0] + args_list = args.list() + template_typenames = ['T'] + instantiations = [Typename.rule.parseString("double")[0]] + instantiated_args_list = instantiate_args_list( + args_list, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + + self.assertEqual(instantiated_args_list[0].ctype.get_typename(), + "double") + + args = ArgumentList.rule.parseString("T x, U y, string z")[0] + args_list = args.list() + template_typenames = ['T', 'U'] + instantiations = [ + Typename.rule.parseString("double")[0], + Typename.rule.parseString("Matrix")[0] + ] + instantiated_args_list = instantiate_args_list( + args_list, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + self.assertEqual(instantiated_args_list[0].ctype.get_typename(), + "double") + self.assertEqual(instantiated_args_list[1].ctype.get_typename(), + "Matrix") + + args = ArgumentList.rule.parseString("T x, U y, T z")[0] + args_list = args.list() + template_typenames = ['T', 'U'] + instantiations = [ + Typename.rule.parseString("double")[0], + Typename.rule.parseString("Matrix")[0] + ] + instantiated_args_list = instantiate_args_list( + args_list, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + self.assertEqual(instantiated_args_list[0].ctype.get_typename(), + "double") + self.assertEqual(instantiated_args_list[1].ctype.get_typename(), + "Matrix") + self.assertEqual(instantiated_args_list[2].ctype.get_typename(), + "double") + + def test_instantiate_return_type(self): + """Test for instantiate_return_type.""" + return_type = ReturnType.rule.parseString("T")[0] + template_typenames = ['T'] + instantiations = [Typename.rule.parseString("double")[0]] + instantiated_return_type = instantiate_return_type( + return_type, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + + self.assertEqual(instantiated_return_type.type1.get_typename(), + "double") + + return_type = ReturnType.rule.parseString("pair")[0] + template_typenames = ['T', 'U'] + instantiations = [ + Typename.rule.parseString("double")[0], + Typename.rule.parseString("char")[0], + ] + instantiated_return_type = instantiate_return_type( + return_type, + template_typenames, + instantiations, + cpp_typename="ExampleClass") + + self.assertEqual(instantiated_return_type.type1.get_typename(), + "double") + self.assertEqual(instantiated_return_type.type2.get_typename(), "char") + + def test_instantiate_name(self): + """Test for instantiate_name.""" + instantiations = [Typename.rule.parseString("Man")[0]] + instantiated_name = instantiate_name("Iron", instantiations) + self.assertEqual(instantiated_name, "IronMan") + + def test_instantiate_namespace(self): + """Test for instantiate_namespace.""" + namespace = Namespace.rule.parseString(""" + namespace gtsam { + #include + template + class Values { + Values(const T& other); + + template + void insert(size_t j, V x); + + template + static S staticMethod(const S& s); + }; + } + """)[0] + instantiated_namespace = instantiate_namespace(namespace) + + self.assertEqual(instantiated_namespace.name, "gtsam") + self.assertEqual(instantiated_namespace.parent, "") + + self.assertEqual(instantiated_namespace.content[0].header, + "gtsam/nonlinear/Values.h") + self.assertIsInstance(instantiated_namespace.content[0], Include) + + self.assertEqual(instantiated_namespace.content[1].name, "ValuesBasis") + self.assertIsInstance(instantiated_namespace.content[1], Class) + + self.assertIsInstance(instantiated_namespace.content[1].ctors[0], + Constructor) + self.assertEqual(instantiated_namespace.content[1].ctors[0].name, + "ValuesBasis") + + self.assertIsInstance(instantiated_namespace.content[1].methods[0], + Method) + self.assertIsInstance(instantiated_namespace.content[1].methods[1], + Method) + self.assertEqual(instantiated_namespace.content[1].methods[0].name, + "insertVector") + self.assertEqual(instantiated_namespace.content[1].methods[1].name, + "insertMatrix") + + self.assertIsInstance( + instantiated_namespace.content[1].static_methods[0], StaticMethod) + self.assertEqual( + instantiated_namespace.content[1].static_methods[0].name, + "staticMethodDouble") + + +if __name__ == '__main__': + unittest.main()