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

Merge branch 'develop' into feature/system-metis-lib

release/4.3a0
Varun Agrawal 2021-08-26 00:25:41 -04:00
parent 8c7a404ace dc148edd5e
commit ad73645c83
638 changed files with 33862 additions and 17424 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3563
.cproject
View File

File diff suppressed because it is too large Load Diff

18
.github/scripts/boost.sh vendored Normal file
View File

@ -0,0 +1,18 @@
### Script to install Boost
BOOST_FOLDER=boost_${BOOST_VERSION//./_}
# Download Boost
wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
# Unzip
tar -zxf ${BOOST_FOLDER}.tar.gz
# Bootstrap
cd ${BOOST_FOLDER}/
./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex
# Build and install
sudo ./b2 -j$(nproc) install
# Rebuild ld cache
sudo ldconfig

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

@ -58,28 +58,29 @@ PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
BUILD_PYBIND="ON"
TYPEDEF_POINTS_TO_VECTORS="ON"
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
mkdir $GITHUB_WORKSPACE/build
cd $GITHUB_WORKSPACE/build
cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \
cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
-DGTSAM_BUILD_TESTS=OFF \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_USE_QUATERNIONS=OFF \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
-DGTSAM_TYPEDEF_POINTS_TO_VECTORS=${TYPEDEF_POINTS_TO_VECTORS} \
-DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \
-DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
make -j$(nproc) install
# Set to 2 cores so that Actions does not error out during resource provisioning.
make -j2 install
cd $GITHUB_WORKSPACE/build/python
$PYTHON setup.py install --user --prefix=

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

@ -92,7 +92,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 +109,12 @@ function test ()
configure
# Actual build:
make -j2 check
# Actual testing
if [ "$(uname)" == "Linux" ]; then
make -j$(nproc)
elif [ "$(uname)" == "Darwin" ]; then
make -j$(sysctl -n hw.physicalcpu)
fi
finish
}

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

@ -12,6 +12,7 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
BOOST_VERSION: 1.67.0
strategy:
fail-fast: false
@ -44,9 +45,9 @@ jobs:
steps:
- name: Checkout
uses: actions/checkout@master
- name: Install (Linux)
if: runner.os == 'Linux'
uses: actions/checkout@v2
- 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
@ -55,17 +56,14 @@ jobs:
# 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
# This key is not in the keystore by default for Ubuntu so we need to add it.
LLVM_KEY=15CF4D18AF4F7421
gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
gpg -a --export $LLVM_KEY | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
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
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
@ -75,11 +73,10 @@ jobs:
echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi
- name: Check Boost version
if: runner.os == 'Linux'
- name: Install Boost
run: |
echo "BOOST_ROOT = $BOOST_ROOT"
- name: Build and Test (Linux)
if: runner.os == 'Linux'
run: |
bash .github/scripts/unix.sh -t
bash .github/scripts/boost.sh
- name: Build and Test
run: bash .github/scripts/unix.sh -t

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

@ -12,6 +12,7 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
strategy:
fail-fast: false
matrix:
@ -31,13 +32,12 @@ jobs:
steps:
- name: Checkout
uses: actions/checkout@master
- name: Install (macOS)
if: runner.os == 'macOS'
uses: actions/checkout@v2
- name: Install Dependencies
run: |
brew tap ProfFan/robotics
brew install cmake ninja
brew install ProfFan/robotics/boost
brew install boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
@ -47,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

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

@ -12,6 +12,7 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
PYTHON_VERSION: ${{ matrix.python_version }}
strategy:
fail-fast: false
matrix:
@ -43,6 +44,14 @@ jobs:
compiler: clang
version: "9"
# NOTE temporarily added this as it is a required check.
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
compiler: clang
version: "9"
build_type: Debug
python_version: "3"
- name: macOS-10.15-xcode-11.3.1
os: macOS-10.15
compiler: xcode
@ -56,7 +65,7 @@ jobs:
steps:
- name: Checkout
uses: actions/checkout@master
uses: actions/checkout@v2
- name: Install (Linux)
if: runner.os == 'Linux'
run: |
@ -66,13 +75,13 @@ jobs:
# 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
# This key is not in the keystore by default for Ubuntu so we need to add it.
LLVM_KEY=15CF4D18AF4F7421
gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
gpg -a --export $LLVM_KEY | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
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
@ -88,7 +97,7 @@ jobs:
run: |
brew tap ProfFan/robotics
brew install cmake ninja
brew install ProfFan/robotics/boost
brew install boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV

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

@ -12,6 +12,7 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ON
BOOST_VERSION: 1.67.0
strategy:
fail-fast: false
@ -56,23 +57,20 @@ jobs:
steps:
- name: Checkout
uses: actions/checkout@master
uses: actions/checkout@v2
- name: Install (Linux)
if: runner.os == 'Linux'
run: |
# LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421
gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
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
@ -84,6 +82,11 @@ jobs:
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi
- name: Install Boost
if: runner.os == 'Linux'
run: |
bash .github/scripts/boost.sh
- name: Install (macOS)
if: runner.os == 'macOS'
run: |

60
.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@master
- 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,24 +59,44 @@ 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
- name: Build (Windows)
if: runner.os == 'Windows'
echo "$env:PATH" >> $env:GITHUB_PATH
- 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"
# 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
echo "BOOST_ROOT_1_72_0: ${env:BOOST_ROOT_1_72_0}"
cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT_1_72_0}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT_1_72_0}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT_1_72_0}\lib"
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"
cmake --build build --config ${{ matrix.build_type }} --target gtsam
cmake --build build --config ${{ matrix.build_type }} --target gtsam_unstable
cmake --build build --config ${{ matrix.build_type }} --target wrap
cmake --build build --config ${{ matrix.build_type }} --target check.base
cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable
cmake --build build --config ${{ matrix.build_type }} --target check.linear
cmake --build build --config ${{ matrix.build_type }} --target check.linear

26
CMakeLists.txt
View File

@ -34,7 +34,7 @@ include(GtsamTesting)
include(GtsamPrinting)
# guard against in-source builds
if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
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()
@ -63,12 +63,6 @@ include(cmake/HandleGlobalBuildFlags.cmake) # Build flags
# Build CppUnitLite
add_subdirectory(CppUnitLite)
# This is the new wrapper
if(GTSAM_BUILD_PYTHON)
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
add_subdirectory(python)
endif()
# Build GTSAM library
add_subdirectory(gtsam)
@ -86,8 +80,24 @@ if (GTSAM_BUILD_UNSTABLE)
add_subdirectory(gtsam_unstable)
endif()
# This is the new wrapper
if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
# Need to set this for the wrap package so we don't use the default value.
set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION}
CACHE STRING "The Python version to use for wrapping")
# Set the include directory for matlab.h
set(GTWRAP_INCLUDE_NAME "wrap")
add_subdirectory(wrap)
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
endif()
# Python toolbox
if(GTSAM_BUILD_PYTHON)
add_subdirectory(python)
endif()
# Matlab toolbox
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
if(GTSAM_INSTALL_MATLAB_TOOLBOX)
add_subdirectory(matlab)
endif()

2
DEVELOP.md
View File

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

52
INSTALL.md
View File

@ -13,16 +13,17 @@ $ 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)
- 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
@ -41,11 +42,6 @@ $ make install
- MacOS 10.6 - 10.14
- Windows 7, 8, 8.1, 10
Known issues:
- MSVC 2013 is not yet supported because it cannot build the serialization module
of Boost 1.55 (or earlier).
2. GTSAM makes extensive use of debug assertions, and we highly recommend you work
in Debug mode while developing (enabled by default). Likewise, it is imperative
that you switch to release mode when running finished code and for timing. GTSAM
@ -70,7 +66,41 @@ 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.
## CMake Configuration Options and Details
## Known Issues
- When using `GTSAM_BUILD_WITH_MARCH_NATIVE=ON`, you may encounter issues in running tests which we are still investigating:
- Use of a version of GCC < 7.5 results in an "Indeterminant Linear System" error for `testSmartProjectionFactor`.
- Use of Boost version < 1.65 with clang will give a segfault for mulitple test cases.
- MSVC 2013 is not yet supported because it cannot build the serialization module of Boost 1.55 (or earlier).
# Windows Installation
This section details how to build a GTSAM `.sln` file using Visual Studio.
### Prerequisites
- Visual Studio with C++ CMake tools for Windows
- All the other pre-requisites listed above.
### Steps
1. Open Visual Studio.
2. Select `Open a local folder` and select the GTSAM source directory.
3. Go to `Project -> CMake Settings`.
- (Optional) Set `Configuration name`.
- (Optional) Set `Configuration type`.
- Set the `Toolset` to `msvc_x64_x64`. If you know what toolset you require, then skip this step.
- Update the `Build root` to `${projectDir}\build\${name}`.
- You can optionally create a new configuration for a `Release` build.
- Set the necessary CMake variables for your use case.
- Click on `Show advanced settings`.
- For `CMake generator`, select a version which matches `Visual Studio <Version> <Year> Win64`, e.g. `Visual Studio 16 2019 Win64`.
- Save the settings (Ctrl + S).
4. Click on `Project -> Generate Cache`. This will generate the CMake build files (as seen in the Output window).
5. The last step will generate a `GTSAM.sln` file in the `build` directory. At this point, GTSAM can be used as a regular Visual Studio project.
# CMake Configuration Options and Details
GTSAM has a number of options that can be configured, which is best done with
one of the following:
@ -78,7 +108,7 @@ one of the following:
- ccmake the curses GUI for cmake
- cmake-gui a real GUI for cmake
### Important Options:
## Important Options:
#### CMAKE_BUILD_TYPE
We support several build configurations for GTSAM (case insensitive)

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

3
cmake/CMakeLists.txt
View File

