12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'borglab:develop' into develop

release/4.3a0
roderick-koehle 2021-10-22 19:28:33 +02:00 committed by GitHub
parent ab92ba7075 791c7bdcab
commit 9440ff6cd8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
296 changed files with 17874 additions and 7127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.github/scripts/boost.sh vendored
View File

@ -9,10 +9,10 @@ tar -zxf ${BOOST_FOLDER}.tar.gz
# Bootstrap
cd ${BOOST_FOLDER}/
./bootstrap.sh
./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex
# Build and install
sudo ./b2 install
sudo ./b2 -j$(nproc) install
# Rebuild ld cache
sudo ldconfig

2
.github/scripts/python.sh vendored
View File

@ -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

16
.github/scripts/unix.sh vendored
View File

@ -68,6 +68,8 @@ function configure()
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \
-DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
-DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
-DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DBOOST_ROOT=$BOOST_ROOT \
-DBoost_NO_SYSTEM_PATHS=ON \
@ -92,7 +94,11 @@ function build ()
configure
make -j2
if [ "$(uname)" == "Linux" ]; then
make -j$(nproc)
elif [ "$(uname)" == "Darwin" ]; then
make -j$(sysctl -n hw.physicalcpu)
fi
finish
}
@ -105,8 +111,12 @@ function test ()
configure
# Actual build:
make -j2 check
# Actual testing
if [ "$(uname)" == "Linux" ]; then
make -j$(nproc) check
elif [ "$(uname)" == "Darwin" ]; then
make -j$(sysctl -n hw.physicalcpu) check
fi
finish
}

11
.github/workflows/build-linux.yml vendored
View File

@ -47,8 +47,7 @@ jobs:
- name: Checkout
uses: actions/checkout@v2
- name: Install (Linux)
if: runner.os == 'Linux'
- name: Install Dependencies
run: |
# LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
@ -63,7 +62,7 @@ jobs:
fi
sudo apt-get -y update
sudo apt-get install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@ -79,7 +78,5 @@ jobs:
run: |
bash .github/scripts/boost.sh
- name: Build and Test (Linux)
if: runner.os == 'Linux'
run: |
bash .github/scripts/unix.sh -t
- name: Build and Test
run: bash .github/scripts/unix.sh -t

9
.github/workflows/build-macos.yml vendored
View File

@ -34,8 +34,7 @@ jobs:
- name: Checkout
uses: actions/checkout@v2
- name: Install (macOS)
if: runner.os == 'macOS'
- name: Install Dependencies
run: |
brew install cmake ninja
brew install boost
@ -48,7 +47,5 @@ jobs:
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
fi
- name: Build and Test (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/unix.sh -t
- name: Build and Test
run: bash .github/scripts/unix.sh -t

30
.github/workflows/build-python.yml vendored
View File

@ -19,22 +19,20 @@ jobs:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
# ubuntu-18.04-gcc-5, #TODO Enable once the Python wrapper is optimized for memory
ubuntu-18.04-gcc-5,
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1,
# ubuntu-18.04-gcc-5-tbb,
ubuntu-18.04-gcc-5-tbb,
]
#TODO update wrapper to prevent OOM
# build_type: [Debug, Release]
build_type: [Release]
build_type: [Debug, Release]
python_version: [3]
include:
# - name: ubuntu-18.04-gcc-5
# os: ubuntu-18.04
# compiler: gcc
# version: "5"
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
compiler: gcc
version: "5"
- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
@ -46,7 +44,7 @@ jobs:
compiler: clang
version: "9"
#NOTE temporarily added this as it is a required check.
# NOTE temporarily added this as it is a required check.
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
compiler: clang
@ -59,11 +57,11 @@ jobs:
compiler: xcode
version: "11.3.1"
# - name: ubuntu-18.04-gcc-5-tbb
# os: ubuntu-18.04
# compiler: gcc
# version: "5"
# flag: tbb
- name: ubuntu-18.04-gcc-5-tbb
os: ubuntu-18.04
compiler: gcc
version: "5"
flag: tbb
steps:
- name: Checkout
@ -83,7 +81,7 @@ jobs:
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

14
.github/workflows/build-special.yml vendored
View File

@ -55,6 +55,12 @@ jobs:
version: "9"
flag: cayley
- name: ubuntu-system-libs
os: ubuntu-18.04
compiler: gcc
version: "9"
flag: system-libs
steps:
- name: Checkout
uses: actions/checkout@v2
@ -70,7 +76,7 @@ jobs:
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@ -126,6 +132,12 @@ jobs:
echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV
echo "GTSAM Uses Cayley map for Rot3"
- name: Use system versions of 3rd party libraries
if: matrix.flag == 'system'
run: |
echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV
echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV
- name: Build & Test
run: |
bash .github/scripts/unix.sh -t

61
.github/workflows/build-windows.yml vendored
View File

@ -12,42 +12,46 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
BOOST_VERSION: 1.72.0
BOOST_EXE: boost_1_72_0-msvc-14.2
strategy:
fail-fast: false
matrix:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
#TODO This build keeps timing out, need to understand why.
# windows-2016-cl,
windows-2019-cl,
]
#TODO This build fails, need to understand why.
# windows-2016-cl,
windows-2019-cl,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
#TODO This build keeps timing out, need to understand why.
#TODO This build fails, need to understand why.
# - name: windows-2016-cl
# os: windows-2016
# compiler: cl
# platform: 32
- name: windows-2019-cl
os: windows-2019
compiler: cl
platform: 64
steps:
- name: Checkout
uses: actions/checkout@v2
- name: Install (Windows)
if: runner.os == 'Windows'
- name: Install Dependencies
shell: powershell
run: |
Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh')
scoop install cmake --global # So we don't get issues with CMP0074 policy
scoop install ninja --global
if ("${{ matrix.compiler }}".StartsWith("clang")) {
scoop install llvm --global
}
if ("${{ matrix.compiler }}" -eq "gcc") {
# Chocolatey GCC is broken on the windows-2019 image.
# See: https://github.com/DaanDeMeyer/doctest/runs/231595515
@ -55,27 +59,38 @@ jobs:
scoop install gcc --global
echo "CC=gcc" >> $GITHUB_ENV
echo "CXX=g++" >> $GITHUB_ENV
} elseif ("${{ matrix.compiler }}" -eq "clang") {
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
} else {
echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV
echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV
echo "CC=${{ matrix.compiler }}" >> $env:GITHUB_ENV
echo "CXX=${{ matrix.compiler }}" >> $env:GITHUB_ENV
}
# Scoop modifies the PATH so we make the modified PATH global.
echo "$env:PATH" >> $GITHUB_PATH
echo "$env:PATH" >> $env:GITHUB_PATH
- name: Download and install Boost
uses: MarkusJx/install-boost@v1.0.1
id: install-boost
with:
boost_version: 1.72.0
toolset: msvc14.2
- name: Install Boost
shell: powershell
run: |
# Snippet from: https://github.com/actions/virtual-environments/issues/2667
$BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64"
- name: Build (Windows)
if: runner.os == 'Windows'
env:
BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }}
# Use the prebuilt binary for Windows
$Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe"
(New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe")
Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH"
# Set the BOOST_ROOT variable
echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV
- name: Checkout
uses: actions/checkout@v2
- name: Build
run: |
cmake -E remove_directory build
cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"

5
CMakeLists.txt
View File

@ -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

2
DEVELOP.md
View File

@ -17,3 +17,5 @@ class GTSAM_EXPORT MyClass { ... };
GTSAM_EXPORT myFunction();
```
More details [here](Using-GTSAM-EXPORT.md).

20
INSTALL.md
View File

@ -13,16 +13,18 @@ $ make install
## Important Installation Notes
1. GTSAM requires the following libraries to be installed on your system:
- BOOST version 1.58 or greater (install through Linux repositories or MacPorts)
- BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes).
- Cmake version 3.0 or higher
- Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
Optional dependent libraries:
- If TBB is installed and detectable by CMake GTSAM will use it automatically.
Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB,
disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB
may be installed from the Ubuntu repositories, and for other platforms it
may be downloaded from https://www.threadingbuildingblocks.org/
disable the CMake flag `GTSAM_WITH_TBB` (enabled by default) by providing
the argument `-DGTSAM_WITH_TBB=OFF` to `cmake`. On Ubuntu, TBB may be
installed from the Ubuntu repositories, and for other platforms it may be
downloaded from https://www.threadingbuildingblocks.org/
- GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
`GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
achieved with MKL disabled. We therefore advise you to benchmark your problem
@ -65,11 +67,15 @@ execute commands as follows for an out-of-source build:
This will build the library and unit tests, run all of the unit tests,
and then install the library itself.
## Boost Notes
Versions of Boost prior to 1.65 have a known bug that prevents proper "deep" serialization of objects, which means that objects encapsulated inside other objects don't get serialized.
This is particularly seen when using `clang` as the C++ compiler.
For this reason we require Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager.
## Known Issues
- When using `GTSAM_BUILD_WITH_MARCH_NATIVE=ON`, you may encounter issues in running tests which we are still investigating:
- Use of a version of GCC < 7.5 results in an "Indeterminant Linear System" error for `testSmartProjectionFactor`.
- Use of Boost version < 1.67 with clang will give a segfault for mulitple test cases.
- MSVC 2013 is not yet supported because it cannot build the serialization module of Boost 1.55 (or earlier).
# Windows Installation

12
README.md
View File

@ -40,7 +40,7 @@ $ make install
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.58 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [Boost](http://www.boost.org/users/download/) >= 1.65 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
@ -55,9 +55,9 @@ Optional prerequisites - used automatically if findable by CMake:
GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. It also introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect.
GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 to use the deprecated methods.
GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4` to use the deprecated methods.
GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag GTSAM_ALLOW_DEPRECATED_SINCE_V41 for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile.
GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V41` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile.
## Wrappers
@ -68,16 +68,16 @@ We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md)
GTSAM includes a state of the art IMU handling scheme based on
- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505)
- Todd Lupton and Salah Sukkarieh, _"Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions"_, TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505)
Our implementation improves on this using integration on the manifold, as detailed in
- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, _"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors"_, Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
If you are using the factor in academic work, please cite the publications above.
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag `GTSAM_TANGENT_PREINTEGRATION` to OFF.
## Additional Information