@ -16,11 +16,8 @@ install(FILES
dllexport.h.in
GtsamBuildTypes.cmake
GtsamMakeConfigFile.cmake
GtsamMatlabWrap.cmake
GtsamTesting.cmake
GtsamPrinting.cmake
FindNumPy.cmake
README.html
DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")

4
cmake/Config.cmake.in
View File

@ -6,7 +6,7 @@
get_filename_component(OUR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
if(EXISTS "${OUR_CMAKE_DIR}/CMakeCache.txt")
# In build tree
set(@PACKAGE_NAME@_INCLUDE_DIR @CMAKE_SOURCE_DIR@ CACHE PATH "@PACKAGE_NAME@ include directory")
set(@PACKAGE_NAME@_INCLUDE_DIR @GTSAM_SOURCE_DIR@ CACHE PATH "@PACKAGE_NAME@ include directory")
else()
# Find installed library
set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory")
@ -15,7 +15,7 @@ endif()
# Find dependencies, required by cmake exported targets:
include(CMakeFindDependencyMacro)
# Allow using cmake < 3.8
if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
find_package(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
else()
find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)

7
cmake/FindTBB.cmake
View File

@ -99,11 +99,8 @@ if(NOT TBB_FOUND)
##################################
if(NOT DEFINED TBB_USE_DEBUG_BUILD)
if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug|RelWithDebInfo|RELWITHDEBINFO|relwithdebinfo)")
set(TBB_BUILD_TYPE DEBUG)
else()
set(TBB_BUILD_TYPE RELEASE)
endif()
# Set build type to RELEASE by default for optimization.
set(TBB_BUILD_TYPE RELEASE)
elseif(TBB_USE_DEBUG_BUILD)
set(TBB_BUILD_TYPE DEBUG)
else()

16
cmake/GtsamBuildTypes.cmake
View File

@ -59,10 +59,10 @@ endif()
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
# Define all cache variables, to be populated below depending on the OS/compiler:
set(GTSAM_COMPILE_OPTIONS_PRIVATE "" CACHE STRING "(Do not edit) Private compiler flags for all build configurations." FORCE)
set(GTSAM_COMPILE_OPTIONS_PUBLIC "" CACHE STRING "(Do not edit) Public compiler flags (exported to user projects) for all build configurations." FORCE)
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE "" CACHE STRING "(Do not edit) Private preprocessor macros for all build configurations." FORCE)
set(GTSAM_COMPILE_DEFINITIONS_PUBLIC "" CACHE STRING "(Do not edit) Public preprocessor macros for all build configurations." FORCE)
set(GTSAM_COMPILE_OPTIONS_PRIVATE "" CACHE INTERNAL "(Do not edit) Private compiler flags for all build configurations." FORCE)
set(GTSAM_COMPILE_OPTIONS_PUBLIC "" CACHE INTERNAL "(Do not edit) Public compiler flags (exported to user projects) for all build configurations." FORCE)
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE "" CACHE INTERNAL "(Do not edit) Private preprocessor macros for all build configurations." FORCE)
set(GTSAM_COMPILE_DEFINITIONS_PUBLIC "" CACHE INTERNAL "(Do not edit) Public preprocessor macros for all build configurations." FORCE)
mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE)
mark_as_advanced(GTSAM_COMPILE_OPTIONS_PUBLIC)
mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
@ -71,7 +71,7 @@ mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PUBLIC)
foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_toupper)
# Define empty cache variables for "public". "private" are creaed below.
# Define empty cache variables for "public". "private" are created below.
set(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper} "" CACHE STRING "(User editable) Public compiler flags (exported to user projects) for `${build_type_toupper}` configuration.")
set(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper} "" CACHE STRING "(User editable) Public preprocessor macros for `${build_type_toupper}` configuration.")
endforeach()
@ -204,9 +204,9 @@ endif()
# Make common binary output directory when on Windows
if(WIN32)
set(RUNTIME_OUTPUT_PATH "${CMAKE_BINARY_DIR}/bin")
set(EXECUTABLE_OUTPUT_PATH "${CMAKE_BINARY_DIR}/bin")
set(LIBRARY_OUTPUT_PATH "${CMAKE_BINARY_DIR}/lib")
set(RUNTIME_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
set(EXECUTABLE_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
set(LIBRARY_OUTPUT_PATH "${GTSAM_BINARY_DIR}/lib")
endif()
# Set up build type list for cmake-gui

421
cmake/GtsamMatlabWrap.cmake
View File

@ -1,421 +0,0 @@
# Set up cache options
option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF)
set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox")
if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
endif()
# GTSAM_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static
# are already compiled into the library by the linker
if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32)
message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
endif()
# Try to automatically configure mex path
if(APPLE)
file(GLOB matlab_bin_directories "/Applications/MATLAB*/bin")
set(mex_program_name "mex")
elseif(WIN32)
file(GLOB matlab_bin_directories "C:/Program Files*/MATLAB/*/bin")
set(mex_program_name "mex.bat")
else()
file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
set(mex_program_name "mex")
endif()
if(GTSAM_CUSTOM_MATLAB_PATH)
set(matlab_bin_directories ${GTSAM_CUSTOM_MATLAB_PATH})
endif()
# Run find_program explicitly putting $PATH after our predefined program
# directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
# on the system path.
list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred
find_program(MEX_COMMAND ${mex_program_name}
PATHS ${matlab_bin_directories} ENV PATH
NO_DEFAULT_PATH)
mark_as_advanced(FORCE MEX_COMMAND)
# Now that we have mex, trace back to find the Matlab installation root
get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
get_filename_component(mex_path "${MEX_COMMAND}" PATH)
if(mex_path MATCHES ".*/win64$")
get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
else()
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
endif()
set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
# User-friendly wrapping function. Builds a mex module from the provided
# interfaceHeader. For example, for the interface header gtsam.h,
# this will build the wrap module 'gtsam'.
#
# Arguments:
#
# interfaceHeader: The relative path to the wrapper interface definition file.
# linkLibraries: Any *additional* libraries to link. Your project library
# (e.g. `lba`), libraries it depends on, and any necessary
# MATLAB libraries will be linked automatically. So normally,
# leave this empty.
# extraIncludeDirs: Any *additional* include paths required by dependent
# libraries that have not already been added by
# include_directories. Again, normally, leave this empty.
# extraMexFlags: Any *additional* flags to pass to the compiler when building
# the wrap code. Normally, leave this empty.
function(wrap_and_install_library interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
wrap_library_internal("${interfaceHeader}" "${linkLibraries}" "${extraIncludeDirs}" "${mexFlags}")
install_wrapped_library_internal("${interfaceHeader}")
endfunction()
# Internal function that wraps a library and compiles the wrapper
function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
if(UNIX AND NOT APPLE)
if(CMAKE_SIZEOF_VOID_P EQUAL 8)
set(mexModuleExt mexa64)
else()
set(mexModuleExt mexglx)
endif()
elseif(APPLE)
set(mexModuleExt mexmaci64)
elseif(MSVC)
if(CMAKE_CL_64)
set(mexModuleExt mexw64)
else()
set(mexModuleExt mexw32)
endif()
endif()
# Wrap codegen interface
#usage: wrap interfacePath moduleName toolboxPath headerPath
# interfacePath : *absolute* path to directory of module interface file
# moduleName : the name of the module, interface file must be called moduleName.h
# toolboxPath : the directory in which to generate the wrappers
# headerPath : path to matlab.h
# Extract module name from interface header file name
get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE)
get_filename_component(modulePath "${interfaceHeader}" PATH)
get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
# Paths for generated files
set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp")
set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex")
message(STATUS "Building wrap module ${moduleName}")
# Find matlab.h in GTSAM
if(("${PROJECT_NAME}" STREQUAL "gtsam") OR
("${PROJECT_NAME}" STREQUAL "gtsam_unstable"))
set(matlab_h_path "${PROJECT_SOURCE_DIR}")
else()
if(NOT GTSAM_INCLUDE_DIR)
message(FATAL_ERROR "You must call find_package(GTSAM) before using wrap")
endif()
list(GET GTSAM_INCLUDE_DIR 0 installed_includes_path)
set(matlab_h_path "${installed_includes_path}/wrap")
endif()
# If building a static mex module, add all cmake-linked libraries to the
# explicit link libraries list so that the next block of code can unpack
# any static libraries
set(automaticDependencies "")
foreach(lib ${moduleName} ${linkLibraries})
#message("MODULE NAME: ${moduleName}")
if(TARGET "${lib}")
get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
# message("DEPENDENT LIBRARIES: ${dependentLibraries}")
if(dependentLibraries)
list(APPEND automaticDependencies ${dependentLibraries})
endif()
endif()
endforeach()
## CHRIS: Temporary fix. On my system the get_target_property above returned Not-found for gtsam module
## This needs to be fixed!!
if(UNIX AND NOT APPLE)
list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE}
${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE})
if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0
list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE})
if(GTSAM_MEX_BUILD_STATIC_MODULE)
#list(APPEND automaticDependencies -Wl,--no-as-needed -lrt)
endif()
endif()
endif()
#message("AUTOMATIC DEPENDENCIES: ${automaticDependencies}")
## CHRIS: End temporary fix
# Separate dependencies
set(correctedOtherLibraries "")
set(otherLibraryTargets "")
set(otherLibraryNontargets "")
set(otherSourcesAndObjects "")
foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
if(TARGET "${lib}")
if(GTSAM_MEX_BUILD_STATIC_MODULE)
get_target_property(target_sources ${lib} SOURCES)
list(APPEND otherSourcesAndObjects ${target_sources})
else()
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryTargets ${lib})
endif()
else()
get_filename_component(file_extension "${lib}" EXT)
get_filename_component(lib_name "${lib}" NAME_WE)
if(file_extension STREQUAL ".a" AND GTSAM_MEX_BUILD_STATIC_MODULE)
# For building a static MEX module, unpack the static library
# and compile its object files into our module
file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects")
execute_process(COMMAND ar -x "${lib}"
WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects"
RESULT_VARIABLE ar_result)
if(NOT ar_result EQUAL 0)
message(FATAL_ERROR "Failed extracting ${lib}")
endif()
# Get list of object files
execute_process(COMMAND ar -t "${lib}"
OUTPUT_VARIABLE object_files
RESULT_VARIABLE ar_result)
if(NOT ar_result EQUAL 0)
message(FATAL_ERROR "Failed listing ${lib}")
endif()
# Add directory to object files
string(REPLACE "\n" ";" object_files_list "${object_files}")
foreach(object_file ${object_files_list})
get_filename_component(file_extension "${object_file}" EXT)
if(file_extension STREQUAL ".o")
list(APPEND otherSourcesAndObjects "${generated_files_path}/${lib_name}_objects/${object_file}")
endif()
endforeach()
else()
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryNontargets ${lib})
endif()
endif()
endforeach()
# Check libraries for conflicting versions built-in to MATLAB
set(dependentLibraries "")
if(NOT "${otherLibraryTargets}" STREQUAL "")
foreach(target ${otherLibraryTargets})
get_target_property(dependentLibrariesOne ${target} INTERFACE_LINK_LIBRARIES)
list(APPEND dependentLibraries ${dependentLibrariesOne})
endforeach()
endif()
list(APPEND dependentLibraries ${otherLibraryNontargets})
check_conflicting_libraries_internal("${dependentLibraries}")
# Set up generation of module source file
file(MAKE_DIRECTORY "${generated_files_path}")
find_package(PythonInterp
${GTSAM_PYTHON_VERSION}
EXACT
REQUIRED)
find_package(PythonLibs
${GTSAM_PYTHON_VERSION}
EXACT
REQUIRED)
set(_ignore gtsam::Point2
gtsam::Point3)
add_custom_command(
OUTPUT ${generated_cpp_file}
DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
COMMAND
${PYTHON_EXECUTABLE}
${CMAKE_SOURCE_DIR}/wrap/matlab_wrapper.py
--src ${interfaceHeader}
--module_name ${moduleName}
--out ${generated_files_path}
--top_module_namespaces ${moduleName}
--ignore ${_ignore}
VERBATIM
WORKING_DIRECTORY ${generated_files_path})
# Set up building of mex module
string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}")
string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
add_library(${moduleName}_matlab_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects})
target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries})
target_link_libraries(${moduleName}_matlab_wrapper ${moduleName})
set_target_properties(${moduleName}_matlab_wrapper PROPERTIES
OUTPUT_NAME "${moduleName}_wrapper"
PREFIX ""
SUFFIX ".${mexModuleExt}"
LIBRARY_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
ARCHIVE_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
RUNTIME_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
CLEAN_DIRECT_OUTPUT 1)
set_property(TARGET ${moduleName}_matlab_wrapper APPEND_STRING PROPERTY COMPILE_FLAGS " ${extraMexFlagsSpaced} ${mexFlagsSpaced} \"-I${MATLAB_ROOT}/extern/include\" -DMATLAB_MEX_FILE -DMX_COMPAT_32")
set_property(TARGET ${moduleName}_matlab_wrapper APPEND PROPERTY INCLUDE_DIRECTORIES ${extraIncludeDirs})
# Disable build type postfixes for the mex module - we install in different directories for each build type instead
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
set_target_properties(${moduleName}_matlab_wrapper PROPERTIES ${build_type_upper}_POSTFIX "")
endforeach()
# Set up platform-specific flags
if(MSVC)
if(CMAKE_CL_64)
set(mxLibPath "${MATLAB_ROOT}/extern/lib/win64/microsoft")
else()
set(mxLibPath "${MATLAB_ROOT}/extern/lib/win32/microsoft")
endif()
target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.lib" "${mxLibPath}/libmx.lib" "${mxLibPath}/libmat.lib")
set_target_properties(${moduleName}_matlab_wrapper PROPERTIES LINK_FLAGS "/export:mexFunction")
set_property(SOURCE "${generated_cpp_file}" APPEND PROPERTY COMPILE_FLAGS "/bigobj")
elseif(APPLE)
set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.dylib" "${mxLibPath}/libmx.dylib" "${mxLibPath}/libmat.dylib")
endif()
# Hacking around output issue with custom command
# Deletes generated build folder
add_custom_target(wrap_${moduleName}_matlab_distclean
COMMAND cmake -E remove_directory ${generated_files_path}
COMMAND cmake -E remove_directory ${compiled_mex_modules_root})
endfunction()
# Internal function that installs a wrap toolbox
function(install_wrapped_library_internal interfaceHeader)
get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
# NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash on the directory name
# here prevents creating the top-level module name directory in the destination.
message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}")
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING PATTERN "*.m")
install(TARGETS ${moduleName}_matlab_wrapper
LIBRARY DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}"
RUNTIME DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}")
endforeach()
else()
install(DIRECTORY "${generated_files_path}/" DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH} FILES_MATCHING PATTERN "*.m")
install(TARGETS ${moduleName}_matlab_wrapper
LIBRARY DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}
RUNTIME DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH})
endif()
endfunction()
# Internal function to check for libraries installed with MATLAB that may conflict
# and prints a warning to move them if problems occur.
function(check_conflicting_libraries_internal libraries)
if(UNIX)
# Set path for matlab's built-in libraries
if(APPLE)
set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
else()
if(CMAKE_CL_64)
set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64")
else()
set(mxLibPath "${MATLAB_ROOT}/bin/glnx86")
endif()
endif()
# List matlab's built-in libraries
file(GLOB matlabLibs RELATIVE "${mxLibPath}" "${mxLibPath}/lib*")
# Convert to base names
set(matlabLibNames "")
foreach(lib ${matlabLibs})
get_filename_component(libName "${lib}" NAME_WE)
list(APPEND matlabLibNames "${libName}")
endforeach()
# Get names of link libraries
set(linkLibNames "")
foreach(lib ${libraries})
string(FIND "${lib}" "/" slashPos)
if(NOT slashPos EQUAL -1)
# If the name is a path, just get the library name
get_filename_component(libName "${lib}" NAME_WE)
list(APPEND linkLibNames "${libName}")
else()
# It's not a path, so see if it looks like a filename
get_filename_component(ext "${lib}" EXT)
if(NOT "${ext}" STREQUAL "")
# It's a filename, so get the base name
get_filename_component(libName "${lib}" NAME_WE)
list(APPEND linkLibNames "${libName}")
else()
# It's not a filename so it must be a short name, add the "lib" prefix
list(APPEND linkLibNames "lib${lib}")
endif()
endif()
endforeach()
# Remove duplicates
list(REMOVE_DUPLICATES linkLibNames)
set(conflictingLibs "")
foreach(lib ${linkLibNames})
list(FIND matlabLibNames "${lib}" libPos)
if(NOT libPos EQUAL -1)
if(NOT conflictingLibs STREQUAL "")
set(conflictingLibs "${conflictingLibs}, ")
endif()
set(conflictingLibs "${conflictingLibs}${lib}")
endif()
endforeach()
if(NOT "${conflictingLibs}" STREQUAL "")
message(WARNING "GTSAM links to the libraries [ ${conflictingLibs} ] on your system, but "
"MATLAB is distributed with its own versions of these libraries which may conflict. "
"If you get strange errors or crashes with the GTSAM MATLAB wrapper, move these "
"libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on "
"your system. MATLAB will usually still work with these libraries moved away, but "
"if not, you'll have to compile the static GTSAM MATLAB wrapper module.")
endif()
endif()
endfunction()
# Helper function to install MATLAB scripts and handle multiple build types where the scripts
# should be installed to all build type toolboxes
function(install_matlab_scripts source_directory patterns)
set(patterns_args "")
set(exclude_patterns "")
if(NOT GTSAM_WRAP_SERIALIZATION)
set(exclude_patterns "testSerialization.m")
endif()
foreach(pattern ${patterns})
list(APPEND patterns_args PATTERN "${pattern}")
endforeach()
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endforeach()
else()
install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endif()
endfunction()

50
cmake/GtsamTesting.cmake
View File

@ -88,36 +88,36 @@ enable_testing()
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON)
option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF)
option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF)
# Add option for combining unit tests
if(MSVC OR XCODE_VERSION)
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
else()
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
endif()
mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
# Add option for combining unit tests
if(MSVC OR XCODE_VERSION)
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
else()
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
endif()
mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
if(GTSAM_BUILD_TESTS)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
# Also add alternative checks using valgrind.
# We don't look for valgrind being installed in the system, since these
# targets are not invoked unless directly instructed by the user.
if (UNIX)
# Run all tests using valgrind:
add_custom_target(check_valgrind)
endif()
# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
if(GTSAM_BUILD_TESTS)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
# Also add alternative checks using valgrind.
# We don't look for valgrind being installed in the system, since these
# targets are not invoked unless directly instructed by the user.
if (UNIX)
# Run all tests using valgrind:
add_custom_target(check_valgrind)
endif()
# Add target to build tests without running
add_custom_target(all.tests)
endif()
# Add target to build tests without running
add_custom_target(all.tests)
endif()
# Add examples target
add_custom_target(examples)
# Add examples target
add_custom_target(examples)
# Add timing target
add_custom_target(timing)
# Add timing target
add_custom_target(timing)
# Implementations of this file's macros:

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

2
cmake/HandleEigen.cmake
View File

@ -42,7 +42,7 @@ else()
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
# The actual include directory (for BUILD cmake target interface):
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
endif()
# Detect Eigen version:

14
cmake/HandleGeneralOptions.cmake
View File

@ -24,6 +24,7 @@ option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available"
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)
@ -40,16 +41,5 @@ elseif(GTSAM_ROT3_EXPMAP)
set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE)
endif()
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
# Set the default Python version. This is later updated in HandlePython.cmake.
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
# Check / set dependent variables for MATLAB wrapper
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
endif()

50
cmake/HandlePython.cmake
View File