2
Using-GTSAM-EXPORT.md
View File

@ -10,7 +10,7 @@ To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and need
3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.)
## When is GTSAM_EXPORT being used incorrectly
Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library. For example, an error in gtsam/base will often show up when compiling the check_base_program or the MATLAB wrapper, but not when compiling/linking gtsam itself. The most common errors will say something like:
Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library. For example, an error in `gtsam/base` will often show up when compiling the `check_base_program` or the MATLAB wrapper, but not when compiling/linking gtsam itself. The most common errors will say something like:
```
Error LNK2019 unresolved external symbol "public: void __cdecl gtsam::SO3::print(class std::basic_string<char,struct std::char_traits<char>,class std::allocator<char> > const &)const " (?print@SO3@gtsam@@QEBAXAEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) referenced in function "public: static void __cdecl gtsam::Testable<class gtsam::SO3>::Print(class gtsam::SO3 const &,class std::basic_string<char,struct std::char_traits<char>,class std::allocator<char> > const &)" (?Print@?$Testable@VSO3@gtsam@@@gtsam@@SAXAEBVSO3@2@AEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) check_geometry_program C:\AFIT\lib\gtsam\build\gtsam\geometry\tests\testSO3.obj

16
cmake/FindTBB.cmake
View File

@ -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"

4
cmake/HandleBoost.cmake
View File

@ -22,7 +22,7 @@ endif()
# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
set(BOOST_FIND_MINIMUM_VERSION 1.58)
set(BOOST_FIND_MINIMUM_VERSION 1.65)
set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
@ -30,7 +30,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM
# Required components
if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.")
message(FATAL_ERROR "Missing required Boost components >= v1.65, please install/upgrade Boost or configure your search paths.")
endif()
option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)

2
cmake/HandleCPack.cmake
View File

@ -25,4 +25,4 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION
# Deb-package specific cpack
set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.65)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")

29
cmake/HandleGeneralOptions.cmake
View File

@ -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()

44
cmake/HandleMetis.cmake Normal file
View File

@ -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
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/include>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
$<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
)
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})

1
cmake/HandlePrintConfiguration.cmake
View File

@ -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})")

44
cmake/HandleTBB.cmake
View File

@ -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()

25
doc/CMakeLists.txt
View File

@ -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
)

64
docker/README.md
View File

@ -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.

2
docker/ubuntu-boost-tbb/build.sh
View File

@ -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 .

2
docker/ubuntu-gtsam-python-vnc/Dockerfile
View File

@ -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

2
docker/ubuntu-gtsam-python-vnc/build.sh
View File

@ -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 .

2
docker/ubuntu-gtsam-python-vnc/vnc.sh
View File

@ -2,4 +2,4 @@
docker run -it \
--workdir="/usr/src/gtsam" \
-p 5900:5900 \
dellaert/ubuntu-gtsam-python-vnc:bionic
borglab/ubuntu-gtsam-python-vnc:bionic

6
docker/ubuntu-gtsam-python/Dockerfile
View File

@ -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

2
docker/ubuntu-gtsam-python/build.sh
View File

@ -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 .

2
docker/ubuntu-gtsam/Dockerfile
View File

@ -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 && \

2
docker/ubuntu-gtsam/build.sh
View File

@ -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 .

569
examples/IMUKittiExampleGPS.cpp
View File

@ -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 <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <cstring>
#include <fstream>
@ -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<ImuMeasurement>& imu_measurements,
vector<GpsMeasurement>& 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<ImuMeasurement> imu_measurements;
vector<GpsMeasurement> gps_measurements;
loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
KittiCalibration kitti_calibration;
vector<ImuMeasurement> imu_measurements;
vector<GpsMeasurement> 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<PreintegratedImuMeasurements> current_summarized_measurement = nullptr;
std::shared_ptr<PreintegratedImuMeasurements> 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<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x);
new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);
new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(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<PreintegratedImuMeasurements>(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<PriorFactor<Pose3>>(
current_pose_key, current_pose_global, sigma_init_x);
new_factors.emplace_shared<PriorFactor<Vector3>>(
current_vel_key, current_velocity_global, sigma_init_v);
new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(
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<PreintegratedImuMeasurements>(imu_params,
current_bias);
new_factors.emplace_shared<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
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<BetweenFactor<imuBias::ConstantBias>>(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<PriorFactor<Pose3>>(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<Pose3>(current_pose_key);
current_velocity_global = result.at<Vector3>(current_vel_key);
current_bias = result.at<imuBias::ConstantBias>(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<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
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<BetweenFactor<imuBias::ConstantBias>>(
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<PriorFactor<Pose3>>(
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<Pose3>(current_pose_key);
current_velocity_global = result.at<Vector3>(current_vel_key);
current_bias = result.at<imuBias::ConstantBias>(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<Pose3>(pose_key);
auto velocity = result.at<Vector3>(vel_key);
auto bias = result.at<imuBias::ConstantBias>(bias_key);
auto pose = result.at<Pose3>(pose_key);
auto velocity = result.at<Vector3>(vel_key);
auto bias = result.at<imuBias::ConstantBias>(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);
}

6
examples/README.md
View File

@ -51,13 +51,13 @@ The directory **vSLAMexample** includes 2 simple examples using GTSAM:
See the separate README file there.
##Undirected Graphical Models (UGM)
## Undirected Graphical Models (UGM)
The best representation for a Markov Random Field is a factor graph :-) This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
can be found at <http://www.di.ens.fr/~mschmidt/Software/UGM>
##Building and Running
To build, cd into the directory and do:
## Building and Running
To build, cd into the top-level gtsam directory and do:
```
mkdir build

5
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -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)

14
gtsam/CMakeLists.txt
View File

@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Include>
$<INSTALL_INTERFACE:include/gtsam/3rdparty/CCOLAMD>
)
if(GTSAM_SUPPORT_NESTED_DISSECTION)
target_include_directories(gtsam BEFORE PUBLIC
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/include>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
$<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
)
endif()
if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
if (NOT BUILD_SHARED_LIBS)

25
gtsam/base/Lie.h
View File

@ -17,6 +17,7 @@
* @author Frank Dellaert
* @author Mike Bosse
* @author Duy Nguyen Ta
* @author Yotam Stern
*/
@ -319,12 +320,28 @@ T expm(const Vector& x, int K=7) {
}
/**
* Linear interpolation between X and Y by coefficient t in [0, 1].
* Linear interpolation between X and Y by coefficient t. Typically t \in [0,1],
* but can also be used to extrapolate before pose X or after pose Y.
*/
template <typename T>
T interpolate(const T& X, const T& Y, double t) {
assert(t >= 0 && t <= 1);
return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
T interpolate(const T& X, const T& Y, double t,
typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
if (Hx || Hy) {
typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
const T between =
traits<T>::Between(X, Y, between_H_x); // between_H_y = identity
typename traits<T>::TangentVector delta = traits<T>::Logmap(between, log_H);
const T Delta = traits<T>::Expmap(t * delta, exp_H);
const T result = traits<T>::Compose(
X, Delta, compose_H_x); // compose_H_xinv_y = identity
if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
if (Hy) *Hy = t * exp_H * log_H;
return result;
}
return traits<T>::Compose(
X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
}
/**

4
gtsam/base/Matrix.h
View File

@ -29,7 +29,7 @@
#include <gtsam/config.h>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <functional>
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
@ -489,7 +489,7 @@ struct MultiplyWithInverseFunction {
// The function phi should calculate f(a)*b, with derivatives in a and b.
// Naturally, the derivative in b is f(a).
typedef boost::function<VectorN(
typedef std::function<VectorN(
const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
Operator;

7
gtsam/base/OptionalJacobian.h
View File

@ -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

7
gtsam/base/Testable.h
View File

@ -34,8 +34,9 @@
#pragma once
#include <boost/concept_check.hpp>
#include <boost/shared_ptr.hpp>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#define GTSAM_PRINT(x)((x).print(#x))
@ -119,10 +120,10 @@ namespace gtsam {
* Binary predicate on shared pointers
*/
template<class V>
struct equals_star : public std::function<bool(const boost::shared_ptr<V>&, const boost::shared_ptr<V>&)> {
struct equals_star : public std::function<bool(const std::shared_ptr<V>&, const std::shared_ptr<V>&)> {
double tol_;
equals_star(double tol = 1e-9) : tol_(tol) {}
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
bool operator()(const std::shared_ptr<V>& expected, const std::shared_ptr<V>& actual) {
if (!actual && !expected) return true;
return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
}

108
gtsam/base/base.i Normal file
View File

@ -0,0 +1,108 @@
//*************************************************************************
// base
//*************************************************************************
namespace gtsam {
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/navigation/ImuBias.h>
// #####
#include <gtsam/base/debug.h>
bool isDebugVersion();
#include <gtsam/base/DSFMap.h>
class IndexPair {
IndexPair();
IndexPair(size_t i, size_t j);
size_t i() const;
size_t j() const;
};
template<KEY = {gtsam::IndexPair}>
class DSFMap {
DSFMap();
KEY find(const KEY& key) const;
void merge(const KEY& x, const KEY& y);
std::map<KEY, Set> sets();
};
class IndexPairSet {
IndexPairSet();
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
void insert(gtsam::IndexPair key);
bool erase(gtsam::IndexPair key); // returns true if value was removed
bool count(gtsam::IndexPair key) const; // returns true if value exists
};
class IndexPairVector {
IndexPairVector();
IndexPairVector(const gtsam::IndexPairVector& other);
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
gtsam::IndexPair at(size_t i) const;
void push_back(gtsam::IndexPair key) const;
};
gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
class IndexPairSetMap {
IndexPairSetMap();
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
gtsam::IndexPairSet at(gtsam::IndexPair& key);
};
#include <gtsam/base/Matrix.h>
bool linear_independent(Matrix A, Matrix B, double tol);
#include <gtsam/base/Value.h>
virtual class Value {
// No constructors because this is an abstract class
// Testable
void print(string s = "") const;
// Manifold
size_t dim() const;
};
#include <gtsam/base/GenericValue.h>
template <T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2,
gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2,
gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,
gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
} // namespace gtsam

2
gtsam/base/lieProxies.h
View File

@ -24,7 +24,7 @@
*
* These should not be used outside of tests, as they are just remappings
* of the original functions. We use these to avoid needing to do
* too much boost::bind magic or writing a bunch of separate proxy functions.
* too much std::bind magic or writing a bunch of separate proxy functions.
*
* Don't expect all classes to work for all of these functions.
*/

345
gtsam/base/numericalDerivative.h
View File

@ -18,8 +18,7 @@
// \callgraph
#pragma once
#include <boost/function.hpp>
#include <boost/bind/bind.hpp>
#include <functional>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
@ -38,13 +37,13 @@ namespace gtsam {
* for a function with one relevant param and an optional derivative:
* Foo bar(const Obj& a, boost::optional<Matrix&> H1)
* Use boost.bind to restructure:
* boost::bind(bar, boost::placeholders::_1, boost::none)
* std::bind(bar, std::placeholders::_1, boost::none)
* This syntax will fix the optional argument to boost::none, while using the first argument provided
*
* For member functions, such as below, with an instantiated copy instanceOfSomeClass
* Foo SomeClass::bar(const Obj& a)
* Use boost bind as follows to create a function pointer that uses the member function:
* boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), boost::placeholders::_1)
* std::bind(&SomeClass::bar, ref(instanceOfSomeClass), std::placeholders::_1)
*
* For additional details, see the documentation:
* http://www.boost.org/doc/libs/release/libs/bind/bind.html
@ -69,7 +68,7 @@ struct FixedSizeMatrix {
template <class X, int N = traits<X>::dimension>
typename Eigen::Matrix<double, N, 1> numericalGradient(
boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
std::function<double(const X&)> h, const X& x, double delta = 1e-5) {
double factor = 1.0 / (2.0 * delta);
BOOST_STATIC_ASSERT_MSG(
@ -109,7 +108,7 @@ typename Eigen::Matrix<double, N, 1> numericalGradient(
template <class Y, class X, int N = traits<X>::dimension>
// TODO Should compute fixed-size matrix
typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
std::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
@ -150,7 +149,7 @@ typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
template<class Y, class X>
typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
double delta = 1e-5) {
return numericalDerivative11<Y, X>(boost::bind(h, boost::placeholders::_1), x,
return numericalDerivative11<Y, X>(std::bind(h, std::placeholders::_1), x,
delta);
}
@ -164,14 +163,14 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const std::function<Y(const X1&, const X2&)>& h,
const X1& x1, const X2& x2, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2)), x1, delta);
std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
}
/** use a raw C++ function pointer */
@ -179,7 +178,7 @@ template<class Y, class X1, class X2>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative21<Y, X1, X2>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
delta);
}
@ -193,14 +192,14 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(cons
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(std::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
// "Template argument X1 must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1), x2, delta);
std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
}
/** use a raw C++ function pointer */
@ -208,7 +207,7 @@ template<class Y, class X1, class X2>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative22<Y, X1, X2>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
delta);
}
@ -225,14 +224,14 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(cons
*/
template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)),
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
x1, delta);
}
@ -240,8 +239,8 @@ template<class Y, class X1, class X2, class X3>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative31<Y, X1, X2, X3>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
@ -258,14 +257,14 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(cons
*/
template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)),
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
x2, delta);
}
@ -273,8 +272,8 @@ template<class Y, class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative32<Y, X1, X2, X3>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
@ -291,14 +290,14 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*
*/
template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1),
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
x3, delta);
}
@ -306,8 +305,8 @@ template<class Y, class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative33<Y, X1, X2, X3>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
@ -324,15 +323,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
boost::cref(x4)),
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4)),
x1, delta);
}
@ -340,8 +339,8 @@ template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative41<Y, X1, X2, X3, X4>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
@ -358,15 +357,15 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
boost::cref(x4)),
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4)),
x2, delta);
}
@ -374,8 +373,8 @@ template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative42<Y, X1, X2, X3, X4>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
@ -392,15 +391,15 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
boost::cref(x4)),
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4)),
x3, delta);
}
@ -408,8 +407,8 @@ template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative43<Y, X1, X2, X3, X4>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
@ -426,15 +425,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
"Template argument X4 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::placeholders::_1),
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1),
x4, delta);
}
@ -442,8 +441,8 @@ template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative44<Y, X1, X2, X3, X4>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
@ -461,15 +460,15 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::cref(x5)),
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5)),
x1, delta);
}
@ -477,9 +476,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative51<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
@ -497,15 +496,15 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
boost::cref(x4), boost::cref(x5)),
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4), std::cref(x5)),
x2, delta);
}
@ -513,9 +512,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
@ -533,15 +532,15 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
boost::cref(x4), boost::cref(x5)),
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4), std::cref(x5)),
x3, delta);
}
@ -549,9 +548,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative53<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
@ -569,15 +568,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::placeholders::_1, boost::cref(x5)),
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1, std::cref(x5)),
x4, delta);
}
@ -585,9 +584,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
@ -605,15 +604,15 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::placeholders::_1),
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::placeholders::_1),
x5, delta);
}
@ -621,9 +620,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
@ -642,15 +641,15 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::cref(x5), boost::cref(x6)),
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5), std::cref(x6)),
x1, delta);
}
@ -658,9 +657,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
@ -679,15 +678,15 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
boost::cref(x4), boost::cref(x5), boost::cref(x6)),
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4), std::cref(x5), std::cref(x6)),
x2, delta);
}
@ -695,9 +694,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
@ -716,15 +715,15 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
boost::cref(x4), boost::cref(x5), boost::cref(x6)),
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4), std::cref(x5), std::cref(x6)),
x3, delta);
}
@ -732,9 +731,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
@ -753,15 +752,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::placeholders::_1, boost::cref(x5), boost::cref(x6)),
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1, std::cref(x5), std::cref(x6)),
x4, delta);
}
@ -769,9 +768,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative64<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
@ -790,15 +789,15 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::placeholders::_1, boost::cref(x6)),
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::placeholders::_1, std::cref(x6)),
x5, delta);
}
@ -806,9 +805,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
@ -827,7 +826,7 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
@ -835,8 +834,8 @@ typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X6, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::cref(x5), boost::placeholders::_1),
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5), std::placeholders::_1),
x6, delta);
}
@ -844,9 +843,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
@ -859,22 +858,22 @@ inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*
* @return n*n Hessian matrix computed via central differencing
*/
template<class X>
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(std::function<double(const X&)> f, const X& x,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
typedef boost::function<double(const X&)> F;
typedef boost::function<VectorD(F, const X&, double)> G;
typedef std::function<double(const X&)> F;
typedef std::function<VectorD(F, const X&, double)> G;
G ng = static_cast<G>(numericalGradient<X> );
return numericalDerivative11<VectorD, X>(
boost::bind(ng, f, boost::placeholders::_1, delta), x, delta);
std::bind(ng, f, std::placeholders::_1, delta), x, delta);
}
template<class X>
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
1e-5) {
return numericalHessian(boost::function<double(const X&)>(f), x, delta);
return numericalHessian(std::function<double(const X&)>(f), x, delta);
}
/** Helper class that computes the derivative of f w.r.t. x1, centered about
@ -882,86 +881,86 @@ inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f
*/
template<class X1, class X2>
class G_x1 {
const boost::function<double(const X1&, const X2&)>& f_;
const std::function<double(const X1&, const X2&)>& f_;
X1 x1_;
double delta_;
public:
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
G_x1(const std::function<double(const X1&, const X2&)>& f, const X1& x1,
double delta) :
f_(f), x1_(x1), delta_(delta) {
}
Vector operator()(const X2& x2) {
return numericalGradient<X1>(
boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_);
std::bind(f_, std::placeholders::_1, std::cref(x2)), x1_, delta_);
}
};
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
G_x1<X1, X2> g_x1(f, x1, delta);
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(
boost::bind<Vector>(boost::ref(g_x1), boost::placeholders::_1)),
std::function<Vector(const X2&)>(
std::bind<Vector>(std::ref(g_x1), std::placeholders::_1)),
x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
return numericalHessian212(std::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(
boost::bind(f, boost::placeholders::_1, boost::cref(x2)));
std::function<double(const X1&)> f2(
std::bind(f, std::placeholders::_1, std::cref(x2)));
return numericalDerivative11<Vector, X1>(
boost::function<Vector(const X1&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
std::function<Vector(const X1&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x1, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
return numericalHessian211(std::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(
boost::bind(f, boost::cref(x1), boost::placeholders::_1));
std::function<double(const X2&)> f2(
std::bind(f, std::cref(x1), std::placeholders::_1));
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
std::function<Vector(const X2&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
return numericalHessian222(std::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
@ -971,17 +970,17 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(doubl
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(
f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)));
std::function<double(const X1&)> f2(std::bind(
f, std::placeholders::_1, std::cref(x2), std::cref(x3)));
return numericalDerivative11<Vector, X1>(
boost::function<Vector(const X1&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
std::function<Vector(const X1&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x1, delta);
}
@ -989,24 +988,24 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian311(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(
f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)));
std::function<double(const X2&)> f2(std::bind(
f, std::cref(x1), std::placeholders::_1, std::cref(x3)));
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
std::function<Vector(const X2&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x2, delta);
}
@ -1014,24 +1013,24 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian322(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X3>::type Vector;
Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
Vector (*numGrad)(std::function<double(const X3&)>, const X3&,
double) = &numericalGradient<X3>;
boost::function<double(const X3&)> f2(boost::bind(
f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1));
std::function<double(const X3&)> f2(std::bind(
f, std::cref(x1), std::cref(x2), std::placeholders::_1));
return numericalDerivative11<Vector, X3>(
boost::function<Vector(const X3&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
std::function<Vector(const X3&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x3, delta);
}
@ -1039,41 +1038,41 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian333(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X2>(
boost::function<double(const X1&, const X2&)>(
boost::bind(f, boost::placeholders::_1, boost::placeholders::_2,
boost::cref(x3))),
std::function<double(const X1&, const X2&)>(
std::bind(f, std::placeholders::_1, std::placeholders::_2,
std::cref(x3))),
x1, x2, delta);
}
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X3>(
boost::function<double(const X1&, const X3&)>(
boost::bind(f, boost::placeholders::_1, boost::cref(x2),
boost::placeholders::_2)),
std::function<double(const X1&, const X3&)>(
std::bind(f, std::placeholders::_1, std::cref(x2),
std::placeholders::_2)),
x1, x3, delta);
}
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X2, X3>(
boost::function<double(const X2&, const X3&)>(
boost::bind(f, boost::cref(x1), boost::placeholders::_1,
boost::placeholders::_2)),
std::function<double(const X2&, const X3&)>(
std::bind(f, std::cref(x1), std::placeholders::_1,
std::placeholders::_2)),
x2, x3, delta);
}
@ -1082,7 +1081,7 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian312(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
@ -1090,7 +1089,7 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian313(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
@ -1098,7 +1097,7 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian323(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}

64
gtsam/base/tests/testOptionalJacobian.cpp
View File

@ -24,40 +24,33 @@ using namespace std;
using namespace gtsam;
//******************************************************************************
#define TEST_CONSTRUCTOR(DIM1, DIM2, X, TRUTHY) \
{ \
OptionalJacobian<DIM1, DIM2> 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<Matrix&> 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));
}
}
//******************************************************************************

5
gtsam/base/treeTraversal-inst.h
View File

@ -158,8 +158,9 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
// Typedefs
typedef typename FOREST::Node Node;
internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
visitorPost, problemSizeThreshold);
tbb::task::spawn_root_and_wait(
internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
visitorPost, problemSizeThreshold));
#else
DepthFirstForest(forest, rootData, visitorPre, visitorPost);
#endif

70
gtsam/base/treeTraversal/parallelTraversalTasks.h
View File

@ -22,7 +22,7 @@
#include <boost/make_shared.hpp>
#ifdef GTSAM_USE_TBB
#include <tbb/task_group.h> // tbb::task_group
#include <tbb/task.h> // tbb::task, tbb::task_list
#include <tbb/scalable_allocator.h> // tbb::scalable_allocator
namespace gtsam {
@ -34,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */
template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
class PreOrderTask
class PreOrderTask : public tbb::task
{
public:
const boost::shared_ptr<NODE>& treeNode;
@ -42,30 +42,28 @@ namespace gtsam {
VISITOR_PRE& visitorPre;
VISITOR_POST& visitorPost;
int problemSizeThreshold;
tbb::task_group& tg;
bool makeNewTasks;
// Keep track of order phase across multiple calls to the same functor
mutable bool isPostOrderPhase;
bool isPostOrderPhase;
PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData,
VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold,
tbb::task_group& tg, bool makeNewTasks = true)
bool makeNewTasks = true)
: treeNode(treeNode),
myData(myData),
visitorPre(visitorPre),
visitorPost(visitorPost),
problemSizeThreshold(problemSizeThreshold),
tg(tg),
makeNewTasks(makeNewTasks),
isPostOrderPhase(false) {}
void operator()() const
tbb::task* execute() override
{
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
{
@ -73,10 +71,14 @@ namespace gtsam {
{
if(!treeNode->children.empty())
{
// Allocate post-order task as a continuation
isPostOrderPhase = true;
recycle_as_continuation();
bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold);
// If we have child tasks, start subtasks and wait for them to complete
tbb::task_group ctg;
tbb::task* firstChild = 0;
tbb::task_list childTasks;
for(const boost::shared_ptr<NODE>& child: treeNode->children)
{
// Process child in a subtask. Important: Run visitorPre before calling
@ -84,30 +86,37 @@ namespace gtsam {
// allocated an extra child, this causes a TBB error.
boost::shared_ptr<DATA> childData = boost::allocate_shared<DATA>(
tbb::scalable_allocator<DATA>(), visitorPre(child, *myData));
ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost,
problemSizeThreshold, ctg, overThreshold));
tbb::task* childTask =
new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost,
problemSizeThreshold, overThreshold);
if (firstChild)
childTasks.push_back(*childTask);
else
firstChild = childTask;
}
ctg.wait();
// Allocate post-order task as a continuation
isPostOrderPhase = true;
tg.run(*this);
// If we have child tasks, start subtasks and wait for them to complete
set_ref_count((int)treeNode->children.size());
spawn(childTasks);
return firstChild;
}
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>& node, DATA& myData) const
void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
{
for(const boost::shared_ptr<NODE>& child: node->children)
{
@ -122,7 +131,7 @@ namespace gtsam {
/* ************************************************************************* */
template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
class RootTask
class RootTask : public tbb::task
{
public:
const ROOTS& roots;
@ -130,31 +139,38 @@ 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, tbb::task_group& tg) :
int problemSizeThreshold) :
roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
problemSizeThreshold(problemSizeThreshold), tg(tg) {}
problemSizeThreshold(problemSizeThreshold) {}
void operator()() const
tbb::task* execute() override
{
typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
// Create data and tasks for our children
tbb::task_list tasks;
for(const boost::shared_ptr<NODE>& root: roots)
{
boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
tasks.push_back(*new(allocate_child())
PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold));
}
// Set TBB ref count
set_ref_count(1 + (int) roots.size());
// Spawn tasks
spawn_and_wait_for_all(tasks);
// Return nullptr
return nullptr;
}
};
template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>&
CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
{
typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask;
tbb::task_group tg;
tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
}
return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold);
}
}