@ -1,22 +1,48 @@
# Set Python version if either Python or MATLAB wrapper is requested.
if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
# Get info about the Python3 interpreter
# https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
find_package(Python3 COMPONENTS Interpreter Development)
if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
if(NOT ${Python3_FOUND})
message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.")
endif()
if(${CMAKE_VERSION} VERSION_LESS "3.12.0")
# Use older version of cmake's find_python
find_package(PythonInterp)
if(NOT ${PYTHONINTERP_FOUND})
message(
FATAL_ERROR
"Cannot find Python interpreter. Please install Python >= 3.6.")
endif()
find_package(PythonLibs ${PYTHON_VERSION_STRING})
set(Python_VERSION_MAJOR ${PYTHON_VERSION_MAJOR})
set(Python_VERSION_MINOR ${PYTHON_VERSION_MINOR})
set(Python_EXECUTABLE ${PYTHON_EXECUTABLE})
else()
# Get info about the Python3 interpreter
# https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
find_package(Python3 COMPONENTS Interpreter Development)
if(NOT ${Python3_FOUND})
message(
FATAL_ERROR
"Cannot find Python3 interpreter. Please install Python >= 3.6.")
endif()
set(Python_VERSION_MAJOR ${Python3_VERSION_MAJOR})
set(Python_VERSION_MINOR ${Python3_VERSION_MINOR})
set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}"
CACHE
STRING
"The version of Python to build the wrappers against."
FORCE)
endif()
set(GTSAM_PYTHON_VERSION
"${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}"
CACHE STRING "The version of Python to build the wrappers against."
FORCE)
endif()
endif()
# Check for build of Unstable modules
if(GTSAM_BUILD_PYTHON)
if(GTSAM_UNSTABLE_BUILD_PYTHON)
if (NOT GTSAM_BUILD_UNSTABLE)

10
cmake/HandleUninstall.cmake
View File