29
gtsam/base/utilities.h Normal file
View File

@ -0,0 +1,29 @@
#pragma once
namespace gtsam {
/**
* For Python __str__().
* Redirect std cout to a string stream so we can return a string representation
* of an object when it prints to cout.
* https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string
*/
struct RedirectCout {
/// constructor -- redirect stdout buffer to a stringstream buffer
RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {}
/// return the string
std::string str() const {
return ssBuffer_.str();
}
/// destructor -- redirect stdout buffer to its original buffer
~RedirectCout() {
std::cout.rdbuf(coutBuffer_);
}
private:
std::stringstream ssBuffer_;
std::streambuf* coutBuffer_;
};
}

507
gtsam/basis/Basis.h Normal file
View File

@ -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 <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/basis/ParameterMatrix.h>
#include <iostream>
/**
* 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<DERIVED>`.
* 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<double, 1, -1>; /* 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 <size_t M>
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 <typename DERIVED>
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<M>.
* 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 <int M>
class VectorEvaluationFunctor : protected EvaluationFunctor {
protected:
using VectorM = Eigen::Matrix<double, M, 1>;
using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
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<M>(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<M>& P,
OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
if (H) *H = H_;
return P.matrix() * this->weights_.transpose();
}
/// c++ sugar
VectorM operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*MxN*/ -1, -1> 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<i<M.
*/
template <int M>
class VectorComponentFunctor : public EvaluationFunctor {
protected:
using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
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<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
if (H) *H = H_;
return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
}
/// c++ sugar
double operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
return apply(P, H);
}
};
/**
* Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
* 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 T>
class ManifoldEvaluationFunctor
: public VectorEvaluationFunctor<traits<T>::dimension> {
enum { M = traits<T>::dimension };
using Base = VectorEvaluationFunctor<M>;
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<M>& P,
OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
// Interpolate the M-dimensional vector to yield a vector in tangent space
Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
// Now call retract with this M-vector, possibly with derivatives
Eigen::Matrix<double, M, M> 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<M>& P,
OptionalJacobian</*MxN*/ -1, -1> 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</*1xN*/ -1, -1> H = boost::none) const {
if (H) *H = this->weights_;
return (this->weights_ * p)(0);
}
/// c++ sugar
double operator()(const typename DERIVED::Parameters& p,
OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
return apply(p, H); // might call apply in derived
}
};
/**
* VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
*
* 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 <int M>
class VectorDerivativeFunctor : protected DerivativeFunctorBase {
protected:
using VectorM = Eigen::Matrix<double, M, 1>;
using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
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<M>(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<M>& P,
OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
if (H) *H = H_;
return P.matrix() * this->weights_.transpose();
}
/// c++ sugar
VectorM operator()(
const ParameterMatrix<M>& P,
OptionalJacobian</*MxMN*/ -1, -1> 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<i<M.
*/
template <int M>
class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
protected:
using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
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<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
if (H) *H = H_;
return P.row(rowIndex_) * this->weights_.transpose();
}
/// c++ sugar
double operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
return apply(P, H);
}
};
// Vector version for MATLAB :-(
static double Derivative(double x, const Vector& p, //
OptionalJacobian</*1xN*/ -1, -1> H = boost::none) {
return DerivativeFunctor(x)(p.transpose(), H);
}
};
} // namespace gtsam

424
gtsam/basis/BasisFactors.h Normal file
View File