@ -6,5 +6,11 @@ configure_file(
"${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
IMMEDIATE @ONLY)
add_custom_target(uninstall
"${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
if (NOT TARGET uninstall) # avoid duplicating this target
add_custom_target(uninstall
"${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
else()
add_custom_target(uninstall_gtsam
"${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
add_dependencies(uninstall uninstall_gtsam)
endif()

26
cmake/README.md
View File

@ -67,32 +67,6 @@ Defines two useful functions for creating CTest unit tests. Also immediately cr
an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to.
## GtsamMatlabWrap
include(GtsamMatlabWrap)
Defines functions for generating MATLAB wrappers. Also immediately creates several CMake options for configuring the wrapper.
* `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper.
Usage example:
`wrap_and_install_library("lba.h" "" "" "")`
Arguments:
interfaceHeader: The relative or absolute path to the wrapper interface
definition file.
linkLibraries: Any *additional* libraries to link. Your project library
(e.g. `lba`), libraries it depends on, and any necessary
MATLAB libraries will be linked automatically. So normally,
leave this empty.
extraIncludeDirs: Any *additional* include paths required by dependent
libraries that have not already been added by
include_directories. Again, normally, leave this empty.
extraMexFlags: Any *additional* flags to pass to the compiler when building
the wrap code. Normally, leave this empty.
## GtsamMakeConfigFile
include(GtsamMakeConfigFile)

49
cmake/example_project/CMakeLists.txt
View File

@ -1,49 +0,0 @@
# This file should be used as a template for creating new projects using the CMake tools
# This project has the following features
# - GTSAM linking
# - Unit tests via CppUnitLite
# - Scripts
# - Automatic MATLAB wrapper generation
###################################################################################
# To create your own project, replace "example" with the actual name of your project
cmake_minimum_required(VERSION 3.0)
project(example CXX C)
# Include GTSAM CMake tools
find_package(GTSAMCMakeTools)
include(GtsamBuildTypes) # Load build type flags and default to Debug mode
include(GtsamTesting) # Easy functions for creating unit tests and scripts
include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation
# Ensure that local folder is searched before library folders
include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
###################################################################################
# Find GTSAM components
find_package(GTSAM REQUIRED) # Uses installed package
# Note: Since Jan-2019, GTSAMConfig.cmake defines exported CMake targets
# that automatically do include the include_directories() without the need
# to call include_directories(), just target_link_libraries(NAME gtsam)
#include_directories(${GTSAM_INCLUDE_DIR})
###################################################################################
# Build static library from common sources
set(CONVENIENCE_LIB_NAME ${PROJECT_NAME})
add_library(${CONVENIENCE_LIB_NAME} SHARED example/PrintExamples.h example/PrintExamples.cpp)
target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam)
# Install library
install(TARGETS ${CONVENIENCE_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
###################################################################################
# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}")
###################################################################################
# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}")
###################################################################################
# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library)
wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "")

32
cmake/example_project/README.md
View File

@ -1,32 +0,0 @@
# MATLAB Wrapper Example Project
This project serves as a lightweight example for demonstrating how to wrap C++ code in MATLAB using GTSAM.
## Compiling
We follow the regular build procedure inside the `example_project` directory:
```sh
mkdir build && cd build
cmake ..
make -j8
sudo make install
sudo ldconfig # ensures the shared object file generated is correctly loaded
```
## Usage
Now you can open MATLAB and add the `gtsam_toolbox` to the MATLAB path
```matlab
addpath('/usr/local/gtsam_toolbox')
```
At this point you are ready to run the example project. Starting from the `example_project` directory inside MATLAB, simply run code like regular MATLAB, e.g.
```matlab
pe = example.PrintExamples();
pe.sayHello();
pe.sayGoodbye();
```

23
cmake/example_project/SayGoodbye.cpp
View File

@ -1,23 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SayGoodbye.cpp
* @brief Example script for example project
* @author Richard Roberts
*/
#include <example/PrintExamples.h>
int main(int argc, char *argv[]) {
example::PrintExamples().sayGoodbye();
return 0;
}

23
cmake/example_project/SayHello.cpp
View File

@ -1,23 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SayHello.cpp
* @brief Example script for example project
* @author Richard Roberts
*/
#include <example/PrintExamples.h>
int main(int argc, char *argv[]) {
example::PrintExamples().sayHello();
return 0;
}

31
cmake/example_project/example.h
View File

@ -1,31 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 example.h
* @brief Example wrapper interface file
* @author Richard Roberts
*/
// This is an interface file for automatic MATLAB wrapper generation. See
// gtsam.h for full documentation and more examples.
#include <example/PrintExamples.h>
namespace example {
class PrintExamples {
PrintExamples();
void sayHello() const;
void sayGoodbye() const;
};
}

44
cmake/example_project/example/PrintExamples.cpp
View File

@ -1,44 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 print_examples.cpp
* @brief Example library file
* @author Richard Roberts
*/
#include <iostream>
#include <example/PrintExamples.h>
namespace example {
void PrintExamples::sayHello() const {
std::cout << internal::getHelloString() << std::endl;
}
void PrintExamples::sayGoodbye() const {
std::cout << internal::getGoodbyeString() << std::endl;
}
namespace internal {
std::string getHelloString() {
return "Hello!";
}
std::string getGoodbyeString() {
return "See you soon!";
}
} // namespace internal
} // namespace example

42
cmake/example_project/example/PrintExamples.h
View File

@ -1,42 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 print_examples.h
* @brief Example library file
* @author Richard Roberts
*/
#pragma once
#include <iostream>
#include <string>
namespace example {
class PrintExamples {
public:
/// Print a greeting
void sayHello() const;
/// Print a farewell
void sayGoodbye() const;
};
namespace internal {
std::string getHelloString();
std::string getGoodbyeString();
} // namespace internal
} // namespace example

43
cmake/example_project/tests/testExample.cpp
View File

@ -1,43 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testExample.cpp
* @brief Unit tests for example
* @author Richard Roberts
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <example/PrintExamples.h>
using namespace gtsam;
TEST(Example, HelloString) {
const std::string expectedString = "Hello!";
EXPECT(assert_equal(expectedString, example::internal::getHelloString()));
}
TEST(Example, GoodbyeString) {
const std::string expectedString = "See you soon!";
EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString()));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

5
doc/Code/LocalizationFactor.cpp
View File

@ -8,7 +8,10 @@ public:
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const
{
if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
const Rot2& R = q.rotation();
if (H) (*H) = (gtsam::Matrix(2, 3) <<
R.c(), -R.s(), 0.0,
R.s(), R.c(), 0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
};

2
doc/LieGroups.lyx
View File

@ -2452,7 +2452,7 @@ First transform
, and then rotate back:
\begin_inset Formula
\[
q=Re^{\Skew{\omega}}R^{T}
q=Re^{\Skew{\omega}}R^{T}p
\]
\end_inset

BIN
doc/LieGroups.pdf
View File

Binary file not shown.

34
doc/gtsam.lyx
View File

@ -1,7 +1,9 @@
#LyX 2.1 created this file. For more info see http://www.lyx.org/
\lyxformat 474
#LyX 2.2 created this file. For more info see http://www.lyx.org/
\lyxformat 508
\begin_document
\begin_header
\save_transient_properties true
\origin unavailable
\textclass article
\begin_preamble
\usepackage{times}
@ -50,16 +52,16 @@
\language_package default
\inputencoding auto
\fontencoding T1
\font_roman ae
\font_sans default
\font_typewriter default
\font_math auto
\font_roman "ae" "default"
\font_sans "default" "default"
\font_typewriter "default" "default"
\font_math "auto" "auto"
\font_default_family rmdefault
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\font_sf_scale 100 100
\font_tt_scale 100 100
\graphics default
\default_output_format default
\output_sync 0
@ -1061,7 +1063,7 @@ noindent
\begin_layout Subsection
\begin_inset CommandInset label
LatexCommand label
name "sub:Full-Posterior-Inference"
name "subsec:Full-Posterior-Inference"
\end_inset
@ -1272,7 +1274,7 @@ ing to odometry measurements.
(see Section
\begin_inset CommandInset ref
LatexCommand ref
reference "sub:Full-Posterior-Inference"
reference "subsec:Full-Posterior-Inference"
\end_inset
@ -1469,7 +1471,7 @@ GPS-like
\begin_inset CommandInset include
LatexCommand lstinputlisting
filename "Code/LocalizationFactor.cpp"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/LocalizationExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:LocalizationFactor},language={C++},numbers=left"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={C++},numbers=left,caption={Excerpt from examples/LocalizationExample.cpp},label={listing:LocalizationFactor}"
\end_inset
@ -1590,15 +1592,15 @@ q_{y}
\begin_inset Formula $2\times3$
\end_inset
matrix:
matrix in tangent space which is the same the as the rotation matrix:
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
H=\left[\begin{array}{ccc}
1 & 0 & 0\\
0 & 1 & 0
\cos(q_{\theta}) & -\sin(q_{\theta}) & 0\\
\sin(q_{\theta}) & \cos(q_{\theta}) & 0
\end{array}\right]
\]
@ -1750,7 +1752,7 @@ global
The marginals can be recovered exactly as in Section
\begin_inset CommandInset ref
LatexCommand ref
reference "sub:Full-Posterior-Inference"
reference "subsec:Full-Posterior-Inference"
\end_inset
@ -1770,7 +1772,7 @@ filename "Code/LocalizationOutput5.txt"
Comparing this with the covariance matrices in Section
\begin_inset CommandInset ref
LatexCommand ref
reference "sub:Full-Posterior-Inference"
reference "subsec:Full-Posterior-Inference"
\end_inset

BIN
doc/gtsam.pdf
View File

Binary file not shown.

3
examples/CombinedImuFactorsExample.cpp
View File

@ -23,7 +23,8 @@
* A row starting with "i" is the first initial position formatted with
* N, E, D, qx, qY, qZ, qW, velN, velE, velD
* A row starting with "0" is an imu measurement
* linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
* (body frame - Forward, Right, Down)
* linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
* A row starting with "1" is a gps correction formatted with
* N, E, D, qX, qY, qZ, qW
* Note that for GPS correction, we're only using the position not the

63
examples/CreateSFMExampleData.cpp
View File

@ -15,8 +15,8 @@
* @author Frank Dellaert
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/slam/dataset.h>
#include <boost/assign/std/vector.hpp>
@ -26,22 +26,16 @@ using namespace gtsam;
/* ************************************************************************* */
void createExampleBALFile(const string& filename, const vector<Point3>& P,
const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
Cal3Bundler()) {
void createExampleBALFile(const string& filename, const vector<Point3>& points,
const Pose3& pose1, const Pose3& pose2,
const Cal3Bundler& K = Cal3Bundler()) {
// Class that will gather all data
SfmData data;
// Create two cameras
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 identity, aPb(aRb, aTb);
// Create two cameras and add them to data
data.cameras.push_back(SfmCamera(pose1, K));
data.cameras.push_back(SfmCamera(pose2, K));
for(const Point3& p: P) {
for (const Point3& p : points) {
// Create the track
SfmTrack track;
track.p = p;
@ -51,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
// Project points in both cameras
for (size_t i = 0; i < 2; i++)
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
// Add track to data
data.tracks.push_back(track);
@ -63,49 +57,66 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
/* ************************************************************************* */
void create5PointExample1() {
// Create two cameras poses
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 pose1, pose2(aRb, aTb);
// Create test data, we need at least 5 points
vector<Point3> P;
P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
vector<Point3> points = {
{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}};
// Assumes example is run in ${GTSAM_TOP}/build/examples
const string filename = "../../examples/data/5pointExample1.txt";
createExampleBALFile(filename, P, pose1, pose2);
const string filename = "../../examples/Data/5pointExample1.txt";
createExampleBALFile(filename, points, pose1, pose2);
}
/* ************************************************************************* */
void create5PointExample2() {
// Create two cameras poses
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(10, 0, 0);
Pose3 pose1, pose2(aRb, aTb);
// Create test data, we need at least 5 points
vector<Point3> P;
P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
vector<Point3> points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, //
{0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, //
{20, -50, 80}};
// Assumes example is run in ${GTSAM_TOP}/build/examples
const string filename = "../../examples/data/5pointExample2.txt";
const string filename = "../../examples/Data/5pointExample2.txt";
Cal3Bundler K(500, 0, 0);
createExampleBALFile(filename, P, pose1, pose2,K);
createExampleBALFile(filename, points, pose1, pose2, K);
}
/* ************************************************************************* */
void create18PointExample1() {
// Create two cameras poses
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 pose1, pose2(aRb, aTb);
// Create test data, we need 15 points
vector<Point3> points = {
{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, //
{0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, //
{-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, //
{-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, //
{-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, //
{0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}};
// Assumes example is run in ${GTSAM_TOP}/build/examples
const string filename = "../../examples/Data/18pointExample1.txt";
createExampleBALFile(filename, points, pose1, pose2);
}
int main(int argc, char* argv[]) {
create5PointExample1();
create5PointExample2();
create18PointExample1();
return 0;
}
/* ************************************************************************* */

131
examples/Data/18pointExample1.txt Normal file
View File

@ -0,0 +1,131 @@
2 18 36
0 0 0 -0
1 0 -6.123233995736766344e-18 -0.10000000000000000555
0 1 -0.10000000000000000555 -0
1 1 -1.2246467991473532688e-17 -0.2000000000000000111
0 2 0.10000000000000000555 -0
1 2 0 -0
0 3 0 -1
1 3 1 -0.20000000000000006661
0 4 0 1
1 4 -1 -0.19999999999999995559
0 5 -0.5 0.25
1 5 -0.25000000000000005551 -0.55000000000000004441
0 6 -0.5 -0.25
1 6 0.24999999999999997224 -0.55000000000000004441
0 7 0.16666666666666665741 0.33333333333333331483
1 7 -0.33333333333333331483 0.10000000000000000555
0 8 0.16666666666666665741 -0.33333333333333331483
1 8 0.33333333333333331483 0.099999999999999977796
0 9 -0.2000000000000000111 1
1 9 -1 -0.39999999999999996669
0 10 0.10000000000000000555 0.5
1 10 -0.5 3.0616169978683830179e-17
0 11 0.10000000000000000555 -0.5
1 11 0.5 -3.0616169978683830179e-17
0 12 -0.2000000000000000111 -0
1 12 -2.4492935982947065376e-17 -0.4000000000000000222
0 13 -0.2000000000000000111 -1
1 13 1 -0.40000000000000007772
0 14 0 -0
1 14 -1.2246467991473532688e-17 -0.2000000000000000111
0 15 0.2000000000000000111 1
1 15 -1 6.1232339957367660359e-17
0 16 0.2000000000000000111 -0
1 16 0 -0
0 17 0.2000000000000000111 -1
1 17 1 -6.1232339957367660359e-17
3.141592653589793116
0
0
-0
0
0
1
0
0
2.2214414690791830509
2.2214414690791826068
0
-6.123233995736766344e-18
-0.10000000000000000555
0
1
0
0
0
0
1
-0.10000000000000000555
0
1
0.10000000000000000555
0
1
0
0.5
0.5
0
-0.5
0.5
-1
-0.5
2
-1
0.5
2
0.25
-0.5
1.5
0.25
0.5
1.5
-0.10000000000000000555
-0.5
0.5
0.10000000000000000555
-0.5
1
0.10000000000000000555
0.5
1
-0.10000000000000000555
0
0.5
-0.10000000000000000555
0.5
0.5
0
0
0.5
0.10000000000000000555
-0.5
0.5
0.10000000000000000555
0
0.5
0.10000000000000000555
0.5
0.5

2636
examples/Data/quadraped_imu_data.csv Normal file
View File

File diff suppressed because it is too large Load Diff

5
examples/ImuFactorsExample.cpp
View File

@ -25,7 +25,8 @@
* A row starting with "i" is the first initial position formatted with
* N, E, D, qx, qY, qZ, qW, velN, velE, velD
* A row starting with "0" is an imu measurement
* linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
* (body frame - Forward, Right, Down)
* linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
* A row starting with "1" is a gps correction formatted with
* N, E, D, qX, qY, qZ, qW
* Note that for GPS correction, we're only using the position not the
@ -125,7 +126,7 @@ int main(int argc, char* argv[]) {
output_filename = var_map["output_filename"].as<string>();
use_isam = var_map["use_isam"].as<bool>();
ISAM2* isam2;
ISAM2* isam2 = 0;
if (use_isam) {
printf("Using ISAM2\n");
ISAM2Params parameters;

21
examples/LocalizationExample.cpp
View File

@ -78,21 +78,22 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
virtual ~UnaryFactor() {}
~UnaryFactor() override {}
// Using the NoiseModelFactor1 base class there are two functions that must be overridden.
// The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested.
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const override {
// The measurement function for a GPS-like measurement is simple:
// error_x = pose.x - measurement.x
// error_y = pose.y - measurement.y
// Consequently, the Jacobians are:
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished();
Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const override {
// The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]
// The error is then simply calculated as E(q) = h(q) - m:
// error_x = q.x - mx
// error_y = q.y - my
// Node's orientation reflects in the Jacobian, in tangent space this is equal to the right-hand rule rotation matrix
// H = [ cos(q.theta) -sin(q.theta) 0 ]
// [ sin(q.theta) cos(q.theta) 0 ]
const Rot2& R = q.rotation();
if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}

4
examples/Pose3Localization.cpp
View File

@ -43,7 +43,7 @@ int main(const int argc, const char* argv[]) {
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for (const Values::ConstKeyValuePair& key_value : *initial) {
for (const auto key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->addPrior(firstKey, Pose3(), priorModel);
@ -74,7 +74,7 @@ int main(const int argc, const char* argv[]) {
// Calculate and print marginal covariances for all variables
Marginals marginals(*graph, result);
for (const auto& key_value : result) {
for (const auto key_value : result) {
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
std::cout << marginals.marginalCovariance(key_value.key) << endl;

2
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) {
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// Additional: rewrite input with simplified keys 0,1,...
Values simpleInitial;
for(const Values::ConstKeyValuePair& key_value: *initial) {
for(const auto key_value: *initial) {
Key key;
if(add)
key = key_value.key + firstKey;

2
examples/Pose3SLAMExample_g2o.cpp
View File

@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) {
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for (const Values::ConstKeyValuePair& key_value : *initial) {
for (const auto key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->addPrior(firstKey, Pose3(), priorModel);

2
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) {
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for (const Values::ConstKeyValuePair& key_value : *initial) {
for (const auto key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->addPrior(firstKey, Pose3(), priorModel);

2
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) {
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for (const Values::ConstKeyValuePair& key_value : *initial) {
for (const auto key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->addPrior(firstKey, Pose3(), priorModel);

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

25
examples/ShonanAveragingCLI.cpp
View File

@ -25,6 +25,9 @@
* Read 3D dataset sphere25000.txt and output to shonan.g2o (default)
* ./ShonanAveragingCLI -i spere2500.txt
*
* If you prefer using a robust Huber loss, you can add the option "-h true",
* for instance
* ./ShonanAveragingCLI -i spere2500.txt -h true
*/
#include <gtsam/base/timing.h>
@ -43,7 +46,8 @@ int main(int argc, char* argv[]) {
string datasetName;
string inputFile;
string outputFile;
int d, seed;
int d, seed, pMin;
bool useHuberLoss;
po::options_description desc(
"Shonan Rotation Averaging CLI reads a *pose* graph, extracts the "
"rotation constraints, and runs the Shonan algorithm.");
@ -58,6 +62,10 @@ int main(int argc, char* argv[]) {
"Write solution to the specified file")(
"dimension,d", po::value<int>(&d)->default_value(3),
"Optimize over 2D or 3D rotations")(
"useHuberLoss,h", po::value<bool>(&useHuberLoss)->default_value(false),
"set True to use Huber loss")("pMin,p",
po::value<int>(&pMin)->default_value(3),
"set to use desired rank pMin")(
"seed,s", po::value<int>(&seed)->default_value(42),
"Random seed for initial estimate");
po::variables_map vm;
@ -85,11 +93,14 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph::shared_ptr inputGraph;
Values::shared_ptr posesInFile;
Values poses;
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
if (d == 2) {
cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
ShonanAveraging2 shonan(inputFile);
ShonanAveraging2::Parameters parameters(lmParams);
parameters.setUseHuber(useHuberLoss);
ShonanAveraging2 shonan(inputFile, parameters);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial);
auto result = shonan.run(initial, pMin);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load2D(inputFile);
@ -101,9 +112,11 @@ int main(int argc, char* argv[]) {
poses = initialize::computePoses<Pose2>(result.first, &poseGraph);
} else if (d == 3) {
cout << "Running Shonan averaging for SO(3) on " << inputFile << endl;
ShonanAveraging3 shonan(inputFile);
ShonanAveraging3::Parameters parameters(lmParams);
parameters.setUseHuber(useHuberLoss);
ShonanAveraging3 shonan(inputFile, parameters);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial);
auto result = shonan.run(initial, pMin);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load3D(inputFile);
@ -118,7 +131,7 @@ int main(int argc, char* argv[]) {
return 1;
}
cout << "Writing result to " << outputFile << endl;
writeG2o(NonlinearFactorGraph(), poses, outputFile);
writeG2o(*inputGraph, poses, outputFile);
return 0;
}

2
examples/SolverComparer.cpp
View File

@ -559,7 +559,7 @@ void runPerturb()
// Perturb values
VectorValues noise;
for(const Values::KeyValuePair& key_val: initial)
for(const Values::KeyValuePair key_val: initial)
{
Vector noisev(key_val.value.dim());
for(Vector::Index i = 0; i < noisev.size(); ++i)

2
gtsam/3rdparty/metis/CMakeLists.txt vendored
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.8)
cmake_minimum_required(VERSION 3.0)
project(METIS)
# Add flags for currect directory and below

1
gtsam/3rdparty/metis/libmetis/CMakeLists.txt vendored
View File

@ -12,6 +12,7 @@ endif()
if(WIN32)
set_target_properties(metis-gtsam PROPERTIES
PREFIX ""
COMPILE_FLAGS /w
RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
endif()

23
gtsam/CMakeLists.txt
View File

@ -135,12 +135,12 @@ endif()
# of any previously installed GTSAM headers.
target_include_directories(gtsam BEFORE PUBLIC
# main gtsam includes:
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}>
$<INSTALL_INTERFACE:include/>
# config.h
$<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
$<BUILD_INTERFACE:${GTSAM_BINARY_DIR}>
# unit tests:
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/CppUnitLite>
)
# 3rdparty libraries: use the "system" flag so they are included via "-isystem"
# and warnings (and warnings-considered-errors) in those headers are not
@ -190,23 +190,8 @@ if(WIN32)
else()
if("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
# Suppress all warnings from 3rd party sources.
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w -Wno-everything")
else()
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
endif()
endif()
# Create the matlab toolbox for the gtsam library
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
# Set up codegen
include(GtsamMatlabWrap)
# Generate, build and install toolbox
set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
if(NOT BUILD_SHARED_LIBS)
list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
endif()
# Wrap
wrap_and_install_library(gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
endif ()

2
gtsam/base/FastList.h
View File

@ -22,6 +22,8 @@
#include <list>
#include <boost/utility/enable_if.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/version.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/list.hpp>
namespace gtsam {

2
gtsam/base/GenericValue.h
View File

@ -74,7 +74,7 @@ public:
}
/// Destructor
virtual ~GenericValue() {
~GenericValue() override {
}
/// equals implementing generic Value interface

2
gtsam/base/Matrix.cpp
View File

@ -153,7 +153,7 @@ const Eigen::IOFormat& matlabFormat() {
/* ************************************************************************* */
//3 argument call
void print(const Matrix& A, const string &s, ostream& stream) {
cout << s << A.format(matlabFormat()) << endl;
stream << s << A.format(matlabFormat()) << endl;
}
/* ************************************************************************* */

6
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>
@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBas
for(size_t i=0; i<m1; i++)
for(size_t j=0; j<n1; j++) {
if(!fpEqual(A(i,j), B(i,j), tol)) {
if(!fpEqual(A(i,j), B(i,j), tol, false)) {
return false;
}
}
@ -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;

3
gtsam/base/ProductLieGroup.h
View File

@ -161,6 +161,9 @@ public:
}
return v;
}
static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
return Logmap(p, Hp);
}
ProductLieGroup expmap(const TangentVector& v) const {
return compose(ProductLieGroup::Expmap(v));
}

13
gtsam/base/Testable.h
View File

@ -33,9 +33,10 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/concept_check.hpp>
#include <stdio.h>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#define GTSAM_PRINT(x)((x).print(#x))
@ -72,10 +73,10 @@ namespace gtsam {
}; // \ Testable
inline void print(float v, const std::string& s = "") {
printf("%s%f\n",s.c_str(),v);
std::cout << (s.empty() ? s : s + " ") << v << std::endl;
}
inline void print(double v, const std::string& s = "") {
printf("%s%lf\n",s.c_str(),v);
std::cout << (s.empty() ? s : s + " ") << v << std::endl;
}
/** Call equal on the object */
@ -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_);
}

9
gtsam/base/TestableAssertions.h
View File

@ -372,16 +372,19 @@ bool assert_stdout_equal(const std::string& expected, const V& actual) {
/**
* Capture print function output and compare against string.
*
* @param s: Optional string to pass to the print() method.
*/
template<class V>
bool assert_print_equal(const std::string& expected, const V& actual) {
template <class V>
bool assert_print_equal(const std::string& expected, const V& actual,
const std::string& s = "") {
// Redirect output to buffer so we can compare
std::stringstream buffer;
// Save the original output stream so we can reset later
std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
// We test against actual std::cout for faithful reproduction
actual.print();
actual.print(s);
// Get output string and reset stdout
std::string actual_ = buffer.str();

4
gtsam/base/ThreadsafeException.h
View File

@ -72,7 +72,7 @@ protected:
}
/// Default destructor doesn't have the noexcept
virtual ~ThreadsafeException() noexcept {
~ThreadsafeException() noexcept override {
}
public:
@ -114,7 +114,7 @@ class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
{
public:
CholeskyFailed() noexcept {}
virtual ~CholeskyFailed() noexcept {}
~CholeskyFailed() noexcept override {}
};
} // namespace gtsam

16
gtsam/base/Vector.cpp
View File

@ -39,7 +39,7 @@ namespace gtsam {
* 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
* 2. https://floating-point-gui.de/errors/comparison/
* ************************************************************************* */
bool fpEqual(double a, double b, double tol) {
bool fpEqual(double a, double b, double tol, bool check_relative_also) {
using std::abs;
using std::isnan;
using std::isinf;
@ -48,7 +48,7 @@ bool fpEqual(double a, double b, double tol) {
double larger = (abs(b) > abs(a)) ? abs(b) : abs(a);
// handle NaNs
if(std::isnan(a) || isnan(b)) {
if(isnan(a) || isnan(b)) {
return isnan(a) && isnan(b);
}
// handle inf
@ -60,13 +60,15 @@ bool fpEqual(double a, double b, double tol) {
else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) {
return abs(a-b) <= tol * DOUBLE_MIN_NORMAL;
}
// Check if the numbers are really close
// Needed when comparing numbers near zero or tol is in vicinity
else if(abs(a-b) <= tol) {
// Check if the numbers are really close.
// Needed when comparing numbers near zero or tol is in vicinity.
else if (abs(a - b) <= tol) {
return true;
}
// Use relative error
else if(abs(a-b) <= tol * min(larger, std::numeric_limits<double>::max())) {
// Check for relative error
else if (abs(a - b) <=
tol * min(larger, std::numeric_limits<double>::max()) &&
check_relative_also) {
return true;
}

8
gtsam/base/Vector.h
View File

@ -85,9 +85,15 @@ static_assert(
* respectively for the comparison to be true.
* If one is NaN/Inf and the other is not, returns false.
*
* @param check_relative_also is a flag which toggles additional checking for
* relative error. This means that if either the absolute error or the relative
* error is within the tolerance, the result will be true.
* By default, the flag is true.
*
* Return true if two numbers are close wrt tol.
*/
GTSAM_EXPORT bool fpEqual(double a, double b, double tol);
GTSAM_EXPORT bool fpEqual(double a, double b, double tol,
bool check_relative_also = true);
/**
* print without optional string, must specify cout yourself

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.
*/

378
gtsam/base/numericalDerivative.h
View File

@ -18,15 +18,7 @@
// \callgraph
#pragma once
#include <boost/function.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <functional>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
@ -45,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, _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), _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
@ -76,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(
@ -116,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),
@ -157,7 +149,8 @@ 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, _1), x, delta);
return numericalDerivative11<Y, X>(std::bind(h, std::placeholders::_1), x,
delta);
}
/**
@ -170,20 +163,23 @@ 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, _1, boost::cref(x2)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
}
/** use a raw C++ function pointer */
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, _1, _2), x1, x2, delta);
return numericalDerivative21<Y, X1, X2>(
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
delta);
}
/**
@ -196,20 +192,23 @@ 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), _1), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
}
/** use a raw C++ function pointer */
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, _1, _2), x1, x2, delta);
return numericalDerivative22<Y, X1, X2>(
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
delta);
}
/**
@ -225,20 +224,24 @@ 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, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
x1, delta);
}
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, _1, _2, _3), x1,
x2, x3, delta);
return numericalDerivative31<Y, X1, X2, X3>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -254,20 +257,24 @@ 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), _1, boost::cref(x3)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
x2, delta);
}
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, _1, _2, _3), x1,
x2, x3, delta);
return numericalDerivative32<Y, X1, X2, X3>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -283,20 +290,24 @@ 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), _1), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
x3, delta);
}
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, _1, _2, _3), x1,
x2, x3, delta);
return numericalDerivative33<Y, X1, X2, X3>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -312,19 +323,25 @@ 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, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4)),
x1, delta);
}
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, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative41<Y, X1, X2, X3, X4>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -340,19 +357,25 @@ 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), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4)),
x2, delta);
}
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, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative42<Y, X1, X2, X3, X4>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -368,19 +391,25 @@ 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), _1, boost::cref(x4)), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4)),
x3, delta);
}
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, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative43<Y, X1, X2, X3, X4>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -396,19 +425,25 @@ 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), _1), x4, delta);
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1),
x4, delta);
}
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, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative44<Y, X1, X2, X3, X4>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -425,19 +460,26 @@ 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, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5)),
x1, delta);
}
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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative51<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -454,19 +496,26 @@ 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), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4), std::cref(x5)),
x2, delta);
}
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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -483,19 +532,26 @@ 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), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4), std::cref(x5)),
x3, delta);
}
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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative53<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -512,19 +568,26 @@ 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), _1, boost::cref(x5)), x4, delta);
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1, std::cref(x5)),
x4, delta);
}
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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -541,19 +604,26 @@ 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), _1), x5, delta);
return numericalDerivative11<Y, X5, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::placeholders::_1),
x5, delta);
}
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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -571,19 +641,26 @@ 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, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5), std::cref(x6)),
x1, delta);
}
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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(
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);
}
/**
@ -601,19 +678,26 @@ 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), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4), std::cref(x5), std::cref(x6)),
x2, delta);
}
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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(
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);
}
/**
@ -631,19 +715,26 @@ 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), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4), std::cref(x5), std::cref(x6)),
x3, delta);
}
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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(
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);
}
/**
@ -661,19 +752,26 @@ 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), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1, std::cref(x5), std::cref(x6)),
x4, delta);
}
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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative64<Y, X1, X2, X3, X4, X5>(
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);
}
/**
@ -691,19 +789,26 @@ 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), _1, boost::cref(x6)), x5, delta);
return numericalDerivative11<Y, X5, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::placeholders::_1, std::cref(x6)),
x5, delta);
}
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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(
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);
}
/**
@ -721,20 +826,27 @@ 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),
"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, X6, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta);
return numericalDerivative11<Y, X6, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5), std::placeholders::_1),
x6, delta);
}
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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(
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);
}
/**
@ -746,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, _1, delta), x,
delta);
return numericalDerivative11<VectorD, X>(
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
@ -769,80 +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_, _1, boost::cref(x2)), x1_, delta_);
return numericalGradient<X1>(
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), _1)), x2, delta);
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, _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, _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), _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, _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);
}
@ -852,15 +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, _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, _1, delta)),
std::function<Vector(const X1&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x1, delta);
}
@ -868,22 +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), _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, _1, delta)),
std::function<Vector(const X2&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x2, delta);
}
@ -891,22 +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), _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, _1, delta)),
std::function<Vector(const X3&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x3, delta);
}
@ -914,35 +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, _1, _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, _1, boost::cref(x2), _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), _1, _2)),
std::function<double(const X2&, const X3&)>(
std::bind(f, std::cref(x1), std::placeholders::_1,
std::placeholders::_2)),
x2, x3, delta);
}
@ -951,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);
}
@ -959,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);
}
@ -967,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);
}

13
gtsam/base/tests/testMatrix.cpp
View File

@ -1163,6 +1163,19 @@ TEST(Matrix , IsVectorSpace) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
}
TEST(Matrix, AbsoluteError) {
double a = 2000, b = 1997, tol = 1e-1;
bool isEqual;
// Test only absolute error
isEqual = fpEqual(a, b, tol, false);
EXPECT(!isEqual);
// Test relative error as well
isEqual = fpEqual(a, b, tol);
EXPECT(isEqual);
}
/* ************************************************************************* */
int main() {
TestResult tr;

6
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -30,10 +30,10 @@ double f(const Vector2& x) {
/* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalGradient) {
Vector2 x(1, 1);
Vector2 x(1, 1.1);
Vector expected(2);
expected << cos(x(1)), -sin(x(0));
expected << cos(x(0)), -sin(x(1));
Vector actual = numericalGradient<Vector2>(f, x);
@ -42,7 +42,7 @@ TEST(testNumericalDerivative, numericalGradient) {
/* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian) {
Vector2 x(1, 1);
Vector2 x(1, 1.1);
Matrix expected(2, 2);
expected << -sin(x(0)), 0.0, 0.0, -cos(x(1));

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_;
};
}

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

@ -155,7 +155,7 @@ namespace gtsam {
public:
virtual ~Choice() {
~Choice() override {
#ifdef DT_DEBUG_MEMORY
std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id() << std::std::endl;
#endif
@ -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

3
gtsam/discrete/DiscreteBayesNet.h
View File

@ -55,6 +55,9 @@ namespace gtsam {
template<class DERIVEDCONDITIONAL>
DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
/// Destructor
virtual ~DiscreteBayesNet() {}
/// @}
/// @name Testable

11
gtsam/discrete/DiscreteFactor.h
View File

@ -74,13 +74,14 @@ public:
/// @name Testable
/// @{
// equals
/// equals
virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
// print
virtual void print(const std::string& s = "DiscreteFactor\n",
const KeyFormatter& formatter = DefaultKeyFormatter) const {
Factor::print(s, formatter);
/// print
void print(
const std::string& s = "DiscreteFactor\n",
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
Base::print(s, formatter);
}
/** Test whether the factor is empty */

8
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -91,6 +91,9 @@ public:
template<class DERIVEDFACTOR>
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// Destructor
virtual ~DiscreteFactorGraph() {}
/// @name Testable
/// @{
@ -129,8 +132,9 @@ public:
double operator()(const DiscreteFactor::Values & values) const;
/// print
void print(const std::string& s = "DiscreteFactorGraph",
const KeyFormatter& formatter =DefaultKeyFormatter) const;
void print(
const std::string& s = "DiscreteFactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/** Solve the factor graph by performing variable elimination in COLAMD order using
* the dense elimination function specified in \c function,

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");

75
gtsam/geometry/Cal3.cpp Normal file
View File

@ -0,0 +1,75 @@
/* ----------------------------------------------------------------------------
* 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 Cal3.cpp
* @brief Common code for all calibration models.
* @author Frank Dellaert
*/
#include <gtsam/geometry/Cal3.h>
#include <cmath>
#include <fstream>
#include <iostream>
namespace gtsam {
/* ************************************************************************* */
Cal3::Cal3(double fov, int w, int h)
: s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) {
double a = fov * M_PI / 360.0; // fov/2 in radians
fx_ = double(w) / (2.0 * tan(a));
fy_ = fx_;
}
/* ************************************************************************* */
Cal3::Cal3(const std::string& path) {
const auto buffer = path + std::string("/calibration_info.txt");
std::ifstream infile(buffer, std::ios::in);
if (infile && !infile.eof()) {
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
} else {
throw std::runtime_error("Cal3: Unable to load the calibration");
}
infile.close();
}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3& cal) {
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py();
return os;
}
/* ************************************************************************* */
void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
/* ************************************************************************* */
bool Cal3::equals(const Cal3& K, double tol) const {
return (std::fabs(fx_ - K.fx_) < tol && std::fabs(fy_ - K.fy_) < tol &&
std::fabs(s_ - K.s_) < tol && std::fabs(u0_ - K.u0_) < tol &&
std::fabs(v0_ - K.v0_) < tol);
}
Matrix3 Cal3::inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
Matrix3 K_inverse;
K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
-v0_ / fy_, 0.0, 0.0, 1.0;
return K_inverse;
}
/* ************************************************************************* */
} // \ namespace gtsam

206
gtsam/geometry/Cal3.h Normal file
View File

@ -0,0 +1,206 @@
/* ----------------------------------------------------------------------------
* 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 Cal3.h
* @brief Common code for all Calibration models.
* @author Varun Agrawal
*/
/**
* @addtogroup geometry
*/
#pragma once
#include <gtsam/geometry/Point2.h>
namespace gtsam {
/**
* Function which makes use of the Implicit Function Theorem to compute the
* Jacobians of `calibrate` using `uncalibrate`.
* This is useful when there are iterative operations in the `calibrate`
* function which make computing jacobians difficult.
*
* Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can
* easily compute the Jacobians:
* df/pi = -I (pn and pi are independent args)
* Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
* Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
*
* @tparam Cal Calibration model.
* @tparam Dim The number of parameters in the calibration model.
* @param p Calibrated point.
* @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters.
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates.
*/
template <typename Cal, size_t Dim>
void calibrateJacobians(const Cal& calibration, const Point2& pn,
OptionalJacobian<2, Dim> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) {
if (Dcal || Dp) {
Eigen::Matrix<double, 2, Dim> H_uncal_K;
Matrix22 H_uncal_pn, H_uncal_pn_inv;
// Compute uncalibrate Jacobians
calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
H_uncal_pn_inv = H_uncal_pn.inverse();
if (Dp) *Dp = H_uncal_pn_inv;
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
}
}
/**
* @brief Common base class for all calibration models.
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3 {
protected:
double fx_ = 1.0f, fy_ = 1.0f; ///< focal length
double s_ = 0.0f; ///< skew
double u0_ = 0.0f, v0_ = 0.0f; ///< principal point
public:
enum { dimension = 5 };
///< shared pointer to calibration object
using shared_ptr = boost::shared_ptr<Cal3>;
/// @name Standard Constructors
/// @{
/// Create a default calibration that leaves coordinates unchanged
Cal3() = default;
/// constructor from doubles
Cal3(double fx, double fy, double s, double u0, double v0)
: fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
/// constructor from vector
Cal3(const Vector5& d)
: fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
/**
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
* @param fov field of view in degrees
* @param w image width
* @param h image height
*/
Cal3(double fov, int w, int h);
/// Virtual destructor
virtual ~Cal3() {}
/// @}
/// @name Advanced Constructors
/// @{
/**
* Load calibration parameters from `calibration_info.txt` file located in
* `path` directory.
*
* The contents of calibration file should be the 5 parameters in order:
* `fx, fy, s, u0, v0`
*
* @param path path to directory containing `calibration_info.txt`.
*/
Cal3(const std::string& path);
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3& cal);
/// print with optional string
virtual void print(const std::string& s = "") const;
/// Check if equal up to specified tolerance
bool equals(const Cal3& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_; }
/// focal length y
inline double fy() const { return fy_; }
/// aspect ratio
inline double aspectRatio() const { return fx_ / fy_; }
/// skew
inline double skew() const { return s_; }
/// image center in x
inline double px() const { return u0_; }
/// image center in y
inline double py() const { return v0_; }
/// return the principal point
Point2 principalPoint() const { return Point2(u0_, v0_); }
/// vectorized form (column-wise)
Vector5 vector() const {
Vector5 v;
v << fx_, fy_, s_, u0_, v0_;
return v;
}
/// return calibration matrix K
virtual Matrix3 K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** @deprecated The following function has been deprecated, use K above */
Matrix3 matrix() const { return K(); }
#endif
/// Return inverted calibration matrix inv(K)
Matrix3 inverse() const;
/// return DOF, dimensionality of tangent space
inline virtual size_t dim() const { return Dim(); }
/// return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
/// @}
/// @name Advanced Interface
/// @{
private:
/// Serialization function
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(fx_);
ar& BOOST_SERIALIZATION_NVP(fy_);
ar& BOOST_SERIALIZATION_NVP(s_);
ar& BOOST_SERIALIZATION_NVP(u0_);
ar& BOOST_SERIALIZATION_NVP(v0_);
}
/// @}
};
} // \ namespace gtsam

117
gtsam/geometry/Cal3Bundler.cpp
View File

@ -15,28 +15,19 @@
* @author ydjian
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3Bundler.h>
namespace gtsam {
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler() :
f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
}
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
double tol)
: f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
/* ************************************************************************* */
Matrix3 Cal3Bundler::K() const {
// This function is needed to ensure skew = 0;
Matrix3 K;
K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
K << fx_, 0, u0_, 0, fy_, v0_, 0, 0, 1.0;
return K;
}
@ -48,35 +39,42 @@ Vector4 Cal3Bundler::k() const {
}
/* ************************************************************************* */
Vector3 Cal3Bundler::vector() const {
return Vector3(f_, k1_, k2_);
Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); }
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
<< ", px: " << cal.px() << ", py: " << cal.py();
return os;
}
/* ************************************************************************* */
void Cal3Bundler::print(const std::string& s) const {
gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K");
gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(),
s + ".K");
}
/* ************************************************************************* */
bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol
|| std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol
|| std::abs(v0_ - K.v0_) > tol)
return false;
return true;
const Cal3* base = dynamic_cast<const Cal3*>(&K);
return (Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
std::fabs(v0_ - K.v0_) < tol);
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// r = x² + y²;
// g = (1 + k(1)*r + k(2)*r²);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
const double f_ = fx_;
// Derivatives make use of intermediate variables above
if (Dcal) {
double rx = r * x, ry = r * y;
@ -94,51 +92,38 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
}
/* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal,
Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2 :-(
// but specialized with k1,k2 non-zero only and fx=fy and s=0
double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn(x, y);
// Copied from Cal3DS2
// but specialized with k1, k2 non-zero only and fx=fy and s=0
double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_;
const Point2 invKPi(px, py);
Point2 pn;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol_)
break;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy;
int iteration = 0;
do {
// initialize pn with distortion included
const double rr = (px * px) + (py * py);
const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
}
if (distance2(uncalibrate(pn), pi) <= tol_) break;
// Set px and py using intrinsic coordinates since that is where radial
// distortion correction is done.
px = pn.x(), py = pn.y();
iteration++;
} while (iteration < maxIterations);
if (iteration >= maxIterations)
throw std::runtime_error(
"Cal3Bundler::calibrate fails to converge. need a better initialization");
"Cal3Bundler::calibrate fails to converge. need a better "
"initialization");
// We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
// Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
// df/pi = -I (pn and pi are independent args)
// Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
// Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
Matrix23 H_uncal_K;
Matrix22 H_uncal_pn, H_uncal_pn_inv;
if (Dcal || Dp) {
// Compute uncalibrate Jacobians
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
H_uncal_pn_inv = H_uncal_pn.inverse();
if (Dp) *Dp = H_uncal_pn_inv;
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
}
calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
return pn;
}
@ -167,14 +152,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
return H;
}
/* ************************************************************************* */
Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
}
/* ************************************************************************* */
Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
return T2.vector() - vector();
}
}
} // namespace gtsam

140
gtsam/geometry/Cal3Bundler.h
View File

@ -14,10 +14,12 @@
* @brief Calibration used by Bundler
* @date Sep 25, 2010
* @author Yong Dian Jian
* @author Varun Agrawal
*/
#pragma once
#include <gtsam/geometry/Cal3.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -27,23 +29,23 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3Bundler {
class GTSAM_EXPORT Cal3Bundler : public Cal3 {
private:
double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion
double tol_ = 1e-5; ///< tolerance value when calibrating
private:
double f_; ///< focal length
double k1_, k2_; ///< radial distortion
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
double tol_; ///< tolerance value when calibrating
public:
// NOTE: We use the base class fx to represent the common focal length.
// Also, image center parameters (u0, v0) are not optimized
// but are treated as constants.
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
/// Default constructor
Cal3Bundler();
Cal3Bundler() = default;
/**
* Constructor
@ -55,16 +57,21 @@ public:
* @param tol optional calibration tolerance value
*/
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
double tol = 1e-5);
double tol = 1e-5)
: Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
virtual ~Cal3Bundler() {}
~Cal3Bundler() override {}
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3Bundler& cal);
/// print with optional string
void print(const std::string& s = "") const;
void print(const std::string& s = "") const override;
/// assert equality up to a tolerance
bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
@ -73,64 +80,41 @@ public:
/// @name Standard Interface
/// @{
Matrix3 K() const; ///< Standard 3*3 calibration matrix
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
/// distorsion parameter k1
inline double k1() const { return k1_; }
/// distorsion parameter k2
inline double k2() const { return k2_; }
/// image center in x
inline double px() const { return u0_; }
/// image center in y
inline double py() const { return v0_; }
Matrix3 K() const override; ///< Standard 3*3 calibration matrix
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
Vector3 vector() const;
/// focal length x
inline double fx() const {
return f_;
}
/// focal length y
inline double fy() const {
return f_;
}
/// distorsion parameter k1
inline double k1() const {
return k1_;
}
/// distorsion parameter k2
inline double k2() const {
return k2_;
}
/// image center in x
inline double px() const {
return u0_;
}
/// image center in y
inline double py() const {
return v0_;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/// get parameter u0
inline double u0() const {
return u0_;
}
inline double u0() const { return u0_; }
/// get parameter v0
inline double v0() const {
return v0_;
}
inline double v0() const { return v0_; }
#endif
/**
* @brief: convert intrinsic coordinates xy to image coordinates uv
* Version of uncalibrate with derivatives
* @param p point in intrinsic coordinates
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
* @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
OptionalJacobian<2, 2> Dp = boost::none) const;
/**
* Convert a pixel coordinate to ideal coordinate xy
@ -140,8 +124,7 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal = boost::none,
Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// @deprecated might be removed in next release, use uncalibrate
@ -157,48 +140,45 @@ public:
/// @name Manifold
/// @{
/// return DOF, dimensionality of tangent space
size_t dim() const override { return Dim(); }
/// return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
/// Update calibration with tangent space delta
Cal3Bundler retract(const Vector& d) const;
inline Cal3Bundler retract(const Vector& d) const {
return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
}
/// Calculate local coordinates to another calibration
Vector3 localCoordinates(const Cal3Bundler& T2) const;
/// dimensionality
virtual size_t dim() const {
return 3;
Vector3 localCoordinates(const Cal3Bundler& T2) const {
return T2.vector() - vector();
}
/// dimensionality
static size_t Dim() {
return 3;
}
private:
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(f_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(tol_);
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
ar& BOOST_SERIALIZATION_NVP(k1_);
ar& BOOST_SERIALIZATION_NVP(k2_);
ar& BOOST_SERIALIZATION_NVP(tol_);
}
/// @}
};
template<>
template <>
struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
template<>
template <>
struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
} // namespace gtsam
} // namespace gtsam

23
gtsam/geometry/Cal3DS2.cpp
View File

@ -13,28 +13,30 @@
* @file Cal3DS2.cpp
* @date Feb 28, 2010
* @author ydjian
* @author Varun Agrawal
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3DS2.h>
namespace gtsam {
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s_) const {
Base::print(s_);
std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
os << (Cal3DS2_Base&)cal;
return os;
}
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s_) const { Base::print(s_); }
/* ************************************************************************* */
bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
return false;
return true;
const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
return Cal3DS2_Base::equals(*base, tol);
}
/* ************************************************************************* */
@ -46,8 +48,5 @@ Cal3DS2 Cal3DS2::retract(const Vector& d) const {
Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
return T2.vector() - vector();
}
}
/* ************************************************************************* */

54
gtsam/geometry/Cal3DS2.h
View File

@ -11,9 +11,11 @@
/**
* @file Cal3DS2.h
* @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
* @brief Calibration of a camera with radial distortion, calculations in base
* class Cal3DS2_Base
* @date Feb 28, 2010
* @author ydjian
* @autho Varun Agrawal
*/
#pragma once
@ -30,35 +32,37 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
using Base = Cal3DS2_Base;
typedef Cal3DS2_Base Base;
public:
public:
enum { dimension = 9 };
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2() : Base() {}
Cal3DS2() = default;
Cal3DS2(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
: Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
virtual ~Cal3DS2() {}
~Cal3DS2() override {}
/// @}
/// @name Advanced Constructors
/// @{
Cal3DS2(const Vector &v) : Base(v) {}
Cal3DS2(const Vector9& v) : Base(v) {}
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3DS2& cal);
/// print with optional string
void print(const std::string& s = "") const override;
@ -70,16 +74,16 @@ public:
/// @{
/// Given delta vector, update calibration
Cal3DS2 retract(const Vector& d) const ;
Cal3DS2 retract(const Vector& d) const;
/// Given a different calibration, calculate update to obtain it
Vector localCoordinates(const Cal3DS2& T2) const ;
Vector localCoordinates(const Cal3DS2& T2) const;
/// Return dimensions of calibration manifold object
virtual size_t dim() const { return dimension ; }
size_t dim() const override { return Dim(); }
/// Return dimensions of calibration manifold object
static size_t Dim() { return dimension; }
inline static size_t Dim() { return dimension; }
/// @}
/// @name Clone
@ -92,30 +96,24 @@ public:
/// @}
private:
private:
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/)
{
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Cal3DS2_Base>(*this));
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
}
/// @}
};
template<>
template <>
struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
template<>
template <>
struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
}

105
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -13,27 +13,17 @@
* @file Cal3DS2_Base.cpp
* @date Feb 28, 2010
* @author ydjian
* @author Varun Agrawal
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam {
/* ************************************************************************* */
Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix3 Cal3DS2_Base::K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
/* ************************************************************************* */
Vector9 Cal3DS2_Base::vector() const {
Vector9 v;
@ -41,6 +31,14 @@ Vector9 Cal3DS2_Base::vector() const {
return v;
}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) {
os << (Cal3&)cal;
os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1()
<< ", p2: " << cal.p2();
return os;
}
/* ************************************************************************* */
void Cal3DS2_Base::print(const std::string& s_) const {
gtsam::print((Matrix)K(), s_ + ".K");
@ -49,31 +47,30 @@ void Cal3DS2_Base::print(const std::string& s_) const {
/* ************************************************************************* */
bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
return false;
return true;
const Cal3* base = dynamic_cast<const Cal3*>(&K);
return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
std::fabs(k2_ - K.k2_) < tol && std::fabs(p1_ - K.p1_) < tol &&
std::fabs(p2_ - K.p2_) < tol;
}
/* ************************************************************************* */
static Matrix29 D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Matrix2& DK) {
static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
double xy, double rr, double r4, double pnx,
double pny, const Matrix2& DK) {
Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Matrix24 DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Matrix29 D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Matrix2 D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Matrix2& DK) {
static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
double k2, double p1, double p2,
const Matrix2& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@ -87,24 +84,23 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR;
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// r² = x² + y²;
// g = (1 + k(1)*r² + k(2)*r⁴);
// dp = [2*k(3)*x*y + k(4)*(r² + 2*x²); 2*k(4)*x*y + k(3)*(r² + 2*y²)];
// pi(:,i) = g * pn(:,i) + dp;
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
// tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
@ -115,37 +111,44 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
const double pny = g * y + dy;
Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
if (Dcal || Dp) {
DK << fx_, s_, 0.0, fy_;
}
// Derivative for calibration
if (H1)
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
if (Dcal) {
*Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
// Derivative for points
if (H2)
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
if (Dp) {
*Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
// Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// Use the following fixed point iteration to invert the radial distortion.
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
(1 / fy_) * (pi.y() - v0_));
const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
(1 / fy_) * (pi.y() - v0_));
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
// initialize by ignoring the distortion at all, might be problematic for
// pixels around boundary
Point2 pn = invKPi;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
if (distance2(uncalibrate(pn), pi) <= tol_) break;
const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px,
yy = py * py;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
@ -153,8 +156,11 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
pn = (invKPi - Point2(dx, dy)) / g;
}
if ( iteration >= maxIterations )
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
if (iteration >= maxIterations)
throw std::runtime_error(
"Cal3DS2::calibrate fails to converge. need a better initialization");
calibrateJacobians<Cal3DS2_Base, dimension>(*this, pn, Dcal, Dp);
return pn;
}
@ -184,8 +190,5 @@ Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
}
/* ************************************************************************* */

126
gtsam/geometry/Cal3DS2_Base.h
View File

@ -14,10 +14,12 @@
* @brief Calibration of a camera with radial distortion
* @date Feb 28, 2010
* @author ydjian
* @author Varun Agrawal
*/
#pragma once
#include <gtsam/geometry/Cal3.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -31,82 +33,77 @@ namespace gtsam {
* http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
* but using only k1,k2,p1, and p2 coefficients.
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
* rr = Pn.x^2 + Pn.y^2
* \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2) ;
* p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y ]
* pi = K*Pn
* r² = P.x² + P.y²
* P̂ = (1 + k1*r² + k2*r⁴) P + [ (2*p1 P.x P.y) + p2 (r² + 2 Pn.x²)
* p1 (r² + 2 Pn.y²) + (2*p2 Pn.x Pn.y) ]
* pi = K*P̂
*/
class GTSAM_EXPORT Cal3DS2_Base {
class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
protected:
double k1_ = 0.0f, k2_ = 0.0f; ///< radial 2nd-order and 4th-order
double p1_ = 0.0f, p2_ = 0.0f; ///< tangential distortion
double tol_ = 1e-5; ///< tolerance value when calibrating
protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double p1_, p2_ ; // tangential distortion
public:
public:
enum { dimension = 9 };
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
Cal3DS2_Base() = default;
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
: Cal3(fx, fy, s, u0, v0),
k1_(k1),
k2_(k2),
p1_(p1),
p2_(p2),
tol_(tol) {}
virtual ~Cal3DS2_Base() {}
~Cal3DS2_Base() override {}
/// @}
/// @name Advanced Constructors
/// @{
Cal3DS2_Base(const Vector &v) ;
Cal3DS2_Base(const Vector9& v)
: Cal3(v(0), v(1), v(2), v(3), v(4)),
k1_(v(5)),
k2_(v(6)),
p1_(v(7)),
p2_(v(8)) {}
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3DS2_Base& cal);
/// print with optional string
virtual void print(const std::string& s = "") const;
void print(const std::string& s = "") const override;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_;}
/// focal length x
inline double fy() const { return fy_;}
/// skew
inline double skew() const { return s_;}
/// image center in x
inline double px() const { return u0_;}
/// image center in y
inline double py() const { return v0_;}
/// First distortion coefficient
inline double k1() const { return k1_;}
inline double k1() const { return k1_; }
/// Second distortion coefficient
inline double k2() const { return k2_;}
inline double k2() const { return k2_; }
/// First tangential distortion coefficient
inline double p1() const { return p1_;}
inline double p1() const { return p1_; }
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/// return calibration matrix -- not really applicable
Matrix3 K() const;
inline double p2() const { return p2_; }
/// return distortion parameter vector
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
@ -121,18 +118,24 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
OptionalJacobian<2,9> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const ;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix2 D2d_intrinsic(const Point2& p) const ;
Matrix2 D2d_intrinsic(const Point2& p) const;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix29 D2d_calibration(const Point2& p) const ;
Matrix29 D2d_calibration(const Point2& p) const;
/// return DOF, dimensionality of tangent space
size_t dim() const override { return Dim(); }
/// return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
/// @}
/// @name Clone
@ -145,30 +148,23 @@ public:
/// @}
private:
private:
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/)
{
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3DS2_Base", boost::serialization::base_object<Cal3>(*this));
ar& BOOST_SERIALIZATION_NVP(k1_);
ar& BOOST_SERIALIZATION_NVP(k2_);
ar& BOOST_SERIALIZATION_NVP(p1_);
ar& BOOST_SERIALIZATION_NVP(p2_);
ar& BOOST_SERIALIZATION_NVP(tol_);
}
/// @}
};
}

73
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -13,6 +13,7 @@
* @file Cal3Fisheye.cpp
* @date Apr 8, 2020
* @author ghaggin
* @author Varun Agrawal
*/
#include <gtsam/base/Matrix.h>
@ -23,18 +24,6 @@
namespace gtsam {
/* ************************************************************************* */
Cal3Fisheye::Cal3Fisheye(const Vector9& v)
: fx_(v[0]),
fy_(v[1]),
s_(v[2]),
u0_(v[3]),
v0_(v[4]),
k1_(v[5]),
k2_(v[6]),
k3_(v[7]),
k4_(v[8]) {}
/* ************************************************************************* */
Vector9 Cal3Fisheye::vector() const {
Vector9 v;
@ -42,13 +31,6 @@ Vector9 Cal3Fisheye::vector() const {
return v;
}
/* ************************************************************************* */
Matrix3 Cal3Fisheye::K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
/* ************************************************************************* */
double Cal3Fisheye::Scaling(double r) {
static constexpr double threshold = 1e-8;
@ -122,14 +104,25 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
}
/* ************************************************************************* */
Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
// initial gues just inverts the pinhole model
Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// 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,
// Perform newtons method, break when solution converges past tol_,
// throw exception if max iterations are reached
const int maxIterations = 10;
int iteration;
@ -140,7 +133,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
// Test convergence
if ((uv_hat - uv).norm() < tol) break;
if ((uv_hat - uv).norm() < tol_) break;
// Newton's method update step
pi = pi - jac.inverse() * (uv_hat - uv);
@ -151,9 +144,19 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
"Cal3Fisheye::calibrate fails to converge. need a better "
"initialization");
calibrateJacobians<Cal3Fisheye, dimension>(*this, pi, Dcal, Dp);
return pi;
}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) {
os << (Cal3&)cal;
os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", k3: " << cal.k3()
<< ", k4: " << cal.k4();
return os;
}
/* ************************************************************************* */
void Cal3Fisheye::print(const std::string& s_) const {
gtsam::print((Matrix)K(), s_ + ".K");
@ -162,24 +165,12 @@ void Cal3Fisheye::print(const std::string& s_) const {
/* ************************************************************************* */
bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol ||
std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol ||
std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol ||
std::abs(k4_ - K.k4_) > tol)
return false;
return true;
const Cal3* base = dynamic_cast<const Cal3*>(&K);
return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
std::fabs(k2_ - K.k2_) < tol && std::fabs(k3_ - K.k3_) < tol &&
std::fabs(k4_ - K.k4_) < tol;
}
/* ************************************************************************* */
Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const {
return Cal3Fisheye(vector() + d);
}
/* ************************************************************************* */
Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const {
return T2.vector() - vector();
}
} // namespace gtsam
/* ************************************************************************* */
} // \ namespace gtsam

107
gtsam/geometry/Cal3Fisheye.h
View File

@ -14,10 +14,12 @@
* @brief Calibration of a fisheye camera
* @date Apr 8, 2020
* @author ghaggin
* @author Varun Agrawal
*/
#pragma once
#include <gtsam/geometry/Cal3.h>
#include <gtsam/geometry/Point2.h>
#include <string>
@ -36,71 +38,58 @@ namespace gtsam {
* Intrinsic coordinates:
* [x_i;y_i] = [x/z; y/z]
* Distorted coordinates:
* r^2 = (x_i)^2 + (y_i)^2
* r² = (x_i)² + (y_i)²
* th = atan(r)
* th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
* th_d = th(1 + k1*th² + k2*th⁴ + k3*th⁶ + k4*th⁸)
* [x_d; y_d] = (th_d / r)*[x_i; y_i]
* Pixel coordinates:
* K = [fx s u0; 0 fy v0 ;0 0 1]
* [u; v; 1] = K*[x_d; y_d; 1]
*/
class GTSAM_EXPORT Cal3Fisheye {
class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
private:
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
double k1_ = 0.0f, k2_ = 0.0f; ///< fisheye distortion coefficients
double k3_ = 0.0f, k4_ = 0.0f; ///< fisheye distortion coefficients
double tol_ = 1e-5; ///< tolerance value when calibrating
public:
enum { dimension = 9 };
typedef boost::shared_ptr<Cal3Fisheye>
shared_ptr; ///< shared pointer to fisheye calibration object
///< shared pointer to fisheye calibration object
using shared_ptr = boost::shared_ptr<Cal3Fisheye>;
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3Fisheye()
: fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
Cal3Fisheye() = default;
Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
const double v0, const double k1, const double k2,
const double k3, const double k4)
: fx_(fx),
fy_(fy),
s_(s),
u0_(u0),
v0_(v0),
const double k3, const double k4, double tol = 1e-5)
: Cal3(fx, fy, s, u0, v0),
k1_(k1),
k2_(k2),
k3_(k3),
k4_(k4) {}
k4_(k4),
tol_(tol) {}
virtual ~Cal3Fisheye() {}
~Cal3Fisheye() override {}
/// @}
/// @name Advanced Constructors
/// @{
explicit Cal3Fisheye(const Vector9& v);
explicit Cal3Fisheye(const Vector9& v)
: Cal3(v(0), v(1), v(2), v(3), v(4)),
k1_(v(5)),
k2_(v(6)),
k3_(v(7)),
k4_(v(8)) {}
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_; }
/// focal length x
inline double fy() const { return fy_; }
/// skew
inline double skew() const { return s_; }
/// image center in x
inline double px() const { return u0_; }
/// image center in y
inline double py() const { return v0_; }
/// First distortion coefficient
inline double k1() const { return k1_; }
@ -113,9 +102,6 @@ class GTSAM_EXPORT Cal3Fisheye {
/// Second tangential distortion coefficient
inline double k4() const { return k4_; }
/// return calibration matrix
Matrix3 K() const;
/// return distortion parameter vector
Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
@ -129,24 +115,34 @@ class GTSAM_EXPORT Cal3Fisheye {
* @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
* coordinates [u; v]
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ...,
* k4)
* @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
/// y_i]
Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
/**
* Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
* y_i]
* @param p point in image coordinates
* @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3Fisheye& cal);
/// print with optional string
virtual void print(const std::string& s = "") const;
void print(const std::string& s = "") const override;
/// assert equality up to a tolerance
bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
@ -155,17 +151,21 @@ class GTSAM_EXPORT Cal3Fisheye {
/// @name Manifold
/// @{
/// Return dimensions of calibration manifold object
size_t dim() const override { return Dim(); }
/// Return dimensions of calibration manifold object
inline static size_t Dim() { return dimension; }
/// Given delta vector, update calibration
Cal3Fisheye retract(const Vector& d) const;
inline Cal3Fisheye retract(const Vector& d) const {
return Cal3Fisheye(vector() + d);
}
/// Given a different calibration, calculate update to obtain it
Vector localCoordinates(const Cal3Fisheye& T2) const;
/// Return dimensions of calibration manifold object
virtual size_t dim() const { return 9; }
/// Return dimensions of calibration manifold object
static size_t Dim() { return 9; }
Vector localCoordinates(const Cal3Fisheye& T2) const {
return T2.vector() - vector();
}
/// @}
/// @name Clone
@ -186,11 +186,8 @@ class GTSAM_EXPORT Cal3Fisheye {
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(fx_);
ar& BOOST_SERIALIZATION_NVP(fy_);
ar& BOOST_SERIALIZATION_NVP(s_);
ar& BOOST_SERIALIZATION_NVP(u0_);
ar& BOOST_SERIALIZATION_NVP(v0_);
ar& boost::serialization::make_nvp(
"Cal3Fisheye", boost::serialization::base_object<Cal3>(*this));
ar& BOOST_SERIALIZATION_NVP(k1_);
ar& BOOST_SERIALIZATION_NVP(k2_);
ar& BOOST_SERIALIZATION_NVP(k3_);

71
gtsam/geometry/Cal3Unified.cpp
View File

@ -13,21 +13,18 @@
* @file Cal3Unified.cpp
* @date Mar 8, 2014
* @author Jing Dong
* @author Varun Agrawal
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Point2.h>
#include <cmath>
namespace gtsam {
/* ************************************************************************* */
Cal3Unified::Cal3Unified(const Vector &v):
Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
/* ************************************************************************* */
Vector10 Cal3Unified::vector() const {
Vector10 v;
@ -35,6 +32,13 @@ Vector10 Cal3Unified::vector() const {
return v;
}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3Unified& cal) {
os << (Cal3DS2_Base&)cal;
os << ", xi: " << cal.xi();
return os;
}
/* ************************************************************************* */
void Cal3Unified::print(const std::string& s) const {
Base::print(s);
@ -43,20 +47,14 @@ void Cal3Unified::print(const std::string& s) const {
/* ************************************************************************* */
bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol ||
std::abs(xi_ - K.xi_) > tol)
return false;
return true;
const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
return Cal3DS2_Base::equals(*base, tol) && std::fabs(xi_ - K.xi_) < tol;
}
/* ************************************************************************* */
// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p,
OptionalJacobian<2,10> H1,
OptionalJacobian<2,2> H2) const {
Point2 Cal3Unified::uncalibrate(const Point2& p, OptionalJacobian<2, 10> Dcal,
OptionalJacobian<2, 2> Dp) const {
// this part of code is modified from Cal3DS2,
// since the second part of this model (after project to normalized plane)
// is same as Cal3DS2
@ -69,50 +67,53 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0);
const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx);
const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx;
const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane
const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane
// Part2: project NPlane point to pixel plane: use Cal3DS2
Point2 m(x,y);
Point2 m(x, y);
Matrix29 H1base;
Matrix2 H2base; // jacobians from Base class
Matrix2 H2base; // jacobians from Base class
Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
// Inlined derivative for calibration
if (H1) {
if (Dcal) {
// part1
Vector2 DU;
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2;
*H1 << H1base, H2base * DU;
*Dcal << H1base, H2base * DU;
}
// Inlined derivative for points
if (H2) {
if (Dp) {
// part1
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
const double mid = -(xi * xs*ys) * denom;
const double mid = -(xi * xs * ys) * denom;
Matrix2 DU;
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
DU << (sqrt_nx + xi * (ys * ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi * (xs * xs + 1)) * denom;
*H2 << H2base * DU;
*Dp << H2base * DU;
}
return puncalib;
}
/* ************************************************************************* */
Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
OptionalJacobian<2, 2> Dp) const {
// calibrate point to Nplane use base class::calibrate()
Point2 pnplane = Base::calibrate(pi, tol);
Point2 pnplane = Base::calibrate(pi);
// call nplane to space
return this->nPlaneToSpace(pnplane);
Point2 pn = this->nPlaneToSpace(pnplane);
calibrateJacobians<Cal3Unified, dimension>(*this, pn, Dcal, Dp);
return pn;
}
/* ************************************************************************* */
Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
const double x = p.x(), y = p.y();
const double xy2 = x * x + y * y;
const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1);
@ -122,7 +123,6 @@ Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
/* ************************************************************************* */
Point2 Cal3Unified::spaceToNPlane(const Point2& p) const {
const double x = p.x(), y = p.y();
const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);
@ -135,11 +135,10 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
}
/* ************************************************************************* */
Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
return T2.vector() - vector();
}
}
/* ************************************************************************* */
} // \ namespace gtsam

87
gtsam/geometry/Cal3Unified.h
View File

@ -14,6 +14,7 @@
* @brief Unified Calibration Model, see Mei07icra for details
* @date Mar 8, 2014
* @author Jing Dong
* @author Varun Agrawal
*/
/**
@ -27,53 +28,57 @@
namespace gtsam {
/**
* @brief Calibration of a omni-directional camera with mirror + lens radial distortion
* @brief Calibration of a omni-directional camera with mirror + lens radial
* distortion
* @addtogroup geometry
* \nosubgrouping
*
* Similar to Cal3DS2, does distortion but has additional mirror parameter xi
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
* Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
* rr = Pn.x^2 + Pn.y^2
* \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² +
* P.y² + 1})]
* r² = Pn.x² + Pn.y²
* \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ;
* k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
using This = Cal3Unified;
using Base = Cal3DS2_Base;
typedef Cal3Unified This;
typedef Cal3DS2_Base Base;
private:
double xi_; // mirror parameter
public:
private:
double xi_ = 0.0f; ///< mirror parameter
public:
enum { dimension = 10 };
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3Unified() : Base(), xi_(0) {}
Cal3Unified() = default;
Cal3Unified(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
: Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
virtual ~Cal3Unified() {}
~Cal3Unified() override {}
/// @}
/// @name Advanced Constructors
/// @{
Cal3Unified(const Vector &v) ;
Cal3Unified(const Vector10& v)
: Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3Unified& cal);
/// print with optional string
void print(const std::string& s = "") const override;
@ -85,7 +90,10 @@ public:
/// @{
/// mirror parameter
inline double xi() const { return xi_;}
inline double xi() const { return xi_; }
/// Return all parameters as a vector
Vector10 vector() const;
/**
* convert intrinsic coordinates xy to image coordinates uv
@ -95,11 +103,12 @@ public:
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p,
OptionalJacobian<2,10> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const ;
OptionalJacobian<2, 10> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Convert a 3D point to normalized unit plane
Point2 spaceToNPlane(const Point2& p) const;
@ -112,41 +121,33 @@ public:
/// @{
/// Given delta vector, update calibration
Cal3Unified retract(const Vector& d) const ;
Cal3Unified retract(const Vector& d) const;
/// Given a different calibration, calculate update to obtain it
Vector10 localCoordinates(const Cal3Unified& T2) const ;
Vector localCoordinates(const Cal3Unified& T2) const;
/// Return dimensions of calibration manifold object
virtual size_t dim() const { return dimension ; }
size_t dim() const override { return Dim(); }
/// Return dimensions of calibration manifold object
static size_t Dim() { return dimension; }
/// Return all parameters as a vector
Vector10 vector() const ;
inline static size_t Dim() { return dimension; }
/// @}
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/)
{
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_);
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
ar& BOOST_SERIALIZATION_NVP(xi_);
}
};
template<>
template <>
struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
template<>
template <>
struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
}

92
gtsam/geometry/Cal3_S2.cpp
View File

@ -22,95 +22,55 @@
#include <iostream>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
Cal3_S2::Cal3_S2(double fov, int w, int h) :
s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
double a = fov * M_PI / 360.0; // fov/2 in radians
fx_ = (double) w / (2.0 * tan(a)); // old formula: fx_ = (double) w * tan(a);
fy_ = fx_;
}
/* ************************************************************************* */
Cal3_S2::Cal3_S2(const std::string &path) :
fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
char buffer[200];
buffer[0] = 0;
sprintf(buffer, "%s/calibration_info.txt", path.c_str());
std::ifstream infile(buffer, std::ios::in);
if (infile)
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
else {
throw std::runtime_error("Cal3_S2: Unable to load the calibration");
}
infile.close();
}
/* ************************************************************************* */
ostream& operator<<(ostream& os, const Cal3_S2& cal) {
os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
<< ", py:" << cal.py() << "}";
std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) {
// Use the base class version since it is identical.
os << (Cal3&)cal;
return os;
}
/* ************************************************************************* */
void Cal3_S2::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
gtsam::print((Matrix)K(), s);
}
/* ************************************************************************* */
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
if (std::abs(fx_ - K.fx_) > tol)
return false;
if (std::abs(fy_ - K.fy_) > tol)
return false;
if (std::abs(s_ - K.s_) > tol)
return false;
if (std::abs(u0_ - K.u0_) > tol)
return false;
if (std::abs(v0_ - K.v0_) > tol)
return false;
return true;
return Cal3::equals(K, tol);
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
OptionalJacobian<2, 2> Dp) const {
OptionalJacobian<2, 2> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal)
*Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
if (Dp)
*Dp << fx_, s_, 0.0, fy_;
if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
if (Dp) *Dp << fx_, s_, 0.0, fy_;
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
OptionalJacobian<2,2> Dp) const {
const double u = p.x(), v = p.y();
double delta_u = u - u0_, delta_v = v - v0_;
double inv_fx = 1/ fx_, inv_fy = 1/fy_;
double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
inv_fy_delta_v);
if(Dcal)
*Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, -inv_fx * point.y(),
-inv_fx, inv_fx_s_inv_fy,
0, -inv_fy * point.y(), 0, 0, -inv_fy;
if(Dp)
*Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
return point;
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
OptionalJacobian<2, 2> Dp) const {
const double u = p.x(), v = p.y();
double delta_u = u - u0_, delta_v = v - v0_;
double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
double inv_fy_delta_v = inv_fy * delta_v;
double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
if (Dcal) {
*Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
-inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
0, 0, -inv_fy;
}
if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
return point;
}
/* ************************************************************************* */
Vector3 Cal3_S2::calibrate(const Vector3& p) const {
return matrix_inverse() * p;
}
Vector3 Cal3_S2::calibrate(const Vector3& p) const { return inverse() * p; }
/* ************************************************************************* */
} // namespace gtsam
} // namespace gtsam

194
gtsam/geometry/Cal3_S2.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <gtsam/geometry/Cal3.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -30,31 +31,25 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3_S2 {
private:
double fx_, fy_, s_, u0_, v0_;
public:
class GTSAM_EXPORT Cal3_S2 : public Cal3 {
public:
enum { dimension = 5 };
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
///< shared pointer to calibration object
using shared_ptr = boost::shared_ptr<Cal3_S2>;
/// @name Standard Constructors
/// @{
/// Create a default calibration that leaves coordinates unchanged
Cal3_S2() :
fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
}
Cal3_S2() = default;
/// constructor from doubles
Cal3_S2(double fx, double fy, double s, double u0, double v0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
}
Cal3_S2(double fx, double fy, double s, double u0, double v0)
: Cal3(fx, fy, s, u0, v0) {}
/// constructor from vector
Cal3_S2(const Vector &d) :
fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
}
Cal3_S2(const Vector5& d) : Cal3(d) {}
/**
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
@ -62,141 +57,65 @@ public:
* @param w image width
* @param h image height
*/
Cal3_S2(double fov, int w, int h);
Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
/// @}
/// @name Advanced Constructors
/// @{
/**
* Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// load calibration from location (default name is calibration_info.txt)
Cal3_S2(const std::string &path);
/**
* Convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/**
* Convert homogeneous image coordinates to intrinsic coordinates
* @param p point in image coordinates
* @return point in intrinsic coordinates
*/
Vector3 calibrate(const Vector3& p) const;
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal);
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3_S2& cal);
/// print with optional string
void print(const std::string& s = "Cal3_S2") const;
void print(const std::string& s = "Cal3_S2") const override;
/// Check if equal up to specified tolerance
bool equals(const Cal3_S2& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const {
return fx_;
}
/// focal length y
inline double fy() const {
return fy_;
}
/// aspect ratio
inline double aspectRatio() const {
return fx_/fy_;
}
/// skew
inline double skew() const {
return s_;
}
/// image center in x
inline double px() const {
return u0_;
}
/// image center in y
inline double py() const {
return v0_;
}
/// return the principal point
Point2 principalPoint() const {
return Point2(u0_, v0_);
}
/// vectorized form (column-wise)
Vector5 vector() const {
Vector5 v;
v << fx_, fy_, s_, u0_, v0_;
return v;
}
/// return calibration matrix K
Matrix3 K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
/** @deprecated The following function has been deprecated, use K above */
Matrix3 matrix() const {
return K();
}
/// return inverted calibration matrix inv(K)
Matrix3 matrix_inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
Matrix3 K_inverse;
K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
return K_inverse;
}
/**
* convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert homogeneous image coordinates to intrinsic coordinates
* @param p point in image coordinates
* @return point in intrinsic coordinates
*/
Vector3 calibrate(const Vector3& p) const;
/// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
inline Cal3_S2 between(const Cal3_S2& q,
OptionalJacobian<5,5> H1=boost::none,
OptionalJacobian<5,5> H2=boost::none) const {
if(H1) *H1 = -I_5x5;
if(H2) *H2 = I_5x5;
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
OptionalJacobian<5, 5> H1 = boost::none,
OptionalJacobian<5, 5> H2 = boost::none) const {
if (H1) *H1 = -I_5x5;
if (H2) *H2 = I_5x5;
return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
q.v0_ - v0_);
}
/// @}
/// @name Manifold
/// @{
/// return DOF, dimensionality of tangent space
inline size_t dim() const { return dimension; }
/// return DOF, dimensionality of tangent space
static size_t Dim() { return dimension; }
inline static size_t Dim() { return dimension; }
/// Given 5-dim tangent vector, create new calibration
inline Cal3_S2 retract(const Vector& d) const {
@ -212,27 +131,22 @@ public:
/// @name Advanced Interface
/// @{
private:
private:
/// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3_S2", boost::serialization::base_object<Cal3>(*this));
}
/// @}
};
template<>
template <>
struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
template<>
template <>
struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
} // \ namespace gtsam
} // \ namespace gtsam

50
gtsam/geometry/Cal3_S2Stereo.cpp
View File

@ -20,20 +20,56 @@
#include <iostream>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) {
os << (Cal3_S2&)cal;
os << ", b: " << cal.baseline();
return os;
}
/* ************************************************************************* */
void Cal3_S2Stereo::print(const std::string& s) const {
K_.print(s+"K: ");
std::cout << s << "Baseline: " << b_ << std::endl;
}
std::cout << s << (s != "" ? " " : "");
std::cout << "K: " << (Matrix)K() << std::endl;
std::cout << "Baseline: " << b_ << std::endl;
}
/* ************************************************************************* */
bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
if (std::abs(b_ - other.b_) > tol) return false;
return K_.equals(other.K_,tol);
const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
return (Cal3_S2::equals(*base, tol) &&
std::fabs(b_ - other.baseline()) < tol);
}
/* ************************************************************************* */
Point2 Cal3_S2Stereo::uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
OptionalJacobian<2, 2> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, 1.0, 0.0;
if (Dp) *Dp << fx_, s_, 0.0, fy_;
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2Stereo::calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
OptionalJacobian<2, 2> Dp) const {
const double u = p.x(), v = p.y();
double delta_u = u - u0_, delta_v = v - v0_;
double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
double inv_fy_delta_v = inv_fy * delta_v;
double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
if (Dcal) {
*Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
-inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, 0,
-inv_fy * point.y(), 0, 0, -inv_fy, 0;
}
if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
return point;
}
/* ************************************************************************* */
} // namespace gtsam
} // namespace gtsam

226
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -22,135 +22,143 @@
namespace gtsam {
/**
* @brief The most common 5DOF 3D->2D calibration, stereo version
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
private:
double b_ = 1.0f; ///< Stereo baseline.
public:
enum { dimension = 6 };
///< shared pointer to stereo calibration object
using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
/// @name Standard Constructors
/// @
/// default calibration leaves coordinates unchanged
Cal3_S2Stereo() = default;
/// constructor from doubles
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
: Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
/// constructor from vector
Cal3_S2Stereo(const Vector6& d)
: Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
/// easy constructor; field-of-view in degrees, assumes zero skew
Cal3_S2Stereo(double fov, int w, int h, double b)
: Cal3_S2(fov, w, h), b_(b) {}
/**
* @brief The most common 5DOF 3D->2D calibration, stereo version
* @addtogroup geometry
* \nosubgrouping
* Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
class GTSAM_EXPORT Cal3_S2Stereo {
private:
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Cal3_S2 K_;
double b_;
/**
* Convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
* @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
public:
/**
* Convert homogeneous image coordinates to intrinsic coordinates
* @param p point in image coordinates
* @return point in intrinsic coordinates
*/
Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
enum { dimension = 6 };
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr; ///< shared pointer to stereo calibration object
/// @}
/// @name Testable
/// @{
/// @name Standard Constructors
/// @
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3_S2Stereo& cal);
/// default calibration leaves coordinates unchanged
Cal3_S2Stereo() :
K_(1, 1, 0, 0, 0), b_(1.0) {
}
/// print with optional string
void print(const std::string& s = "") const override;
/// constructor from doubles
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
K_(fx, fy, s, u0, v0), b_(b) {
}
/// Check if equal up to specified tolerance
bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
/// constructor from vector
Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
/// @}
/// @name Standard Interface
/// @{
/// easy constructor; field-of-view in degrees, assumes zero skew
Cal3_S2Stereo(double fov, int w, int h, double b) :
K_(fov, w, h), b_(b) {
}
/// return calibration, same for left and right
const Cal3_S2& calibration() const { return *this; }
/// @}
/// @name Testable
/// @{
/// return calibration matrix K, same for left and right
Matrix3 K() const override { return Cal3_S2::K(); }
void print(const std::string& s = "") const;
/// return baseline
inline double baseline() const { return b_; }
/// Check if equal up to specified tolerance
bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
/// vectorized form (column-wise)
Vector6 vector() const {
Vector6 v;
v << Cal3_S2::vector(), b_;
return v;
}
/// @}
/// @name Standard Interface
/// @{
/// @}
/// @name Manifold
/// @{
/// return calibration, same for left and right
const Cal3_S2& calibration() const { return K_;}
/// return DOF, dimensionality of tangent space
inline size_t dim() const override { return Dim(); }
/// return calibration matrix K, same for left and right
Matrix matrix() const { return K_.matrix();}
/// return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
/// focal length x
inline double fx() const { return K_.fx();}
/// Given 6-dim tangent vector, create new calibration
inline Cal3_S2Stereo retract(const Vector& d) const {
return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
py() + d(4), b_ + d(5));
}
/// focal length x
inline double fy() const { return K_.fy();}
/// Unretraction for the calibration
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
return T2.vector() - vector();
}
/// skew
inline double skew() const { return K_.skew();}
/// @}
/// @name Advanced Interface
/// @{
/// image center in x
inline double px() const { return K_.px();}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
ar& BOOST_SERIALIZATION_NVP(b_);
}
/// @}
};
/// image center in y
inline double py() const { return K_.py();}
// Define GTSAM traits
template <>
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
/// return the principal point
Point2 principalPoint() const { return K_.principalPoint();}
template <>
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
/// return baseline
inline double baseline() const { return b_; }
/// vectorized form (column-wise)
Vector6 vector() const {
Vector6 v;
v << K_.vector(), b_;
return v;
}
/// @}
/// @name Manifold
/// @{
/// return DOF, dimensionality of tangent space
inline size_t dim() const { return dimension; }
/// return DOF, dimensionality of tangent space
static size_t Dim() { return dimension; }
/// Given 6-dim tangent vector, create new calibration
inline Cal3_S2Stereo retract(const Vector& d) const {
return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
}
/// Unretraction for the calibration
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
return T2.vector() - vector();
}
/// @}
/// @name Advanced Interface
/// @{
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/)
{
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(b_);
}
/// @}
};
// Define GTSAM traits
template<>
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
template<>
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
} // \ namespace gtsam
} // \ namespace gtsam

25
gtsam/geometry/CalibratedCamera.h
View File

@ -119,22 +119,20 @@ public:
/// @name Standard Constructors
/// @{
/** default constructor */
PinholeBase() {
}
/// Default constructor
PinholeBase() {}
/** constructor with pose */
explicit PinholeBase(const Pose3& pose) :
pose_(pose) {
}
/// Constructor with pose
explicit PinholeBase(const Pose3& pose) : pose_(pose) {}
/// @}
/// @name Advanced Constructors
/// @{
explicit PinholeBase(const Vector &v) :
pose_(Pose3::Expmap(v)) {
}
explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(v)) {}
/// Default destructor
virtual ~PinholeBase() = default;
/// @}
/// @name Testable
@ -144,7 +142,7 @@ public:
bool equals(const PinholeBase &camera, double tol = 1e-9) const;
/// print
void print(const std::string& s = "PinholeBase") const;
virtual void print(const std::string& s = "PinholeBase") const;
/// @}
/// @name Standard Interface
@ -324,6 +322,11 @@ public:
/// Return canonical coordinate
Vector localCoordinates(const CalibratedCamera& T2) const;
/// print
void print(const std::string& s = "CalibratedCamera") const override {
PinholeBase::print(s);
}
/// @deprecated
inline size_t dim() const {
return dimension;

98
gtsam/geometry/CameraSet.h
View File

@ -69,9 +69,12 @@ protected:
public:
/// Destructor
virtual ~CameraSet() = default;
/// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
using MatrixZD = Eigen::Matrix<double, ZDim, D>;
using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
/**
* print
@ -139,6 +142,57 @@ public:
return ErrorVector(project2(point, Fs, E), measured);
}
/**
* 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)
* 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) {
// 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));
// 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;
// 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));
// 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
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
@ -148,45 +202,7 @@ public:
template<int N> // N = 2 or 3
static SymmetricBlockMatrix SchurComplement(const FBlocks& 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();
// Create a SymmetricBlockMatrix
size_t M1 = D * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const MatrixZD& 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)
// (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 MatrixZD& 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
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
return SchurComplement<N,D>(Fs, E, P, b);
}
/// Computes Point Covariance P, with lambda parameter

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 {

21
gtsam/geometry/Line3.h
View File

@ -102,6 +102,27 @@ class Line3 {
*/
Point3 point(double distance = 0) const;
/**
* Return the rotation of the line.
*/
inline Rot3 R() const {
return R_;
}
/**
* Return the x-coordinate of the intersection of the line with the xy plane.
*/
inline double a() const {
return a_;
}
/**
* Return the y-coordinate of the intersection of the line with the xy plane.
*/
inline double b() const {
return b_;
}
/**
* Transform a line from world to camera frame
* @param wTc - Pose3 of camera in world frame

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