@ -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 <gtsam/basis/Basis.h>
#include <gtsam/nonlinear/FunctorizedFactor.h>
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 BASIS>
class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor<double, Vector> {
private:
using Base = FunctorizedFactor<double, Vector>;
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 BASIS, int M>
class GTSAM_EXPORT VectorEvaluationFactor
: public FunctorizedFactor<Vector, ParameterMatrix<M>> {
private:
using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
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<M>(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<M>(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<BASIS, P> controlPrior(key, measured, model,
* N, i, t, a, b);
* where N is the degree and i is the component index.
*/
template <class BASIS, size_t P>
class GTSAM_EXPORT VectorComponentFactor
: public FunctorizedFactor<double, ParameterMatrix<P>> {
private:
using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
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<P>(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<P>(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<Chebyshev2, Rot3> rotationFactor(key, measurement,
* model, N, x, a, b);
*
* where `x` is the value (e.g. timestep) at which the rotation was evaluated.
*/
template <class BASIS, typename T>
class GTSAM_EXPORT ManifoldEvaluationFactor
: public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
private:
using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::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<T>(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<T>(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 BASIS>
class GTSAM_EXPORT DerivativeFactor
: public FunctorizedFactor<double, typename BASIS::Parameters> {
private:
using Base = FunctorizedFactor<double, typename BASIS::Parameters>;
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 BASIS, int M>
class GTSAM_EXPORT VectorDerivativeFactor
: public FunctorizedFactor<Vector, ParameterMatrix<M>> {
private:
using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
using Func = typename BASIS::template VectorDerivativeFunctor<M>;
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 BASIS, int P>
class GTSAM_EXPORT ComponentDerivativeFactor
: public FunctorizedFactor<double, ParameterMatrix<P>> {
private:
using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
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

6
gtsam/basis/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
# Install headers
file(GLOB basis_headers "*.h")
install(FILES ${basis_headers} DESTINATION include/gtsam/basis)
# Build tests
add_subdirectory(tests)

98
gtsam/basis/Chebyshev.cpp Normal file
View File

@ -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 <gtsam/basis/Chebyshev.h>
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

109
gtsam/basis/Chebyshev.h Normal file
View File

@ -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 <gtsam/base/Manifold.h>
#include <gtsam/basis/Basis.h>
#include <unsupported/Eigen/KroneckerProduct>
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<Chebyshev1Basis> {
using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
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<Chebyshev2Basis> {
using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
/**
* 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

205
gtsam/basis/Chebyshev2.cpp Normal file
View File

@ -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 <gtsam/basis/Chebyshev2.h>
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

148
gtsam/basis/Chebyshev2.h Normal file
View File

@ -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 <gtsam/base/Manifold.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/basis/Basis.h>
#include <boost/function.hpp>
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<Chebyshev2> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using Base = Basis<Chebyshev2>;
using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
/// 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 <size_t M>
static Matrix matrix(boost::function<Eigen::Matrix<double, M, 1>(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

99
gtsam/basis/FitBasis.h Normal file
View File

@ -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 <gtsam/basis/Basis.h>
#include <gtsam/basis/BasisFactors.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam {
/// Our sequence representation is a map of {x: y} values where y = f(x)
using Sequence = std::map<double, double>;
/// A sample is a key-value pair from a sequence.
using Sample = std::pair<double, double>;
/**
* Class that does regression via least squares
* Example usage:
* size_t N = 3;
* auto fit = FitBasis<Chebyshev2>(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 Basis>
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<EvaluationFactor<Basis>>(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<Parameters>(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

112
gtsam/basis/Fourier.h Normal file
View File

@ -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 <gtsam/basis/Basis.h>
namespace gtsam {
/// Fourier basis
class GTSAM_EXPORT FourierBasis : public Basis<FourierBasis> {
public:
using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
/**
* @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

215
gtsam/basis/ParameterMatrix.h Normal file
View File

@ -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 <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/VectorSpace.h>
#include <iostream>
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 <int M>
class ParameterMatrix {
using MatrixType = Eigen::Matrix<double, M, -1>;
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<double, -1, M> transpose() const { return matrix_.transpose(); }
/**
* Get the matrix row specified by `index`.
* @param index: The row index to retrieve.
*/
Eigen::Matrix<double, 1, -1> 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<MatrixType, 1, -1, false> {
return matrix_.row(index);
}
/**
* Get the matrix column specified by `index`.
* @param index: The column index to retrieve.
*/
Eigen::Matrix<double, M, 1> 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<MatrixType, M, 1, true> {
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<M> operator+(const ParameterMatrix<M>& other) const {
return ParameterMatrix<M>(matrix_ + other.matrix());
}
/**
* Add a MxN-sized vector to the ParameterMatrix.
* @param other: Vector which is reshaped and added.
*/
ParameterMatrix<M> operator+(
const Eigen::Matrix<double, -1, 1>& other) const {
// This form avoids a deep copy and instead typecasts `other`.
Eigen::Map<const MatrixType> other_(other.data(), M, cols());
return ParameterMatrix<M>(matrix_ + other_);
}
/**
* Subtract a ParameterMatrix from another.
* @param other: ParameterMatrix to subtract.
*/
ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const {
return ParameterMatrix<M>(matrix_ - other.matrix());
}
/**
* Subtract a MxN-sized vector from the ParameterMatrix.
* @param other: Vector which is reshaped and subracted.
*/
ParameterMatrix<M> operator-(
const Eigen::Matrix<double, -1, 1>& other) const {
Eigen::Map<const MatrixType> other_(other.data(), M, cols());
return ParameterMatrix<M>(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<double, -1, -1>& 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<M>& 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<double, -1, -1, Eigen::RowMajor>;
Vector result(matrix_.size());
Eigen::Map<RowMajor>(&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 <int M>
struct traits<ParameterMatrix<M>>
: public internal::VectorSpace<ParameterMatrix<M>> {};
/* ************************************************************************* */
// Stream operator that takes a ParameterMatrix. Used for printing.
template <int M>
inline std::ostream& operator<<(std::ostream& os,
const ParameterMatrix<M>& parameterMatrix) {
os << parameterMatrix.matrix();
return os;
}
} // namespace gtsam

146
gtsam/basis/basis.i Normal file
View File

@ -0,0 +1,146 @@
//*************************************************************************
// basis
//*************************************************************************
namespace gtsam {
// TODO(gerry): add all the Functors to the Basis interfaces, e.g.
// `EvaluationFunctor`
#include <gtsam/basis/Fourier.h>
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 <gtsam/basis/Chebyshev.h>
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 <gtsam/basis/Chebyshev2.h>
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 <gtsam/basis/ParameterMatrix.h>
template <M = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}>
class ParameterMatrix {
ParameterMatrix(const size_t N);
ParameterMatrix(const Matrix& matrix);
Matrix matrix() const;
void print(const string& s = "") const;
};
#include <gtsam/basis/BasisFactors.h>
template <BASIS = {gtsam::Chebyshev2, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::FourierBasis}>
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 <BASIS, M>
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<gtsam::Chebyshev2, 3>
VectorEvaluationFactorChebyshev2D3;
typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 4>
VectorEvaluationFactorChebyshev2D4;
typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 12>
VectorEvaluationFactorChebyshev2D12;
template <BASIS, P>
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<gtsam::Chebyshev2, 3>
VectorComponentFactorChebyshev2D3;
typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 4>
VectorComponentFactorChebyshev2D4;
typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 12>
VectorComponentFactorChebyshev2D12;
template <BASIS, T>
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 <gtsam/basis/FitBasis.h>
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
class FitBasis {
FitBasis(const std::map<double, double>& sequence,
const gtsam::noiseModel::Base* model, size_t N);
static gtsam::NonlinearFactorGraph NonlinearGraph(
const std::map<double, double>& sequence,
const gtsam::noiseModel::Base* model, size_t N);
static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
const std::map<double, double>& sequence,
const gtsam::noiseModel::Base* model, size_t N);
Parameters parameters() const;
};
} // namespace gtsam

1
gtsam/basis/tests/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
gtsamAddTestsGlob(basis "test*.cpp" "" "gtsam")

236
gtsam/basis/tests/testChebyshev.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/basis/Chebyshev.h>
#include <gtsam/basis/FitBasis.h>
#include <gtsam/nonlinear/factorTesting.h>
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 <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
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<EvaluationFactor<Chebyshev1Basis>>(key, x(i), model, N,
t(i));
}
// Solve
Values initial;
initial.insert<Vector>(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<Vector>(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<Chebyshev1Basis> 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<double, double, double>(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<double, double, double>(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);
}
//******************************************************************************

435
gtsam/basis/tests/testChebyshev2.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/basis/Chebyshev2.h>
#include <gtsam/basis/FitBasis.h>
#include <gtsam/nonlinear/factorTesting.h>
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<double, /*2x2N*/ -1, -1> 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<Vector2(ParameterMatrix<2>)> f = boost::bind(
&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none);
Matrix numericalH =
numericalDerivative11<Vector2, ParameterMatrix<2>, 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<Chebyshev2> 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<double, -1, 1> 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<double, -1, 1> 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<double, -1, 1> 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<double, -1, 1> 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<double, double>(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<double, double>(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<M>;
VecD fx(N, x, 0, 3);
ParameterMatrix<M> X(N);
Matrix actualH(M, M * N);
EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
// Test Jacobian
Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix<M>, 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<M>;
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<M> 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<Vector1, ParameterMatrix<M>, 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<M>;
size_t row = 1;
CompFunc fx(N, row, x, 0, 3);
ParameterMatrix<M> X(N);
Matrix actualH(1, M * N);
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, ParameterMatrix<M>, 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);
}
//******************************************************************************

254
gtsam/basis/tests/testFourier.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/basis/FitBasis.h>
#include <gtsam/basis/Fourier.h>
#include <gtsam/nonlinear/factorTesting.h>
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<Vector>(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<FourierBasis> 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<JacobianFactor>(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<EvaluationFactor<FourierBasis>>(key, desiredValue,
model, 7, x);
}
// Solve FourierFactorGraph
Values values;
values.insert<Vector>(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<FourierBasis> 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<double, double>(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);
}
//******************************************************************************

145
gtsam/basis/tests/testParameterMatrix.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/basis/BasisFactors.h>
#include <gtsam/basis/Chebyshev2.h>
#include <gtsam/basis/ParameterMatrix.h>
#include <gtsam/inference/Symbol.h>
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<M> 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<M> 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<M> 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<M> 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<M> 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<M> params(Matrix::Ones(M, N));
ParameterMatrix<M> expected(2 * Matrix::Ones(M, N));
// Add vector
EXPECT(assert_equal(expected, params + Vector::Ones(M * N)));
// Add another ParameterMatrix
ParameterMatrix<M> actual = params + ParameterMatrix<M>(Matrix::Ones(M, N));
EXPECT(assert_equal(expected, actual));
}
//******************************************************************************
TEST(ParameterMatrix, Subtraction) {
ParameterMatrix<M> params(2 * Matrix::Ones(M, N));
ParameterMatrix<M> expected(Matrix::Ones(M, N));
// Subtract vector
EXPECT(assert_equal(expected, params - Vector::Ones(M * N)));
// Subtract another ParameterMatrix
ParameterMatrix<M> actual = params - ParameterMatrix<M>(Matrix::Ones(M, N));
EXPECT(assert_equal(expected, actual));
}
//******************************************************************************
TEST(ParameterMatrix, Multiplication) {
ParameterMatrix<M> 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<M> params(Matrix::Ones(M, N));
// vector
EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
// identity
EXPECT(assert_equal(ParameterMatrix<M>::identity(),
ParameterMatrix<M>(Matrix::Zero(M, 0))));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

6
gtsam/config.h.in
View File

@ -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

4
gtsam/discrete/DecisionTree-inl.h
View File

@ -450,7 +450,7 @@ namespace gtsam {
template<typename L, typename Y>
template<typename M, typename X>
DecisionTree<L, Y>::DecisionTree(const DecisionTree<M, X>& other,
const std::map<M, L>& map, boost::function<Y(const X&)> op) {
const std::map<M, L>& map, std::function<Y(const X&)> op) {
root_ = convert(other.root_, map, op);
}
@ -568,7 +568,7 @@ namespace gtsam {
template<typename M, typename X>
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::convert(
const typename DecisionTree<M, X>::NodePtr& f, const std::map<M, L>& map,
boost::function<Y(const X&)> op) {
std::function<Y(const X&)> op) {
typedef DecisionTree<M, X> MX;
typedef typename MX::Leaf MXLeaf;

12
gtsam/discrete/DecisionTree.h
View File

@ -20,10 +20,12 @@
#pragma once
#include <gtsam/discrete/Assignment.h>
#include <boost/function.hpp>
#include <functional>
#include <iostream>
#include <vector>
#include <map>
#include <vector>
namespace gtsam {
@ -38,8 +40,8 @@ namespace gtsam {
public:
/** Handy typedefs for unary and binary function types */
typedef boost::function<Y(const Y&)> Unary;
typedef boost::function<Y(const Y&, const Y&)> Binary;
typedef std::function<Y(const Y&)> Unary;
typedef std::function<Y(const Y&, const Y&)> Binary;
/** A label annotated with cardinality */
typedef std::pair<L,size_t> LabelC;
@ -107,7 +109,7 @@ namespace gtsam {
/** Convert to a different type */
template<typename M, typename X> NodePtr
convert(const typename DecisionTree<M, X>::NodePtr& f, const std::map<M,
L>& map, boost::function<Y(const X&)> op);
L>& map, std::function<Y(const X&)> op);
/** Default constructor */
DecisionTree();
@ -143,7 +145,7 @@ namespace gtsam {
/** Convert from a different type */
template<typename M, typename X>
DecisionTree(const DecisionTree<M, X>& other,
const std::map<M, L>& map, boost::function<Y(const X&)> op);
const std::map<M, L>& map, std::function<Y(const X&)> op);
/// @}
/// @name Testable

2
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) {
}
/** I can't get this to work !
class Mul: boost::function<double(const double&, const double&)> {
class Mul: std::function<double(const double&, const double&)> {
inline double operator()(const double& a, const double& b) {
return a * b;
}

2
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -196,7 +196,7 @@ TEST(DT, conversion)
map<string, Label> ordering;
ordering[A] = X;
ordering[B] = Y;
boost::function<bool(const int&)> op = convert;
std::function<bool(const int&)> op = convert;
BDT f2(f1, ordering, op);
// f1.print("f1");
// f2.print("f2");

14
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -106,11 +106,21 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
/* ************************************************************************* */
Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// initial gues just inverts the pinhole model
// Apply inverse camera matrix to map the pixel coordinate (u, v)
// of the equidistant fisheye image to angular coordinate space (xd, yd)
// with radius theta given in radians.
const double u = uv.x(), v = uv.y();
const double yd = (v - v0_) / fy_;
const double xd = (u - s_ * yd - u0_) / fx_;
Point2 pi(xd, yd);
const double theta = sqrt(xd * xd + yd * yd);
// Provide initial guess for the Gauss-Newton search.
// The angular coordinates given by (xd, yd) are mapped back to
// the focal plane of the perspective undistorted projection pi.
// See Cal3Unified.calibrate() using the same pattern for the
// undistortion of omnidirectional fisheye projection.
const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
Point2 pi(scale * xd, scale * yd);
// Perform newtons method, break when solution converges past tol_,
// throw exception if max iterations are reached

170
gtsam/geometry/CameraSet.h
View File

@ -147,51 +147,149 @@ public:
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* Fixed size version
*/
template<int N, int ND> // N = 2 or 3, ND is the camera dimension
static SymmetricBlockMatrix SchurComplement(
const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
*/
template <int N,
int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
static SymmetricBlockMatrix SchurComplement(
const std::vector<
Eigen::Matrix<double, ZDim, ND>,
Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& 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<DenseIndex> 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<DenseIndex> 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<double, ZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(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>(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<double, ZDim, ND>& Fj = Fs[j];
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* In this version, we allow for the case where the keys in the Jacobian are
* organized differently from the keys in the output SymmetricBlockMatrix In
* particular: each diagonal block of the Jacobian F captures 2 poses (useful
* for rolling shutter and extrinsic calibration) such that F keeps the block
* structure that makes the Schur complement trick fast.
*
* N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is
* the Hessian block dimension
*/
template <int N, int ND, int NDD>
static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(
const std::vector<
Eigen::Matrix<double, ZDim, ND>,
Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b,
const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
size_t nrNonuniqueKeys = jacobianKeys.size();
size_t nrUniqueKeys = hessianKeys.size();
// Marginalize point: note - we reuse the standard SchurComplement function.
SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs, E, P, b);
// Pack into an Hessian factor, allow space for b term.
std::vector<DenseIndex> dims(nrUniqueKeys + 1);
std::fill(dims.begin(), dims.end() - 1, NDD);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessianUniqueKeys;
// Deal with the fact that some blocks may share the same keys.
if (nrUniqueKeys == nrNonuniqueKeys) {
// Case when there is 1 calibration key per camera:
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix(augmentedHessian.selfadjointView()));
} else {
// When multiple cameras share a calibration we have to rearrange
// the results of the Schur complement matrix.
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // includes b
std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
nonuniqueDims.back() = 1;
augmentedHessian = SymmetricBlockMatrix(
nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
// Get map from key to location in the new augmented Hessian matrix (the
// one including only unique keys).
std::map<Key, size_t> keyToSlotMap;
for (size_t k = 0; k < nrUniqueKeys; k++) {
keyToSlotMap[hessianKeys[k]] = k;
}
// Initialize matrix to zero.
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1));
// Add contributions for each key: note this loops over the hessian with
// nonUnique keys (augmentedHessian) and populates an Hessian that only
// includes the unique keys (that is what we want to return).
for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
Key key_i = jacobianKeys.at(i);
// Update information vector.
augmentedHessianUniqueKeys.updateOffDiagonalBlock(
keyToSlotMap[key_i], nrUniqueKeys,
augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
// Update blocks.
for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
Key key_j = jacobianKeys.at(j);
if (i == j) {
augmentedHessianUniqueKeys.updateDiagonalBlock(
keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
} else { // (i < j)
if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
augmentedHessianUniqueKeys.updateOffDiagonalBlock(
keyToSlotMap[key_i], keyToSlotMap[key_j],
augmentedHessian.aboveDiagonalBlock(i, j));
} else {
augmentedHessianUniqueKeys.updateDiagonalBlock(
keyToSlotMap[key_i],
augmentedHessian.aboveDiagonalBlock(i, j) +
augmentedHessian.aboveDiagonalBlock(i, j).transpose());
}
}
}
} // end of for over cameras
}
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
// Update bottom right element of the matrix.
augmentedHessianUniqueKeys.updateDiagonalBlock(
nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
}
return augmentedHessianUniqueKeys;
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
@ -206,7 +304,7 @@ public:
}
/// Computes Point Covariance P, with lambda parameter
template<int N> // N = 2 or 3
template<int N> // N = 2 or 3 (point dimension)
static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
const Matrix& E, double lambda, bool diagonalDamping = false) {
@ -258,7 +356,7 @@ public:
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/
template<int N> // N = 2 or 3
template<int N> // N = 2 or 3 (point dimension)
static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
const Eigen::Matrix<double, N, N>& P, const Vector& b,
const KeyVector& allKeys, const KeyVector& keys,

4
gtsam/geometry/Cyclic.h
View File

@ -17,7 +17,9 @@
#include <gtsam/base/Group.h>
#include <gtsam/base/Testable.h>
#include <iostream> // for cout :-(
#include <cassert>
#include <iostream> // for cout :-(
namespace gtsam {

2
gtsam/geometry/PinholeCamera.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
* \nosubgrouping
*/
template<typename Calibration>
class PinholeCamera: public PinholeBaseK<Calibration> {
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
public:

2
gtsam/geometry/PinholePose.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* \nosubgrouping
*/
template<typename CALIBRATION>
class PinholeBaseK: public PinholeBase {
class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
private:

10
gtsam/geometry/Point2.h
View File

@ -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<Point2, Point2>;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
using Point2Pairs = std::vector<Point2Pair>;
/// 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<Point2, Point2> Point2Pair;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
// For MATLAB wrapper
typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;

2
gtsam/geometry/Pose3.h
View File

@ -142,7 +142,7 @@ public:
static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
/**
* Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
* 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

56
gtsam/geometry/SO3.cpp
View File

@ -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);
}

1
gtsam/geometry/SimpleCamera.h
View File

@ -21,6 +21,7 @@
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3_S2.h>

1029
gtsam/geometry/geometry.i Normal file
View File

File diff suppressed because it is too large Load Diff

7
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -20,12 +20,11 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -54,8 +53,8 @@ TEST(CalibratedCamera, Create) {
EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH)));
// Check derivative
boost::function<CalibratedCamera(Pose3)> f = //
boost::bind(CalibratedCamera::Create, _1, boost::none);
std::function<CalibratedCamera(Pose3)> f = //
std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

84
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -17,6 +17,7 @@
*/
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -125,6 +126,89 @@ TEST(CameraSet, Pinhole) {
EXPECT(assert_equal(actualE, E));
}
/* ************************************************************************* */
TEST(CameraSet, SchurComplementAndRearrangeBlocks) {
typedef PinholePose<Cal3Bundler> Camera;
typedef CameraSet<Camera> Set;
// this is the (block) Jacobian with respect to the nonuniqueKeys
std::vector<Eigen::Matrix<double, 2, 12>,
Eigen::aligned_allocator<Eigen::Matrix<double, 2, 12> > > Fs;
Fs.push_back(1 * Matrix::Ones(2, 12)); // corresponding to key pair (0,1)
Fs.push_back(2 * Matrix::Ones(2, 12)); // corresponding to key pair (1,2)
Fs.push_back(3 * Matrix::Ones(2, 12)); // corresponding to key pair (2,0)
Matrix E = 4 * Matrix::Identity(6, 3) + Matrix::Ones(6, 3);
E(0, 0) = 3;
E(1, 1) = 2;
E(2, 2) = 5;
Matrix Et = E.transpose();
Matrix P = (Et * E).inverse();
Vector b = 5 * Vector::Ones(6);
{ // SchurComplement
// Actual
SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3, 12>(Fs, E,
P, b);
Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
// Expected
Matrix F = Matrix::Zero(6, 3 * 12);
F.block<2, 12>(0, 0) = 1 * Matrix::Ones(2, 12);
F.block<2, 12>(2, 12) = 2 * Matrix::Ones(2, 12);
F.block<2, 12>(4, 24) = 3 * Matrix::Ones(2, 12);
Matrix Ft = F.transpose();
Matrix H = Ft * F - Ft * E * P * Et * F;
Vector v = Ft * (b - E * P * Et * b);
Matrix expectedAugmentedHessian = Matrix::Zero(3 * 12 + 1, 3 * 12 + 1);
expectedAugmentedHessian.block<36, 36>(0, 0) = H;
expectedAugmentedHessian.block<36, 1>(0, 36) = v;
expectedAugmentedHessian.block<1, 36>(36, 0) = v.transpose();
expectedAugmentedHessian(36, 36) = b.squaredNorm();
EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
}
{ // SchurComplementAndRearrangeBlocks
KeyVector nonuniqueKeys;
nonuniqueKeys.push_back(0);
nonuniqueKeys.push_back(1);
nonuniqueKeys.push_back(1);
nonuniqueKeys.push_back(2);
nonuniqueKeys.push_back(2);
nonuniqueKeys.push_back(0);
KeyVector uniqueKeys;
uniqueKeys.push_back(0);
uniqueKeys.push_back(1);
uniqueKeys.push_back(2);
// Actual
SymmetricBlockMatrix augmentedHessianBM =
Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(
Fs, E, P, b, nonuniqueKeys, uniqueKeys);
Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
// Expected
// we first need to build the Jacobian F according to unique keys
Matrix F = Matrix::Zero(6, 18);
F.block<2, 6>(0, 0) = Fs[0].block<2, 6>(0, 0);
F.block<2, 6>(0, 6) = Fs[0].block<2, 6>(0, 6);
F.block<2, 6>(2, 6) = Fs[1].block<2, 6>(0, 0);
F.block<2, 6>(2, 12) = Fs[1].block<2, 6>(0, 6);
F.block<2, 6>(4, 12) = Fs[2].block<2, 6>(0, 0);
F.block<2, 6>(4, 0) = Fs[2].block<2, 6>(0, 6);
Matrix Ft = F.transpose();
Vector v = Ft * (b - E * P * Et * b);
Matrix H = Ft * F - Ft * E * P * Et * F;
Matrix expectedAugmentedHessian(19, 19);
expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm();
EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
}
}
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
TEST(CameraSet, Stereo) {

27
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -5,16 +5,15 @@
* @date December 17, 2013
*/
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <sstream>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -42,15 +41,15 @@ TEST(EssentialMatrix, FromRotationAndDirection) {
1e-8));
Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none,
boost::none),
std::bind(EssentialMatrix::FromRotationAndDirection,
std::placeholders::_1, trueDirection, boost::none, boost::none),
trueRotation);
EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(
boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none,
boost::none),
trueDirection);
std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation,
std::placeholders::_1, boost::none, boost::none),
trueDirection);
EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
}
@ -176,7 +175,7 @@ TEST (EssentialMatrix, FromPose3_a) {
Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
EXPECT(assert_equal(expectedH, actualH, 1e-7));
}
@ -189,7 +188,7 @@ TEST (EssentialMatrix, FromPose3_b) {
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}

12
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -21,10 +21,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind/bind.hpp>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
using boost::none;
@ -138,8 +137,9 @@ TEST(OrientedPlane3, errorVector) {
Vector2(actual[0], actual[1])));
EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
std::bind(&OrientedPlane3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
@ -150,8 +150,8 @@ TEST(OrientedPlane3, errorVector) {
TEST(OrientedPlane3, jacobian_retract) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
Matrix33 H_actual;
boost::function<OrientedPlane3(const Vector3&)> f =
boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
std::function<OrientedPlane3(const Vector3&)> f = std::bind(
&OrientedPlane3::retract, plane, std::placeholders::_1, boost::none);
{
Vector3 v(-0.1, 0.2, 0.3);
plane.retract(v, H_actual);

12
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -22,13 +22,12 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <iostream>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -66,8 +65,9 @@ TEST(PinholeCamera, Create) {
EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
// Check derivative
boost::function<Camera(Pose3,Cal3_S2)> f = //
boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
std::function<Camera(Pose3, Cal3_S2)> f = //
std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@ -81,8 +81,8 @@ TEST(PinholeCamera, Pose) {
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

4
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) {
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose,_1,boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

22
gtsam/geometry/tests/testPoint3.cpp
View File

@ -14,14 +14,14 @@
* @brief Unit tests for Point3 class
*/
#include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point3.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <boost/function.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point3)
@ -101,7 +101,7 @@ TEST( Point3, dot) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<double(const Point3&, const Point3&)> f =
std::function<double(const Point3&, const Point3&)> f =
[](const Point3& p, const Point3& q) { return gtsam::dot(p, q); };
{
gtsam::dot(p, q, H1, H2);
@ -123,8 +123,9 @@ TEST( Point3, dot) {
/* ************************************************************************* */
TEST(Point3, cross) {
Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> f =
boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8);
cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
@ -142,8 +143,9 @@ TEST( Point3, cross2) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<Point3(const Point3&, const Point3&)> f = boost::bind(&gtsam::cross, _1, _2, //
boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
gtsam::cross(p, q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9));
@ -163,7 +165,7 @@ TEST (Point3, normalize) {
Point3 expected(point / sqrt(14.0));
EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Point3, Point3>(
boost::bind(gtsam::normalize, _1, boost::none), point);
std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

73
gtsam/geometry/tests/testPose3.cpp
View File

@ -22,8 +22,7 @@
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
#include <CppUnitLite/TestHarness.h>
#include <cmath>
@ -215,7 +214,7 @@ TEST(Pose3, translation) {
EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Point3, Pose3>(
boost::bind(&Pose3::translation, _1, boost::none), T);
std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -226,7 +225,7 @@ TEST(Pose3, rotation) {
EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Rot3, Pose3>(
boost::bind(&Pose3::rotation, _1, boost::none), T);
std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -1047,12 +1046,76 @@ TEST(Pose3, interpolate) {
EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t)));
}
/* ************************************************************************* */
Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); }
TEST(Pose3, interpolateJacobians) {
{
Pose3 X = Pose3::identity();
Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
double t = 0.5;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
}
{
Pose3 X = Pose3::identity();
Pose3 Y(Rot3::identity(), Point3(1, 0, 0));
double t = 0.3;
Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0));
Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
}
{
Pose3 X = Pose3::identity();
Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
double t = 0.5;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
}
{
Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
double t = 0.3;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
Matrix actualJacobianX, actualJacobianY;
interpolate(X, Y, t, actualJacobianX, actualJacobianY);
Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
}
}
/* ************************************************************************* */
TEST(Pose3, Create) {
Matrix63 actualH1, actualH2;
Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
EXPECT(assert_equal(T, actual));
boost::function<Pose3(Rot3,Point3)> create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none);
std::function<Pose3(Rot3, Point3)> create =
std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
}

45
gtsam/geometry/tests/testRot3.cpp
View File

@ -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<double>();
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
}
/* ************************************************************************* */

27
gtsam/geometry/tests/testSO3.cpp
View File

@ -15,15 +15,12 @@
* @author Frank Dellaert
**/
#include <gtsam/geometry/SO3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/SO3.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -211,7 +208,7 @@ TEST(SO3, ExpmapDerivative) {
TEST(SO3, ExpmapDerivative2) {
const Vector3 theta(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), theta);
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@ -222,7 +219,7 @@ TEST(SO3, ExpmapDerivative2) {
TEST(SO3, ExpmapDerivative3) {
const Vector3 theta(10, 20, 30);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), theta);
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@ -277,7 +274,7 @@ TEST(SO3, ExpmapDerivative5) {
TEST(SO3, ExpmapDerivative6) {
const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), thetahat);
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat);
Matrix3 Jactual;
SO3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
@ -288,7 +285,7 @@ TEST(SO3, LogmapDerivative) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
boost::bind(&SO3::Logmap, _1, boost::none), R);
std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual));
}
@ -298,7 +295,7 @@ TEST(SO3, JacobianLogmap) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
boost::bind(&SO3::Logmap, _1, boost::none), R);
std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
Matrix3 Jactual;
SO3::Logmap(R, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
@ -308,7 +305,7 @@ TEST(SO3, JacobianLogmap) {
TEST(SO3, ApplyDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
};
@ -331,7 +328,7 @@ TEST(SO3, ApplyDexp) {
TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
};
@ -357,7 +354,7 @@ TEST(SO3, vec) {
Matrix actualH;
const Vector9 actual = R2.vec(actualH);
CHECK(assert_equal(expected, actual));
boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
std::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
CHECK(assert_equal(numericalH, actualH));
}
@ -371,7 +368,7 @@ TEST(Matrix, compose) {
Matrix actualH;
const Matrix3 actual = so3::compose(M, R, actualH);
CHECK(assert_equal(expected, actual));
boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
std::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
return so3::compose(M, R);
};
Matrix numericalH = numericalDerivative11(f, M, 1e-2);

6
gtsam/geometry/tests/testSO4.cpp
View File

@ -166,7 +166,7 @@ TEST(SO4, vec) {
Matrix actualH;
const Vector16 actual = Q2.vec(actualH);
EXPECT(assert_equal(expected, actual));
boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
std::function<Vector16(const SO4&)> f = [](const SO4& Q) {
return Q.vec();
};
const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
@ -179,7 +179,7 @@ TEST(SO4, topLeft) {
Matrix actualH;
const Matrix3 actual = topLeft(Q3, actualH);
EXPECT(assert_equal(expected, actual));
boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
std::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
return topLeft(Q3);
};
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
@ -192,7 +192,7 @@ TEST(SO4, stiefel) {
Matrix actualH;
const Matrix43 actual = stiefel(Q3, actualH);
EXPECT(assert_equal(expected, actual));
boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
std::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
return stiefel(Q3);
};
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);

4
gtsam/geometry/tests/testSOn.cpp
View File

@ -189,7 +189,7 @@ Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); }
/// Test Jacobian of Retract at origin
TEST(SOn, RetractJacobian) {
Matrix actualH = RetractJacobian(3);
boost::function<Matrix(const Vector &)> h = [](const Vector &v) {
std::function<Matrix(const Vector &)> h = [](const Vector &v) {
return SOn::ChartAtOrigin::Retract(v).matrix();
};
Vector3 v;
@ -205,7 +205,7 @@ TEST(SOn, vec) {
SOn Q = SOn::ChartAtOrigin::Retract(v);
Matrix actualH;
const Vector actual = Q.vec(actualH);
boost::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
std::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
CHECK(assert_equal(H, actualH));
}

31
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -16,24 +16,22 @@
* @author Zhaoyang Lv
*/
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <functional>
#include <boost/bind/bind.hpp>
#include <boost/function.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
using symbol_shorthand::X;
@ -243,8 +241,9 @@ TEST(Similarity3, GroupAction) {
EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa)));
// Test derivative
boost::function<Point3(Similarity3, Point3)> f = boost::bind(
&Similarity3::transformFrom, _1, _2, boost::none, boost::none);
// Use lambda to resolve overloaded method
std::function<Point3(const Similarity3&, const Point3&)>
f = [](const Similarity3& S, const Point3& p){ return S.transformFrom(p); };
Point3 q(1, 2, 3);
for (const auto& T : { T1, T2, T3, T4, T5, T6 }) {

42
gtsam/geometry/tests/testUnit3.cpp
View File

@ -32,13 +32,12 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind/bind.hpp>
#include <cmath>
#include <random>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
using gtsam::symbol_shorthand::U;
@ -127,8 +126,9 @@ TEST(Unit3, dot) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2, //
boost::none, boost::none);
std::function<double(const Unit3&, const Unit3&)> f =
std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
p.dot(q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
@ -158,13 +158,13 @@ TEST(Unit3, error) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q);
std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q);
p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
{
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r);
std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r);
p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
@ -185,25 +185,33 @@ TEST(Unit3, error2) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, q);
p.errorVector(q, actual, boost::none);
EXPECT(assert_equal(expected, actual, 1e-5));
}
{
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, r);
p.errorVector(r, actual, boost::none);
EXPECT(assert_equal(expected, actual, 1e-5));
}
{
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, q);
p.errorVector(q, boost::none, actual);
EXPECT(assert_equal(expected, actual, 1e-5));
}
{
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, r);
p.errorVector(r, boost::none, actual);
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -221,13 +229,13 @@ TEST(Unit3, distance) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalGradient<Unit3>(
boost::bind(&Unit3::distance, &p, _1, boost::none), q);
std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q);
p.distance(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
{
expected = numericalGradient<Unit3>(
boost::bind(&Unit3::distance, &p, _1, boost::none), r);
std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r);
p.distance(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
@ -319,7 +327,7 @@ TEST(Unit3, basis) {
Matrix62 actualH;
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
boost::bind(BasisTest, _1, boost::none), p);
std::bind(BasisTest, std::placeholders::_1, boost::none), p);
// without H, first time
EXPECT(assert_equal(expected, p.basis(), 1e-6));
@ -348,7 +356,7 @@ TEST(Unit3, basis_derivatives) {
p.basis(actualH);
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
boost::bind(BasisTest, _1, boost::none), p);
std::bind(BasisTest, std::placeholders::_1, boost::none), p);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
}
@ -376,8 +384,8 @@ TEST(Unit3, retract) {
TEST (Unit3, jacobian_retract) {
Matrix22 H;
Unit3 p;
boost::function<Unit3(const Vector2&)> f =
boost::bind(&Unit3::retract, p, _1, boost::none);
std::function<Unit3(const Vector2&)> f =
std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none);
{
Vector2 v (-0.2, 0.1);
p.retract(v, H);
@ -440,7 +448,7 @@ TEST (Unit3, FromPoint3) {
Unit3 expected(point);
EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5));
Matrix expectedH = numericalDerivative11<Unit3, Point3>(
boost::bind(Unit3::FromPoint3, _1, boost::none), point);
std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}

64
gtsam/geometry/tests/testUtilities.cpp Normal file
View File

@ -0,0 +1,64 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* @file testUtilities.cpp
* @date Aug 19, 2021
* @author Varun Agrawal
* @brief Tests for the utilities.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/utilities.h>
using namespace gtsam;
using gtsam::symbol_shorthand::L;
using gtsam::symbol_shorthand::R;
using gtsam::symbol_shorthand::X;
/* ************************************************************************* */
TEST(Utilities, ExtractPoint2) {
Point2 p0(0, 0), p1(1, 0);
Values values;
values.insert<Point2>(L(0), p0);
values.insert<Point2>(L(1), p1);
values.insert<Rot3>(R(0), Rot3());
values.insert<Pose3>(X(0), Pose3());
Matrix all_points = utilities::extractPoint2(values);
EXPECT_LONGS_EQUAL(2, all_points.rows());
}
/* ************************************************************************* */
TEST(Utilities, ExtractPoint3) {
Point3 p0(0, 0, 0), p1(1, 0, 0);
Values values;
values.insert<Point3>(L(0), p0);
values.insert<Point3>(L(1), p1);
values.insert<Rot3>(R(0), Rot3());
values.insert<Pose3>(X(0), Pose3());
Matrix all_points = utilities::extractPoint3(values);
EXPECT_LONGS_EQUAL(2, all_points.rows());
}
/* ************************************************************************* */
int main() {
srand(time(nullptr));
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

10
gtsam/geometry/triangulation.h
View File

@ -18,14 +18,16 @@
#pragma once
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/TriangulationFactor.h>
namespace gtsam {
@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
} // \namespace gtsam

3514
gtsam/gtsam.i
View File

File diff suppressed because it is too large Load Diff

26
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -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

4
gtsam/inference/EliminateableFactorGraph.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <functional>
#include <boost/variant.hpp>
#include <boost/optional.hpp>
@ -86,7 +86,7 @@ namespace gtsam {
typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
/// The function type that does a single dense elimination step on a subgraph.
typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
typedef std::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
/// Typedef for an optional variable index as an argument to elimination functions
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;

30
gtsam/inference/Key.h
View File

@ -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<std::string(Key)> KeyFormatter;
using KeyFormatter = std::function<std::string(Key)>;
// 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<Key> KeyVector;
using KeyVector = FastVector<Key>;
// TODO(frank): Nothing fast about these :-(
typedef FastList<Key> KeyList;
typedef FastSet<Key> KeySet;
typedef FastMap<Key, int> KeyGroupMap;
using KeyList = FastList<Key>;
using KeySet = FastSet<Key>;
using KeyGroupMap = FastMap<Key, int>;
/// 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<typename T> struct traits;

45
gtsam/inference/LabeledSymbol.cpp
View File

@ -15,15 +15,12 @@
* @author: Alex Cunningham
*/
#include <iostream>
#include <boost/bind/bind.hpp>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/inference/LabeledSymbol.h>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
namespace gtsam {
using namespace std;
@ -109,17 +106,37 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const {
/* ************************************************************************* */
static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);}
boost::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c;
std::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
// Use lambda function to check equality
auto equals = [](unsigned char s, unsigned char c) { return s == c; };
return std::bind(
equals,
std::bind(&LabeledSymbol::chr, std::bind(make, std::placeholders::_1)),
c);
}
boost::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
return boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label;
std::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
// Use lambda function to check equality
auto equals = [](unsigned char s, unsigned char c) { return s == c; };
return std::bind(
equals,
std::bind(&LabeledSymbol::label, std::bind(make, std::placeholders::_1)),
label);
}
boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c &&
boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label;
std::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
// Use lambda functions for && and ==
auto logical_and = [](bool is_type, bool is_label) { return is_type == is_label; };
auto equals = [](unsigned char s, unsigned char c) { return s == c; };
return std::bind(logical_and,
std::bind(equals,
std::bind(&LabeledSymbol::chr,
std::bind(make, std::placeholders::_1)),
c),
std::bind(equals,
std::bind(&LabeledSymbol::label,
std::bind(make, std::placeholders::_1)),
label));
}
/* ************************************************************************* */

8
gtsam/inference/LabeledSymbol.h
View File

@ -19,8 +19,8 @@
#pragma once
#include <functional>
#include <gtsam/inference/Symbol.h>
#include <boost/function.hpp>
namespace gtsam {
@ -89,13 +89,13 @@ public:
*/
// Checks only the type
static boost::function<bool(gtsam::Key)> TypeTest(unsigned char c);
static std::function<bool(gtsam::Key)> TypeTest(unsigned char c);
// Checks only the robot ID (label_)
static boost::function<bool(gtsam::Key)> LabelTest(unsigned char label);
static std::function<bool(gtsam::Key)> LabelTest(unsigned char label);
// Checks both type and the robot ID
static boost::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
static std::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
// Converts to upper/lower versions of labels
LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }

4
gtsam/inference/Ordering.cpp
View File

@ -25,8 +25,12 @@
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
#ifdef GTSAM_USE_SYSTEM_METIS
#include <metis.h>
#else
#include <gtsam/3rdparty/metis/include/metis.h>
#endif
#endif
using namespace std;

7
gtsam/inference/Symbol.cpp
View File

@ -62,8 +62,11 @@ Symbol::operator std::string() const {
static Symbol make(gtsam::Key key) { return Symbol(key);}
boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
return boost::bind(&Symbol::chr, boost::bind(make, boost::placeholders::_1)) == c;
std::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
auto equals = [](unsigned char s, unsigned char c) { return s == c; };
return std::bind(
equals, std::bind(&Symbol::chr, std::bind(make, std::placeholders::_1)),
c);
}
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) {

Some files were not shown because too many files have changed in this diff Show More