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

Merge branch 'release/4.1.0'

release/4.3a0
Fan Jiang 2021-12-20 16:56:43 -05:00
parent d5f256f3e2 4c0e87be78
commit 19a9e1e0e5
794 changed files with 72313 additions and 19072 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
.github/scripts/python.sh vendored Normal file
View File

@ -0,0 +1,127 @@
#!/bin/bash
##########################################################
# Build and test the GTSAM Python wrapper.
##########################################################
set -x -e
# install TBB with _debug.so files
function install_tbb()
{
TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download
TBB_VERSION=4.4.5
TBB_DIR=tbb44_20160526oss
TBB_SAVEPATH="/tmp/tbb.tgz"
if [ "$(uname)" == "Linux" ]; then
OS_SHORT="lin"
TBB_LIB_DIR="intel64/gcc4.4"
SUDO="sudo"
elif [ "$(uname)" == "Darwin" ]; then
OS_SHORT="osx"
TBB_LIB_DIR=""
SUDO=""
fi
wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH
tar -C /tmp -xf $TBB_SAVEPATH
TBBROOT=/tmp/$TBB_DIR
# Copy the needed files to the correct places.
# This works correctly for CI builds, instead of setting path variables.
# This is what Homebrew does to install TBB on Macs
$SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/
$SUDO cp -R $TBBROOT/include/ /usr/local/include/
}
if [ -z ${PYTHON_VERSION+x} ]; then
echo "Please provide the Python version to build against!"
exit 127
fi
if [ -z ${WRAPPER+x} ]; then
echo "Please provide the wrapper to build!"
exit 126
fi
PYTHON="python${PYTHON_VERSION}"
if [[ $(uname) == "Darwin" ]]; then
brew install wget
else
# Install a system package required by our library
sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools
fi
PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
case $WRAPPER in
"cython")
BUILD_CYTHON="ON"
BUILD_PYBIND="OFF"
TYPEDEF_POINTS_TO_VECTORS="OFF"
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/cython/requirements.txt
;;
"pybind")
BUILD_CYTHON="OFF"
BUILD_PYBIND="ON"
TYPEDEF_POINTS_TO_VECTORS="ON"
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
;;
*)
exit 126
;;
esac
mkdir $GITHUB_WORKSPACE/build
cd $GITHUB_WORKSPACE/build
cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_BUILD_TESTS=OFF -DGTSAM_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_INSTALL_CYTHON_TOOLBOX=${BUILD_CYTHON} \
-DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
-DGTSAM_TYPEDEF_POINTS_TO_VECTORS=${TYPEDEF_POINTS_TO_VECTORS} \
-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 &
while ps -p $! > /dev/null
do
sleep 60
now=$(date +%s)
printf "%d seconds have elapsed\n" $(( (now - start) ))
done
case $WRAPPER in
"cython")
cd $GITHUB_WORKSPACE/build/cython
$PYTHON setup.py install --user --prefix=
cd $GITHUB_WORKSPACE/build/cython/gtsam/tests
$PYTHON -m unittest discover
;;
"pybind")
cd $GITHUB_WORKSPACE/build/python
$PYTHON setup.py install --user --prefix=
cd $GITHUB_WORKSPACE/python/gtsam/tests
$PYTHON -m unittest discover
;;
*)
echo "THIS SHOULD NEVER HAPPEN!"
exit 125
;;
esac

36
.travis.sh → .github/scripts/unix.sh vendored Executable file → Normal file
View File

@ -1,20 +1,25 @@
#!/bin/bash
##########################################################
# Build and test GTSAM for *nix based systems.
# Specifically Linux and macOS.
##########################################################
# install TBB with _debug.so files
function install_tbb()
{
TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download
TBB_VERSION=4.4.2
TBB_DIR=tbb44_20151115oss
TBB_VERSION=4.4.5
TBB_DIR=tbb44_20160526oss
TBB_SAVEPATH="/tmp/tbb.tgz"
if [ "$TRAVIS_OS_NAME" == "linux" ]; then
if [ "$(uname)" == "Linux" ]; then
OS_SHORT="lin"
TBB_LIB_DIR="intel64/gcc4.4"
SUDO="sudo"
elif [ "$TRAVIS_OS_NAME" == "osx" ]; then
OS_SHORT="lin"
elif [ "$(uname)" == "Darwin" ]; then
OS_SHORT="osx"
TBB_LIB_DIR=""
SUDO=""
@ -25,7 +30,7 @@ function install_tbb()
TBBROOT=/tmp/$TBB_DIR
# Copy the needed files to the correct places.
# This works correctly for travis builds, instead of setting path variables.
# This works correctly for CI builds, instead of setting path variables.
# This is what Homebrew does to install TBB on Macs
$SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/
$SUDO cp -R $TBBROOT/include/ /usr/local/include/
@ -38,15 +43,14 @@ function configure()
set -e # Make sure any error makes the script to return an error code
set -x # echo
SOURCE_DIR=`pwd`
BUILD_DIR=build
SOURCE_DIR=$GITHUB_WORKSPACE
BUILD_DIR=$GITHUB_WORKSPACE/build
#env
git clean -fd || true
rm -fr $BUILD_DIR || true
mkdir $BUILD_DIR && cd $BUILD_DIR
install_tbb
[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
if [ ! -z "$GCC_VERSION" ]; then
export CC=gcc-$GCC_VERSION
@ -59,11 +63,13 @@ function configure()
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DCMAKE_VERBOSE_MAKEFILE=OFF
-DBOOST_ROOT=$BOOST_ROOT \
-DBoost_NO_SYSTEM_PATHS=ON \
-DBoost_ARCHITECTURE=-x64
}
@ -71,7 +77,7 @@ function configure()
function finish ()
{
# Print ccache stats
ccache -s
[ -x "$(command -v ccache)" ] && ccache -s
cd $SOURCE_DIR
}
@ -111,4 +117,4 @@ case $1 in
-t)
test
;;
esac
esac

78
.github/workflows/build-linux.yml vendored Normal file
View File

@ -0,0 +1,78 @@
name: Linux CI
on: [push, pull_request]
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
runs-on: ${{ matrix.os }}
env:
CTEST_OUTPUT_ON_FAILURE: ON
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
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: [
ubuntu-18.04-gcc-5,
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
compiler: gcc
version: "5"
- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
compiler: gcc
version: "9"
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
compiler: clang
version: "9"
steps:
- name: Checkout
uses: actions/checkout@master
- 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
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 "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}"
echo "::set-env name=CXX::clang++-${{ matrix.version }}"
fi
- name: Check Boost version
if: runner.os == 'Linux'
run: |
echo "BOOST_ROOT = $BOOST_ROOT"
- name: Build and Test (Linux)
if: runner.os == 'Linux'
run: |
bash .github/scripts/unix.sh -t

51
.github/workflows/build-macos.yml vendored Normal file
View File

@ -0,0 +1,51 @@
name: macOS CI
on: [pull_request]
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
runs-on: ${{ matrix.os }}
env:
CTEST_OUTPUT_ON_FAILURE: ON
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
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: [
macOS-10.15-xcode-11.3.1,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: macOS-10.15-xcode-11.3.1
os: macOS-10.15
compiler: xcode
version: "11.3.1"
steps:
- name: Checkout
uses: actions/checkout@master
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
fi
- name: Build and Test (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/unix.sh -t

108
.github/workflows/build-python.yml vendored Normal file
View File

@ -0,0 +1,108 @@
name: Python CI
on: [pull_request]
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }} Python ${{ matrix.python_version }}
runs-on: ${{ matrix.os }}
env:
CTEST_OUTPUT_ON_FAILURE: ON
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
PYTHON_VERSION: ${{ matrix.python_version }}
WRAPPER: ${{ matrix.wrapper }}
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: [
ubuntu-18.04-gcc-5,
# ubuntu-18.04-gcc-9, # TODO Disabled for now because of timeouts
ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1,
ubuntu-18.04-gcc-5-tbb,
]
build_type: [Debug, Release]
python_version: [3]
wrapper: [pybind]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
compiler: gcc
version: "5"
# TODO Disabled for now because of timeouts
# - name: ubuntu-18.04-gcc-9
# os: ubuntu-18.04
# compiler: gcc
# version: "9"
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
compiler: clang
version: "9"
- name: macOS-10.15-xcode-11.3.1
os: macOS-10.15
compiler: xcode
version: "11.3.1"
- name: ubuntu-18.04-gcc-5-tbb
os: ubuntu-18.04
compiler: gcc
version: "5"
flag: tbb
steps:
- name: Checkout
uses: actions/checkout@master
- 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
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
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}"
echo "::set-env name=CXX::clang++-${{ matrix.version }}"
fi
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
fi
- name: Set GTSAM_WITH_TBB Flag
if: matrix.flag == 'tbb'
run: |
echo "::set-env name=GTSAM_WITH_TBB::ON"
echo "GTSAM Uses TBB"
- name: Build (Linux)
if: runner.os == 'Linux'
run: |
bash .github/scripts/python.sh
- name: Build (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/python.sh

112
.github/workflows/build-special.yml vendored Normal file
View File

@ -0,0 +1,112 @@
name: Special Cases CI
on: [pull_request]
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
runs-on: ${{ matrix.os }}
env:
CTEST_OUTPUT_ON_FAILURE: ON
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ON
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:
[
ubuntu-gcc-deprecated,
ubuntu-gcc-quaternions,
ubuntu-gcc-tbb,
]
build_type: [Debug, Release]
include:
- name: ubuntu-gcc-deprecated
os: ubuntu-18.04
compiler: gcc
version: "9"
flag: deprecated
- name: ubuntu-gcc-quaternions
os: ubuntu-18.04
compiler: gcc
version: "9"
flag: quaternions
- name: ubuntu-gcc-tbb
os: ubuntu-18.04
compiler: gcc
version: "9"
flag: tbb
steps:
- name: Checkout
uses: actions/checkout@master
- 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
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 "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}"
echo "::set-env name=CXX::clang++-${{ matrix.version }}"
fi
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
echo "::set-env name=CXX::g++-${{ matrix.version }}"
else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
fi
- name: Set Allow Deprecated Flag
if: matrix.flag == 'deprecated'
run: |
echo "::set-env name=GTSAM_ALLOW_DEPRECATED_SINCE_V41::ON"
echo "Allow deprecated since version 4.1"
- name: Set Use Quaternions Flag
if: matrix.flag == 'quaternions'
run: |
echo "::set-env name=GTSAM_USE_QUATERNIONS::ON"
echo "Use Quaternions for rotations"
- name: Set GTSAM_WITH_TBB Flag
if: matrix.flag == 'tbb'
run: |
echo "::set-env name=GTSAM_WITH_TBB::ON"
echo "GTSAM Uses TBB"
- name: Build & Test
run: |
bash .github/scripts/unix.sh -t

75
.github/workflows/build-windows.yml vendored Normal file
View File

@ -0,0 +1,75 @@
name: Windows CI
on: [pull_request]
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
runs-on: ${{ matrix.os }}
env:
CTEST_OUTPUT_ON_FAILURE: ON
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
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: [
windows-2016-cl,
windows-2019-cl,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: windows-2016-cl
os: windows-2016
compiler: cl
- name: windows-2019-cl
os: windows-2019
compiler: cl
steps:
- name: Checkout
uses: actions/checkout@master
- name: Install (Windows)
if: runner.os == 'Windows'
shell: powershell
run: |
Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh')
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
# See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413
scoop install gcc --global
echo "::set-env name=CC::gcc"
echo "::set-env name=CXX::g++"
} elseif ("${{ matrix.compiler }}" -eq "clang") {
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
} else {
echo "::set-env name=CC::${{ matrix.compiler }}"
echo "::set-env name=CXX::${{ matrix.compiler }}"
}
# Scoop modifies the PATH so we make the modified PATH global.
echo "::set-env name=PATH::$env:PATH"
- name: Build (Windows)
if: runner.os == 'Windows'
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 --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

5
.github/workflows/trigger-python.yml vendored
View File

@ -1,6 +1,9 @@
# This triggers Cython builds on `gtsam-manylinux-build`
name: Trigger Python Builds
on: push
on:
push:
branches:
- develop
jobs:
triggerCython:
runs-on: ubuntu-latest

1
.gitignore vendored
View File

@ -21,3 +21,4 @@ cython/gtsam_wrapper.pxd
/CMakeSettings.json
# for QtCreator:
CMakeLists.txt.user*
xcode/

43
.travis.python.sh
View File

@ -1,43 +0,0 @@
#!/bin/bash
set -x -e
if [ -z ${PYTHON_VERSION+x} ]; then
echo "Please provide the Python version to build against!"
exit 127
fi
PYTHON="python${PYTHON_VERSION}"
if [[ $(uname) == "Darwin" ]]; then
brew install wget
else
# Install a system package required by our library
sudo apt-get install wget libicu-dev python3-pip python3-setuptools
fi
CURRDIR=$(pwd)
sudo $PYTHON -m pip install -r ./cython/requirements.txt
mkdir $CURRDIR/build
cd $CURRDIR/build
cmake $CURRDIR -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \
-DGTSAM_USE_QUATERNIONS=OFF \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF \
-DCMAKE_INSTALL_PREFIX=$CURRDIR/../gtsam_install
make -j$(nproc) install
cd cython
sudo $PYTHON setup.py install
cd $CURRDIR/cython/gtsam/tests
$PYTHON -m unittest discover

141
.travis.yml
View File

@ -1,141 +0,0 @@
language: cpp
cache: ccache
sudo: required
dist: xenial
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- sourceline: 'deb http://apt.llvm.org/xenial/ llvm-toolchain-xenial-9 main'
key_url: 'https://apt.llvm.org/llvm-snapshot.gpg.key'
packages:
- g++-9
- clang-9
- build-essential pkg-config
- cmake
- python3-dev libpython-dev
- python3-numpy
- libboost-all-dev
# before_install:
# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi
install:
- if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi
- if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi
# We first do the compile stage specified below, then the matrix expansion specified after.
stages:
- compile
- test
- special
env:
global:
- MAKEFLAGS="-j2"
- CCACHE_SLOPPINESS=pch_defines,time_macros
# Compile stage without building examples/tests to populate the caches.
jobs:
# -------- STAGE 1: COMPILE -----------
include:
# on Mac, GCC
- stage: compile
os: osx
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: osx
compiler: gcc
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Mac, CLANG
- stage: compile
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Linux, GCC
- stage: compile
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Linux, CLANG
- stage: compile
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Linux, with deprecated ON to make sure that path still compiles/tests
- stage: special
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
script: bash .travis.sh -b
# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests
- stage: special
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON
script: bash .travis.sh -t
# -------- STAGE 2: TESTS -----------
# on Mac, GCC
- stage: test
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -t
- stage: test
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -t
- stage: test
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -t
- stage: test
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -t
- stage: test
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -t
# on Linux, with quaternions ON to make sure that path still compiles/tests
- stage: special
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release GTSAM_BUILD_UNSTABLE=OFF GTSAM_USE_QUATERNIONS=ON
script: bash .travis.sh -t
- stage: special
os: linux
compiler: gcc
env: PYTHON_VERSION=3
script: bash .travis.python.sh
- stage: special
os: osx
compiler: clang
env: PYTHON_VERSION=3
script: bash .travis.python.sh

366
CMakeLists.txt
View File

@ -9,8 +9,8 @@ endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 3)
set (GTSAM_VERSION_MINOR 1)
set (GTSAM_VERSION_PATCH 0)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@ -65,18 +65,20 @@ add_custom_target(uninstall
# Configurable Options
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON)
option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
endif()
option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON)
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF)
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" 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)
if(NOT MSVC AND NOT XCODE_VERSION)
@ -99,37 +101,40 @@ 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)
option(GTSAM_INSTALL_CYTHON_TOOLBOX "Enable/Disable installation of Cython toolbox" OFF)
option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab/cython wrap utility (necessary for matlab/cython interface)" ON)
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build the cython wrapper for (or Default)")
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 OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
endif()
if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND GTSAM_BUILD_TYPE_POSTFIXES)
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
endif()
if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
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.")
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()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
endif()
if(GTSAM_BUILD_PYTHON)
if(GTSAM_UNSTABLE_BUILD_PYTHON)
if (NOT GTSAM_BUILD_UNSTABLE)
message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")
set(GTSAM_UNSTABLE_BUILD_PYTHON OFF)
endif()
endif()
if(GTSAM_INSTALL_CYTHON_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
message(FATAL_ERROR "GTSAM_INSTALL_CYTHON_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the CYTHON toolbox cannot deal with this yet. Please turn one of the two options off.")
set(GTSAM_PY_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/python")
endif()
# Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
if (CMAKE_GENERATOR STREQUAL "Ninja" AND
((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9) OR
(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.5)))
# Force colored warnings in Ninja's output, if the compiler has -fdiagnostics-color support.
# Rationale in https://github.com/ninja-build/ninja/issues/814
add_compile_options(-fdiagnostics-color=always)
endif()
###############################################################################
# Find boost
@ -138,18 +143,18 @@ set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
if(MSVC)
# By default, boost only builds static libraries on windows
set(Boost_USE_STATIC_LIBS ON) # only find static libs
# If we ever reset above on windows and, ...
# If we use Boost shared libs, disable auto linking.
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
if(NOT Boost_USE_STATIC_LIBS)
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
endif()
# Virtual memory range for PCH exceeded on VS2015
if(MSVC_VERSION LESS 1910) # older than VS2017
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295)
endif()
# By default, boost only builds static libraries on windows
set(Boost_USE_STATIC_LIBS ON) # only find static libs
# If we ever reset above on windows and, ...
# If we use Boost shared libs, disable auto linking.
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
if(NOT Boost_USE_STATIC_LIBS)
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
endif()
# Virtual memory range for PCH exceeded on VS2015
if(MSVC_VERSION LESS 1910) # older than VS2017
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295)
endif()
endif()
# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT()
@ -157,7 +162,7 @@ endif()
# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017
#
if(MSVC AND BUILD_SHARED_LIBS)
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
endif()
# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
@ -227,16 +232,16 @@ find_package(GooglePerfTools)
###############################################################################
# Support ccache, if installed
if(NOT MSVC AND NOT XCODE_VERSION)
find_program(CCACHE_FOUND ccache)
if(CCACHE_FOUND)
if(GTSAM_BUILD_WITH_CCACHE)
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
else()
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "")
set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "")
endif()
endif(CCACHE_FOUND)
find_program(CCACHE_FOUND ccache)
if(CCACHE_FOUND)
if(GTSAM_BUILD_WITH_CCACHE)
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
else()
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "")
set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "")
endif()
endif(CCACHE_FOUND)
endif()
###############################################################################
@ -280,74 +285,74 @@ option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF)
# Switch for using system Eigen or GTSAM-bundled Eigen
if(GTSAM_USE_SYSTEM_EIGEN)
find_package(Eigen3 REQUIRED)
find_package(Eigen3 REQUIRED)
# Use generic Eigen include paths e.g. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
# Use generic Eigen include paths e.g. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
# check if MKL is also enabled - can have one or the other, but not both!
# Note: Eigen >= v3.2.5 includes our patches
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
endif()
# check if MKL is also enabled - can have one or the other, but not both!
# Note: Eigen >= v3.2.5 includes our patches
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
endif()
# Check for Eigen version which doesn't work with MKL
# See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details.
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
endif()
# Check for Eigen version which doesn't work with MKL
# See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details.
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
endif()
# The actual include directory (for BUILD cmake target interface):
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
# The actual include directory (for BUILD cmake target interface):
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
else()
# Use bundled Eigen include path.
# Clear any variables set by FindEigen3
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
endif()
# Use bundled Eigen include path.
# Clear any variables set by FindEigen3
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
endif()
# set full path to be used by external projects
# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
# set full path to be used by external projects
# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
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/")
# The actual include directory (for BUILD cmake target interface):
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
endif()
# Detect Eigen version:
set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h")
if (EXISTS ${EIGEN_VER_H})
file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION)
file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION)
# Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION XX", etc...
# Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION XX", etc...
string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}")
string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}")
string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}")
string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}")
string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}")
string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}")
string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}")
string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}")
string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}")
string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}")
string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}")
string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}")
set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}")
set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}")
message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}")
message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}")
else()
message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`")
message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`")
endif ()
if (MSVC)
if (BUILD_SHARED_LIBS)
# mute eigen static assert to avoid errors in shared lib
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
endif()
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
if (BUILD_SHARED_LIBS)
# mute eigen static assert to avoid errors in shared lib
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
endif()
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
endif()
if (APPLE AND BUILD_SHARED_LIBS)
# Set the default install directory on macOS
set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
# Set the default install directory on macOS
set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
endif()
###############################################################################
@ -356,42 +361,42 @@ endif()
# Build list of possible allocators
set(possible_allocators "")
if(GTSAM_USE_TBB)
list(APPEND possible_allocators TBB)
set(preferred_allocator TBB)
list(APPEND possible_allocators TBB)
set(preferred_allocator TBB)
else()
list(APPEND possible_allocators BoostPool STL)
set(preferred_allocator STL)
list(APPEND possible_allocators BoostPool STL)
set(preferred_allocator STL)
endif()
if(GOOGLE_PERFTOOLS_FOUND)
list(APPEND possible_allocators tcmalloc)
list(APPEND possible_allocators tcmalloc)
endif()
# Check if current allocator choice is valid and set cache option
list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid)
if(allocator_valid EQUAL -1)
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE)
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE)
else()
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator")
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator")
endif()
set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators})
mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR)
# Define compile flags depending on allocator
if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool")
set(GTSAM_ALLOCATOR_BOOSTPOOL 1)
set(GTSAM_ALLOCATOR_BOOSTPOOL 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL")
set(GTSAM_ALLOCATOR_STL 1)
set(GTSAM_ALLOCATOR_STL 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB")
set(GTSAM_ALLOCATOR_TBB 1)
set(GTSAM_ALLOCATOR_TBB 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc")
set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator
list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator
list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
endif()
if(MSVC)
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code
endif()
# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
@ -419,14 +424,11 @@ endif()
# Build CppUnitLite
add_subdirectory(CppUnitLite)
# Build wrap
if (GTSAM_BUILD_WRAP)
add_subdirectory(wrap)
# suppress warning of cython line being too long
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation")
endif()
endif(GTSAM_BUILD_WRAP)
# 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)
@ -447,23 +449,9 @@ endif()
# Matlab toolbox
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
add_subdirectory(matlab)
add_subdirectory(matlab)
endif()
# Cython wrap
if (GTSAM_INSTALL_CYTHON_TOOLBOX)
set(GTSAM_INSTALL_CYTHON_TOOLBOX 1)
# Set up cache options
# Cython install path appended with Build type (e.g. cython, cythonDebug, etc).
# This does not override custom values set from the command line
set(GTSAM_CYTHON_INSTALL_PATH "${PROJECT_BINARY_DIR}/cython${GTSAM_BUILD_TAG}" CACHE PATH "Cython toolbox destination, blank defaults to PROJECT_BINARY_DIR/cython<GTSAM_BUILD_TAG>")
set(GTSAM_EIGENCY_INSTALL_PATH ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency)
add_subdirectory(cython ${GTSAM_CYTHON_INSTALL_PATH})
else()
set(GTSAM_INSTALL_CYTHON_TOOLBOX 0) # This will go into config.h
endif()
# Install config and export files
GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
@ -509,116 +497,116 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>=
# Print configuration variables
message(STATUS "===============================================================")
message(STATUS "================ Configuration Options ======================")
message(STATUS " CMAKE_CXX_COMPILER_ID type : ${CMAKE_CXX_COMPILER_ID}")
message(STATUS " CMAKE_CXX_COMPILER_VERSION : ${CMAKE_CXX_COMPILER_VERSION}")
message(STATUS " CMake version : ${CMAKE_VERSION}")
message(STATUS " CMake generator : ${CMAKE_GENERATOR}")
message(STATUS " CMake build tool : ${CMAKE_BUILD_TOOL}")
print_config("CMAKE_CXX_COMPILER_ID type" "${CMAKE_CXX_COMPILER_ID}")
print_config("CMAKE_CXX_COMPILER_VERSION" "${CMAKE_CXX_COMPILER_VERSION}")
print_config("CMake version" "${CMAKE_VERSION}")
print_config("CMake generator" "${CMAKE_GENERATOR}")
print_config("CMake build tool" "${CMAKE_BUILD_TOOL}")
message(STATUS "Build flags ")
print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ")
print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ")
print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'")
print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests")
print_enabled_config(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all'")
print_enabled_config(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'")
if (DOXYGEN_FOUND)
print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ")
print_enabled_config(${GTSAM_BUILD_DOCS} "Build Docs")
endif()
print_config_flag(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries ")
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ")
print_enabled_config(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries")
print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name")
if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
print_enabled_config(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
print_enabled_config(${GTSAM_UNSTABLE_BUILD_PYTHON} "Build GTSAM unstable Python ")
print_enabled_config(${GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX} "Build MATLAB Toolbox for unstable")
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
print_config_flag(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ")
message(STATUS " Build type : ${CMAKE_BUILD_TYPE}")
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
print_enabled_config(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ")
print_config("Build type" "${CMAKE_BUILD_TYPE}")
print_config("C compilation flags" "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
print_config("C++ compilation flags" "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
endif()
print_build_options_for_target(gtsam)
message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
if(GTSAM_USE_TBB)
message(STATUS " Use Intel TBB : Yes")
print_config("Use Intel TBB" "Yes")
elseif(TBB_FOUND)
message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled")
print_config("Use Intel TBB" "TBB found but GTSAM_WITH_TBB is disabled")
else()
message(STATUS " Use Intel TBB : TBB not found")
print_config("Use Intel TBB" "TBB not found")
endif()
if(GTSAM_USE_EIGEN_MKL)
message(STATUS " Eigen will use MKL : Yes")
print_config("Eigen will use MKL" "Yes")
elseif(MKL_FOUND)
message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
print_config("Eigen will use MKL" "MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
else()
message(STATUS " Eigen will use MKL : MKL not found")
print_config("Eigen will use MKL" "MKL not found")
endif()
if(GTSAM_USE_EIGEN_MKL_OPENMP)
message(STATUS " Eigen will use MKL and OpenMP : Yes")
print_config("Eigen will use MKL and OpenMP" "Yes")
elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
elseif(OPENMP_FOUND AND NOT MKL_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found")
print_config("Eigen will use MKL and OpenMP" "OpenMP found but MKL not found")
elseif(OPENMP_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
else()
message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found")
print_config("Eigen will use MKL and OpenMP" "OpenMP not found")
endif()
message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}")
print_config("Default allocator" "${GTSAM_DEFAULT_ALLOCATOR}")
if(GTSAM_THROW_CHEIRALITY_EXCEPTION)
message(STATUS " Cheirality exceptions enabled : YES")
print_config("Cheirality exceptions enabled" "YES")
else()
message(STATUS " Cheirality exceptions enabled : NO")
print_config("Cheirality exceptions enabled" "NO")
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
message(STATUS " Build with ccache : Yes")
elseif(CCACHE_FOUND)
message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else()
message(STATUS " Build with ccache : No")
endif()
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
print_config("Build with ccache" "Yes")
elseif(CCACHE_FOUND)
print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else()
print_config("Build with ccache" "No")
endif()
endif()
message(STATUS "Packaging flags ")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "Packaging flags")
print_config("CPack Source Generator" "${CPACK_SOURCE_GENERATOR}")
print_config("CPack Generator" "${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1")
print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ")
message(STATUS "MATLAB toolbox flags")
print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ")
if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
message(STATUS " MATLAB root : ${MATLAB_ROOT}")
message(STATUS " MEX binary : ${MEX_COMMAND}")
print_config("MATLAB root" "${MATLAB_ROOT}")
print_config("MEX binary" "${MEX_COMMAND}")
endif()
message(STATUS "Python toolbox flags ")
print_enabled_config(${GTSAM_BUILD_PYTHON} "Build Python module with pybind ")
if(GTSAM_BUILD_PYTHON)
print_config("Python version" ${GTSAM_PYTHON_VERSION})
endif()
message(STATUS "Cython toolbox flags ")
print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ")
if(GTSAM_INSTALL_CYTHON_TOOLBOX)
message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}")
endif()
message(STATUS "===============================================================")
# Print warnings at the end
if(GTSAM_WITH_TBB AND NOT TBB_FOUND)
message(WARNING "TBB 4.4 or newer was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
message(WARNING "TBB 4.4 or newer was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
endif()
if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND)
message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning. See INSTALL.md for notes on performance.")
message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning. See INSTALL.md for notes on performance.")
endif()
if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.")
message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.")
endif()
# Include CPack *after* all flags

4
CppUnitLite/Test.h
View File

@ -64,7 +64,7 @@ protected:
class testGroup##testName##Test : public Test \
{ public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \
virtual ~testGroup##testName##Test () {};\
void run (TestResult& result_);} \
void run (TestResult& result_) override;} \
testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_)
@ -82,7 +82,7 @@ protected:
class testGroup##testName##Test : public Test \
{ public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \
virtual ~testGroup##testName##Test () {};\
void run (TestResult& result_);} \
void run (TestResult& result_) override;} \
testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_)

2
LICENSE
View File

@ -23,3 +23,5 @@ ordering library
- Included unmodified in gtsam/3rdparty/metis
- Licenced under Apache License v 2.0, provided in
gtsam/3rdparty/metis/LICENSE.txt
- Spectra v0.9.0: Sparse Eigenvalue Computation Toolkit as a Redesigned ARPACK.
- Licenced under MPL2, provided at https://github.com/yixuan/spectra

26
README.md
View File

@ -1,5 +1,11 @@
# README - Georgia Tech Smoothing and Mapping Library
**Important Note**
As of August 1 2020, the `develop` branch is officially in "Pre 4.1" mode, and features deprecated in 4.0 have been removed. Please use the last [4.0.3 release](https://github.com/borglab/gtsam/releases/tag/4.0.3) if you need those features.
However, most are easily converted and can be tracked down (in 4.0.3) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4`.
## What is GTSAM?
GTSAM is a C++ library that implements smoothing and
@ -7,13 +13,16 @@ mapping (SAM) in robotics and vision, using Factor Graphs and Bayes
Networks as the underlying computing paradigm rather than sparse
matrices.
| Platform | Build Status |
|:---------:|:-------------:|
| gcc/clang | [![Build Status](https://travis-ci.com/borglab/gtsam.svg?branch=develop)](https://travis-ci.com/borglab/gtsam/) |
| MSVC | [![Build status](https://ci.appveyor.com/api/projects/status/3enllitj52jsxwfg/branch/develop?svg=true)](https://ci.appveyor.com/project/dellaert/gtsam) |
The current support matrix is:
| Platform | Compiler | Build Status |
|:------------:|:---------:|:-------------:|
| Ubuntu 18.04 | gcc/clang | ![Linux CI](https://github.com/borglab/gtsam/workflows/Linux%20CI/badge.svg) |
| macOS | clang | ![macOS CI](https://github.com/borglab/gtsam/workflows/macOS%20CI/badge.svg) |
| Windows | MSVC | ![Windows CI](https://github.com/borglab/gtsam/workflows/Windows%20CI/badge.svg) |
On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](##Wrappers).
On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](#wrappers).
## Quickstart
@ -44,9 +53,12 @@ Optional prerequisites - used automatically if findable by CMake:
## GTSAM 4 Compatibility
GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. We also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
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.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.
Also, GTSAM 4 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.
## Wrappers

33
appveyor.yml
View File

@ -1,33 +0,0 @@
# version format
version: 4.0.3-{branch}-build{build}
os: Visual Studio 2019
clone_folder: c:\projects\gtsam
platform: x64
configuration: Release
environment:
CTEST_OUTPUT_ON_FAILURE: 1
BOOST_ROOT: C:/Libraries/boost_1_71_0
build_script:
- cd c:\projects\gtsam\build
# As of Dec 2019, not all unit tests build cleanly for MSVC, so we'll just
# check that parts of GTSAM build correctly:
#- cmake --build .
- cmake --build . --config Release --target gtsam
- cmake --build . --config Release --target gtsam_unstable
- cmake --build . --config Release --target wrap
#- cmake --build . --target check
- cmake --build . --config Release --target check.base
- cmake --build . --config Release --target check.base_unstable
- cmake --build . --config Release --target check.linear
before_build:
- cd c:\projects\gtsam
- mkdir build
- cd build
# Disable examples to avoid AppVeyor timeout
- cmake -G "Visual Studio 16 2019" .. -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF

2
cmake/CMakeLists.txt
View File

@ -17,8 +17,6 @@ install(FILES
GtsamBuildTypes.cmake
GtsamMakeConfigFile.cmake
GtsamMatlabWrap.cmake
GtsamPythonWrap.cmake
GtsamCythonWrap.cmake
GtsamTesting.cmake
GtsamPrinting.cmake
FindCython.cmake

20
cmake/GtsamBuildTypes.cmake
View File

@ -104,8 +104,24 @@ if(MSVC)
set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING /MD /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
else()
# Common to all configurations, next for each configuration:
# "-fPIC" is to ensure proper code generation for shared libraries
set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON -Wall -fPIC CACHE STRING "(User editable) Private compiler flags for all configurations.")
if (
((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR
(CMAKE_CXX_COMPILER_ID MATCHES "GNU")
)
set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
endif()
set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON
-Wall # Enable common warnings
-fPIC # ensure proper code generation for shared libraries
$<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address
-Wreturn-type -Werror=return-type # Error on missing return()
-Wformat -Werror=format-security # Error on wrong printf() arguments
$<$<COMPILE_LANGUAGE:CXX>:${flag_override_}> # Enforce the use of the override keyword
#
CACHE STRING "(User editable) Private compiler flags for all configurations.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO -g -O3 CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE -O3 CACHE STRING "(User editable) Private compiler flags for Release configuration.")

204
cmake/GtsamCythonWrap.cmake
View File

@ -1,204 +0,0 @@
# Check Cython version, need to be >=0.25.2
# Unset these cached variables to avoid surprises when the python/cython
# in the current environment are different from the cached!
unset(PYTHON_EXECUTABLE CACHE)
unset(CYTHON_EXECUTABLE CACHE)
unset(PYTHON_INCLUDE_DIR CACHE)
unset(PYTHON_MAJOR_VERSION CACHE)
unset(PYTHON_LIBRARY CACHE)
# Allow override from command line
if(NOT DEFINED GTSAM_USE_CUSTOM_PYTHON_LIBRARY)
if(GTSAM_PYTHON_VERSION STREQUAL "Default")
find_package(PythonInterp REQUIRED)
find_package(PythonLibs REQUIRED)
else()
find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
endif()
endif()
find_package(Cython 0.25.2 REQUIRED)
execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
"from __future__ import print_function;import sys;print(sys.version[0], end='')"
OUTPUT_VARIABLE PYTHON_MAJOR_VERSION
)
# User-friendly Cython wrapping and installing function.
# Builds a Cython module from the provided interface_header.
# For example, for the interface header gtsam.h,
# this will build the wrap module 'gtsam'.
#
# Arguments:
#
# interface_header: The relative path to the wrapper interface definition file.
# extra_imports: extra header to import in the Cython pxd file.
# For example, to use Cython gtsam.pxd in your own module,
# use "from gtsam cimport *"
# install_path: destination to install the library
# libs: libraries to link with
# dependencies: Dependencies which need to be built before the wrapper
function(wrap_and_install_library_cython interface_header extra_imports install_path libs dependencies)
# Paths for generated files
get_filename_component(module_name "${interface_header}" NAME_WE)
set(generated_files_path "${install_path}")
wrap_library_cython("${interface_header}" "${generated_files_path}" "${extra_imports}" "${libs}" "${dependencies}")
endfunction()
function(set_up_required_cython_packages)
# Set up building of cython module
include_directories(${PYTHON_INCLUDE_DIRS})
find_package(NumPy REQUIRED)
include_directories(${NUMPY_INCLUDE_DIRS})
endfunction()
# Convert pyx to cpp by executing cython
# This is the first step to compile cython from the command line
# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
#
# Arguments:
# - target: The specified target for this step
# - pyx_file: The input pyx_file in full *absolute* path
# - generated_cpp: The output cpp file in full absolute path
# - include_dirs: Directories to include when executing cython
function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
foreach(dir ${include_dirs})
set(includes_for_cython ${includes_for_cython} -I ${dir})
endforeach()
add_custom_command(
OUTPUT ${generated_cpp}
COMMAND
${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus -${PYTHON_MAJOR_VERSION} ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
VERBATIM)
add_custom_target(${target} ALL DEPENDS ${generated_cpp})
endfunction()
# Build the cpp file generated by converting pyx using cython
# This is the second step to compile cython from the command line
# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
#
# Arguments:
# - target: The specified target for this step
# - cpp_file: The input cpp_file in full *absolute* path
# - output_lib_we: The output lib filename only (without extension)
# - output_dir: The output directory
function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
add_library(${target} MODULE ${cpp_file})
if(WIN32)
# Use .pyd extension instead of .dll on Windows
set_target_properties(${target} PROPERTIES SUFFIX ".pyd")
# Add full path to the Python library
target_link_libraries(${target} ${PYTHON_LIBRARIES})
endif()
if(APPLE)
set(link_flags "-undefined dynamic_lookup")
endif()
set_target_properties(${target}
PROPERTIES COMPILE_FLAGS "-w"
LINK_FLAGS "${link_flags}"
OUTPUT_NAME ${output_lib_we}
PREFIX ""
${CMAKE_BUILD_TYPE_UPPER}_POSTFIX ""
LIBRARY_OUTPUT_DIRECTORY ${output_dir})
endfunction()
# Cythonize a pyx from the command line as described at
# http://cython.readthedocs.io/en/latest/src/reference/compilation.html
# Arguments:
# - target: The specified target
# - pyx_file: The input pyx_file in full *absolute* path
# - output_lib_we: The output lib filename only (without extension)
# - output_dir: The output directory
# - include_dirs: Directories to include when executing cython
# - libs: Libraries to link with
# - interface_header: For dependency. Any update in interface header will re-trigger cythonize
function(cythonize target pyx_file output_lib_we output_dir include_dirs libs interface_header dependencies)
get_filename_component(pyx_path "${pyx_file}" DIRECTORY)
get_filename_component(pyx_name "${pyx_file}" NAME_WE)
set(generated_cpp "${output_dir}/${pyx_name}.cpp")
set_up_required_cython_packages()
pyx_to_cpp(${target}_pyx2cpp ${pyx_file} ${generated_cpp} "${include_dirs}")
# Late dependency injection, to make sure this gets called whenever the interface header is updated
# See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} ${pyx_file} APPEND)
if (NOT "${dependencies}" STREQUAL "")
add_dependencies(${target}_pyx2cpp "${dependencies}")
endif()
build_cythonized_cpp(${target} ${generated_cpp} ${output_lib_we} ${output_dir})
if (NOT "${libs}" STREQUAL "")
target_link_libraries(${target} "${libs}")
endif()
add_dependencies(${target} ${target}_pyx2cpp)
if(TARGET ${python_install_target})
add_dependencies(${python_install_target} ${target})
endif()
endfunction()
# Internal function that wraps a library and compiles the wrapper
function(wrap_library_cython interface_header generated_files_path extra_imports libs dependencies)
# Wrap codegen interface
# Extract module path and name from interface header file name
# wrap requires interfacePath to be *absolute*
get_filename_component(interface_header "${interface_header}" ABSOLUTE)
get_filename_component(module_path "${interface_header}" PATH)
get_filename_component(module_name "${interface_header}" NAME_WE)
# Wrap module to Cython pyx
message(STATUS "Cython wrapper generating ${generated_files_path}/${module_name}.pyx")
set(generated_pyx "${generated_files_path}/${module_name}.pyx")
if(NOT EXISTS ${generated_files_path})
file(MAKE_DIRECTORY "${generated_files_path}")
endif()
add_custom_command(
OUTPUT ${generated_pyx}
DEPENDS ${interface_header} wrap
COMMAND
wrap --cython ${module_path} ${module_name} ${generated_files_path} "${extra_imports}"
VERBATIM
WORKING_DIRECTORY ${generated_files_path}/../)
add_custom_target(cython_wrap_${module_name}_pyx ALL DEPENDS ${generated_pyx})
if(NOT "${dependencies}" STREQUAL "")
add_dependencies(cython_wrap_${module_name}_pyx ${dependencies})
endif()
message(STATUS "Cythonize and build ${module_name}.pyx")
get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
cythonize(cythonize_${module_name} ${generated_pyx} ${module_name}
${generated_files_path} "${include_dirs}" "${libs}" ${interface_header} cython_wrap_${module_name}_pyx)
# distclean
add_custom_target(wrap_${module_name}_cython_distclean
COMMAND cmake -E remove_directory ${generated_files_path})
endfunction()
# Helper function to install Cython scripts and handle multiple build types where the scripts
# should be installed to all build type toolboxes
#
# Arguments:
# source_directory: The source directory to be installed. "The last component of each directory
# name is appended to the destination directory but a trailing slash may be
# used to avoid this because it leaves the last component empty."
# (https://cmake.org/cmake/help/v3.3/command/install.html?highlight=install#installing-directories)
# dest_directory: The destination directory to install to.
# patterns: list of file patterns to install
function(install_cython_scripts source_directory dest_directory patterns)
set(patterns_args "")
set(exclude_patterns "")
foreach(pattern ${patterns})
list(APPEND patterns_args PATTERN "${pattern}")
endforeach()
file(COPY "${source_directory}" DESTINATION "${dest_directory}"
FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endfunction()

38
cmake/GtsamMatlabWrap.cmake
View File

@ -23,6 +23,11 @@ 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
@ -209,15 +214,34 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
# Set up generation of module source file
file(MAKE_DIRECTORY "${generated_files_path}")
if(GTSAM_PYTHON_VERSION STREQUAL "Default")
find_package(PythonInterp REQUIRED)
find_package(PythonLibs REQUIRED)
else()
find_package(PythonInterp
${GTSAM_PYTHON_VERSION}
EXACT
REQUIRED)
find_package(PythonLibs
${GTSAM_PYTHON_VERSION}
EXACT
REQUIRED)
endif()
set(_ignore gtsam::Point2
gtsam::Point3)
add_custom_command(
OUTPUT ${generated_cpp_file}
DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
COMMAND
wrap --matlab
${modulePath}
${moduleName}
${generated_files_path}
${matlab_h_path}
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})

32
cmake/GtsamPrinting.cmake
View File

@ -1,14 +1,3 @@
# print configuration variables
# Usage:
#print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ")
function(print_config_flag flag msg)
if (flag)
message(STATUS " ${msg}: Enabled")
else ()
message(STATUS " ${msg}: Disabled")
endif ()
endfunction()
# Based on https://github.com/jimbraun/XCDF/blob/master/cmake/CMakePadString.cmake
function(string_pad RESULT_NAME DESIRED_LENGTH VALUE)
string(LENGTH "${VALUE}" VALUE_LENGTH)
@ -26,6 +15,27 @@ endfunction()
set(GTSAM_PRINT_SUMMARY_PADDING_LENGTH 50 CACHE STRING "Padding of cmake summary report lines after configuring.")
mark_as_advanced(GTSAM_PRINT_SUMMARY_PADDING_LENGTH)
# print configuration variables with automatic padding
# Usage:
# print_config(${GTSAM_BUILD_TESTS} "Build Tests")
function(print_config config msg)
string_pad(padded_config ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${config}")
message(STATUS "${padded_config}: ${msg}")
endfunction()
# print configuration variable with enabled/disabled value
# Usage:
# print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests ")
function(print_enabled_config config msg)
string_pad(padded_msg ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${msg}")
if (config)
message(STATUS "${padded_msg}: Enabled")
else ()
message(STATUS "${padded_msg}: Disabled")
endif ()
endfunction()
# Print " var: ${var}" padding with spaces as needed
function(print_padded variable_name)
string_pad(padded_prop ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${variable_name}")

102
cmake/GtsamPythonWrap.cmake
View File

@ -1,102 +0,0 @@
#Setup cache options
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version")
set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation")
set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python")
if(NOT GTSAM_PYTHON_INSTALL_PATH)
set(GTSAM_PYTHON_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/borg/python")
endif()
#Author: Paul Furgale Modified by Andrew Melim
function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
# # Boost
# find_package(Boost COMPONENTS python filesystem system REQUIRED)
# include_directories(${Boost_INCLUDE_DIRS})
# # Find Python
# FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
# INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
IF(APPLE)
# The apple framework headers don't include the numpy headers for some reason.
GET_FILENAME_COMPONENT(REAL_PYTHON_INCLUDE ${PYTHON_INCLUDE_DIRS} REALPATH)
IF( ${REAL_PYTHON_INCLUDE} MATCHES Python.framework)
message("Trying to find extra headers for numpy from ${REAL_PYTHON_INCLUDE}.")
message("Looking in ${REAL_PYTHON_INCLUDE}/../../Extras/lib/python/numpy/core/include/numpy")
FIND_PATH(NUMPY_INCLUDE_DIR arrayobject.h
${REAL_PYTHON_INCLUDE}/../../Extras/lib/python/numpy/core/include/numpy
${REAL_PYTHON_INCLUDE}/numpy
)
IF(${NUMPY_INCLUDE_DIR} MATCHES NOTFOUND)
message("Unable to find numpy include directories: ${NUMPY_INCLUDE_DIR}")
ELSE()
message("Found headers at ${NUMPY_INCLUDE_DIR}")
INCLUDE_DIRECTORIES(${NUMPY_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${NUMPY_INCLUDE_DIR}/..)
ENDIF()
ENDIF()
ENDIF(APPLE)
if(MSVC)
add_library(${moduleName}_python MODULE ${ARGN})
set_target_properties(${moduleName}_python PROPERTIES
OUTPUT_NAME ${moduleName}_python
CLEAN_DIRECT_OUTPUT 1
VERSION 1
SOVERSION 0
SUFFIX ".pyd")
target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
message(${PYLIB_OUTPUT_FILE})
get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd)
ELSE()
# Create a shared library
add_library(${moduleName}_python SHARED ${generated_cpp_file})
set_target_properties(${moduleName}_python PROPERTIES
OUTPUT_NAME ${moduleName}_python
CLEAN_DIRECT_OUTPUT 1)
target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
# On OSX and Linux, the python library must end in the extension .so. Build this
# filename here.
get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION)
set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
message(${PYLIB_OUTPUT_FILE})
get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
set(PYLIB_SO_NAME lib${moduleName}_python.so)
ENDIF(MSVC)
# Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package
set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam)
# Cause the library to be output in the correct directory.
add_custom_command(TARGET ${moduleName}_python
POST_BUILD
COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
COMMENT "Copying library files to python directory" )
# Cause the library to be output in the correct directory.
add_custom_command(TARGET ${TARGET_NAME}
POST_BUILD
COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
COMMENT "Copying library files to python directory" )
get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES)
list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME})
set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}")
endfunction(wrap_python)
# Macro to get list of subdirectories
macro(SUBDIRLIST result curdir)
file(GLOB children RELATIVE ${curdir} ${curdir}/*)
set(dirlist "")
foreach(child ${children})
if(IS_DIRECTORY ${curdir}/${child})
list(APPEND dirlist ${child})
endif()
endforeach()
set(${result} ${dirlist})
endmacro()

5
cmake/dllexport.h.in
View File

@ -47,9 +47,14 @@
# endif
# endif
#else
#ifdef __APPLE__
# define @library_name@_EXPORT __attribute__((visibility("default")))
# define @library_name@_EXTERN_EXPORT extern
#else
# define @library_name@_EXPORT
# define @library_name@_EXTERN_EXPORT extern
#endif
#endif
#undef BUILD_SHARED_LIBS

57
cython/CMakeLists.txt
View File

@ -1,57 +0,0 @@
# Install cython components
include(GtsamCythonWrap)
# Create the cython toolbox for the gtsam library
if (GTSAM_INSTALL_CYTHON_TOOLBOX)
# Add the new make target command
set(python_install_target python-install)
add_custom_target(${python_install_target}
COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_CYTHON_INSTALL_PATH}/setup.py install
WORKING_DIRECTORY ${GTSAM_CYTHON_INSTALL_PATH})
# build and include the eigency version of eigency
add_subdirectory(gtsam_eigency)
include_directories(${GTSAM_EIGENCY_INSTALL_PATH})
# Fix for error "C1128: number of sections exceeded object file format limit"
if(MSVC)
add_compile_options(/bigobj)
endif()
# First set up all the package related files.
# This also ensures the below wrap operations work correctly.
set(CYTHON_INSTALL_REQUIREMENTS_FILE "${PROJECT_SOURCE_DIR}/cython/requirements.txt")
# Install the custom-generated __init__.py
# This makes the cython (sub-)directories into python packages, so gtsam can be found while wrapping gtsam_unstable
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${GTSAM_CYTHON_INSTALL_PATH}/gtsam/__init__.py COPYONLY)
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable/__init__.py COPYONLY)
configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${GTSAM_CYTHON_INSTALL_PATH}/setup.py)
# Wrap gtsam
add_custom_target(gtsam_header DEPENDS "../gtsam.h")
wrap_and_install_library_cython("../gtsam.h" # interface_header
"" # extra imports
"${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path
gtsam # library to link with
"wrap;cythonize_eigency;gtsam;gtsam_header" # dependencies which need to be built before wrapping
)
add_dependencies(${python_install_target} gtsam gtsam_header)
# Wrap gtsam_unstable
if(GTSAM_BUILD_UNSTABLE)
add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h")
wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header
"from gtsam.gtsam cimport *" # extra imports
"${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path
gtsam_unstable # library to link with
"gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping
)
add_dependencies(${python_install_target} gtsam_unstable gtsam_unstable_header)
endif()
# install scripts and tests
install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
endif ()

147
cython/README.md
View File

@ -1,147 +0,0 @@
# Python Wrapper
This is the Python wrapper around the GTSAM C++ library. We use Cython to generate the bindings to the underlying C++ code.
## Requirements
- If you want to build the GTSAM python library for a specific python version (eg 3.6),
use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used.
- If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment),
then the environment should be active while building GTSAM.
- This wrapper needs `Cython(>=0.25.2)`, `backports_abc(>=0.5)`, and `numpy(>=1.11.0)`. These can be installed as follows:
```bash
pip install -r <gtsam_folder>/cython/requirements.txt
```
- For compatibility with GTSAM's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
named `gtsam_eigency`, to interface between C++'s Eigen and Python's numpy.
## Install
- Run cmake with the `GTSAM_INSTALL_CYTHON_TOOLBOX` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory defined by `GTSAM_CYTHON_INSTALL_PATH`, which is by default `<PROJECT_BINARY_DIR>/cython` in Release mode and `<PROJECT_BINARY_DIR>/cython<CMAKE_BUILD_TYPE>` for other modes.
- Build GTSAM and the wrapper with `make`.
- To install, simply run `make python-install`.
- The same command can be used to install into a virtual environment if it is active.
- **NOTE**: if you don't want GTSAM to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install GTSAM to a subdirectory of the build directory.
- You can also directly run `make python-install` without running `make`, and it will compile all the dependencies accordingly.
## Unit Tests
The Cython toolbox also has a small set of unit tests located in the
test directory. To run them:
```bash
cd <GTSAM_CYTHON_INSTALL_PATH>
python -m unittest discover
```
## Utils
TODO
## Examples
TODO
## Writing Your Own Scripts
See the tests for examples.
### Some Important Notes:
- Vector/Matrix:
- GTSAM expects double-precision floating point vectors and matrices.
Hence, you should pass numpy matrices with `dtype=float`, or `float64`.
- Also, GTSAM expects _column-major_ matrices, unlike the default storage
scheme in numpy. Hence, you should pass column-major matrices to GTSAM using
the flag order='F'. And you always get column-major matrices back.
For more details, see [this link](https://github.com/wouterboomsma/eigency#storage-layout---why-arrays-are-sometimes-transposed).
- Passing row-major matrices of different dtype, e.g. `int`, will also work
as the wrapper converts them to column-major and dtype float for you,
using numpy.array.astype(float, order='F', copy=False).
However, this will result a copy if your matrix is not in the expected type
and storage order.
- Inner namespace: Classes in inner namespace will be prefixed by <innerNamespace>\_ in Python.
Examples: `noiseModel_Gaussian`, `noiseModel_mEstimator_Tukey`
- Casting from a base class to a derive class must be done explicitly.
Examples:
```python
noiseBase = factor.noiseModel()
noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
```
## Wrapping Custom GTSAM-based Project
Please refer to the template project and the corresponding tutorial available [here](https://github.com/borglab/GTSAM-project-python).
## KNOWN ISSUES
- Doesn't work with python3 installed from homebrew
- size-related issue: can only wrap up to a certain number of classes: up to mEstimator!
- Guess: 64 vs 32b? disutils Compiler flags?
- Bug with Cython 0.24: instantiated factor classes return FastVector<size_t> for keys(), which can't be casted to FastVector<Key>
- Upgrading to 0.25 solves the problem
- Need default constructor and default copy constructor for almost every classes... :(
- support these constructors by default and declare "delete" for special classes?
### TODO
- [ ] allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython.
- [ ] a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in GTSAM?)
- [ ] inner namespaces ==> inner packages?
- [ ] Wrap fixed-size Matrices/Vectors?
### Completed/Cancelled:
- [x] Fix Python tests: don't use " import <package> \* ": Bad style!!! (18-03-17 19:50)
- [x] Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files
- [x] Wrap unstable @done (18-03-17 15:30)
- [x] Unify cython/GTSAM.h and the original GTSAM.h @done (18-03-17 15:30)
- [x] 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem.
- [x] 06-03-17: manage to remove the requirements for default and copy constructors
- [ ] 25-11-16: Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector<Key>, to FastVector<size_t>.
- [ ] Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: `GTSAM::JointMarginal __pyx_t_1;` Users don't have to wrap this constructor, however.
- [x] Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00)
- [x] Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30)
- [x] CMake install script @done (25-11-16 02:30)
- [ ] [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy
- [ ] forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work?
- [x] wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00)
- [x] Global functions @done (22-11-16 21:00)
- [x] [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00)
- [x] Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00)
- [x] Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00)
- [x] Casting from parent and grandparents @done (16-11-16 17:00)
- [x] Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00)
- [x] Support "print obj" @done (16-11-16 17:00)
- [x] methods for FastVector: at, [], ... @done (16-11-16 17:00)
- [x] Cython: Key and size_t: traits<size_t> doesn't exist @done (16-09-12 18:34)
- [x] KeyVector, KeyList, KeySet... @done (16-09-13 17:19)
- [x] [Nice to have] parse typedef @done (16-09-13 17:19)
- [x] ctypedef at correct places @done (16-09-12 18:34)
- [x] expand template variable type in constructor/static methods? @done (16-09-12 18:34)
- [x] NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20)
- [x] Value: no default constructor @done (16-09-13 17:20)
- [x] ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25)
- [x] Delete duplicate methods in derived class @done (16-09-12 13:38)
- [x] Fix return properly @done (16-09-11 17:14)
- [x] handle pair @done (16-09-11 17:14)
- [x] Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59)
- [x] Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59)
- [x] Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22)
- [x] Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05)
- [ ] Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20)
- [ ] Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28)
- inference before symbolic/linear
- what's the purpose of "virtual" ??

26
cython/gtsam/__init__.py
View File

@ -1,26 +0,0 @@
from .gtsam import *
try:
import gtsam_unstable
def _deprecated_wrapper(item, name):
def wrapper(*args, **kwargs):
from warnings import warn
message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) +
'Please import it from gtsam_unstable.')
warn(message)
return item(*args, **kwargs)
return wrapper
for name in dir(gtsam_unstable):
if not name.startswith('__'):
item = getattr(gtsam_unstable, name)
if callable(item):
item = _deprecated_wrapper(item, name)
globals()[name] = item
except ImportError:
pass

153
cython/gtsam/examples/ImuFactorExample.py
View File

@ -1,153 +0,0 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
A script validating and demonstrating the ImuFactor inference.
Author: Frank Dellaert, Varun Agrawal
"""
from __future__ import print_function
import math
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand_B as B
from gtsam import symbol_shorthand_V as V
from gtsam import symbol_shorthand_X as X
from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D
from PreintegrationExample import POSES_FIG, PreintegrationExample
BIAS_KEY = B(0)
np.set_printoptions(precision=3, suppress=True)
class ImuFactorExample(PreintegrationExample):
def __init__(self):
self.velocity = np.array([2, 0, 0])
self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
# Choose one of these twists to change scenario:
zero_twist = (np.zeros(3), np.zeros(3))
forward_twist = (np.zeros(3), self.velocity)
loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity)
sick_twist = (
np.array([math.radians(30), -math.radians(30), 0]), self.velocity)
accBias = np.array([-0.3, 0.1, 0.2])
gyroBias = np.array([0.1, 0.3, -0.1])
bias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
dt = 1e-2
super(ImuFactorExample, self).__init__(sick_twist, bias, dt)
def addPrior(self, i, graph):
state = self.scenario.navState(i)
graph.push_back(gtsam.PriorFactorPose3(
X(i), state.pose(), self.priorNoise))
graph.push_back(gtsam.PriorFactorVector(
V(i), state.velocity(), self.velNoise))
def run(self):
graph = gtsam.NonlinearFactorGraph()
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
T = 12
num_poses = T + 1 # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses):
state_i = self.scenario.navState(float(i))
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
pose = state_i.pose().compose(poseNoise)
velocity = state_i.velocity() + np.random.randn(3)*0.1
initial.insert(X(i), pose)
initial.insert(V(i), velocity)
# simulate the loop
i = 0 # state index
actual_state_i = self.scenario.navState(0)
for k, t in enumerate(np.arange(0, T, self.dt)):
# get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
actual_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3)*0.1)
# Plot IMU many times
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
# Plot every second
if k % int(1 / self.dt) == 0:
self.plotGroundTruthPose(t)
# create IMU factor every second
if (k + 1) % int(1 / self.dt) == 0:
factor = gtsam.ImuFactor(X(i), V(i), X(
i + 1), V(i + 1), BIAS_KEY, pim)
graph.push_back(factor)
if True:
print(factor)
print(pim.predict(actual_state_i, self.actualBias))
pim.resetIntegration()
actual_state_i = self.scenario.navState(t + self.dt)
i += 1
# add priors on beginning and end
self.addPrior(0, graph)
self.addPrior(num_poses - 1, graph)
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
# Calculate and print marginal covariances
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
# Plot resulting poses
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1)
i += 1
gtsam.utils.plot.set_axes_equal(POSES_FIG)
print(result.atimuBias_ConstantBias(BIAS_KEY))
plt.ioff()
plt.show()
if __name__ == '__main__':
ImuFactorExample().run()

42
cython/gtsam_eigency/CMakeLists.txt
View File

@ -1,42 +0,0 @@
include(GtsamCythonWrap)
# Copy eigency's sources to the build folder
# so that the cython-generated header "conversions_api.h" can be found when cythonizing eigency's core
# and eigency's cython pxd headers can be found when cythonizing gtsam
file(COPY "." DESTINATION ".")
set(OUTPUT_DIR "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency")
set(EIGENCY_INCLUDE_DIR ${OUTPUT_DIR})
# This is to make the build/cython/gtsam_eigency folder a python package
configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py)
# include eigency headers
include_directories(${EIGENCY_INCLUDE_DIR})
# Cythonize and build eigency
message(STATUS "Cythonize and build eigency")
# Important trick: use "../gtsam_eigency/conversions.pyx" to let cython know that the conversions module is
# a part of the gtsam_eigency package and generate the function call import_gtsam_eigency__conversions()
# in conversions_api.h correctly!
cythonize(cythonize_eigency_conversions "../gtsam_eigency/conversions.pyx" "conversions"
"${OUTPUT_DIR}" "${EIGENCY_INCLUDE_DIR}" "" "" "")
cythonize(cythonize_eigency_core "../gtsam_eigency/core.pyx" "core"
${OUTPUT_DIR} "${EIGENCY_INCLUDE_DIR}" "" "" "")
# Include Eigen headers:
target_include_directories(cythonize_eigency_conversions PUBLIC
$<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
$<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
)
target_include_directories(cythonize_eigency_core PUBLIC
$<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
$<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
)
add_dependencies(cythonize_eigency_core cythonize_eigency_conversions)
add_custom_target(cythonize_eigency)
add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core)
if(TARGET ${python_install_target})
add_dependencies(${python_install_target} cythonize_eigency)
endif()

20
cython/gtsam_eigency/LICENSE.txt
View File

@ -1,20 +0,0 @@
Copyright (c) 2016 Wouter Boomsma
Permission is hereby granted, free of charge, to any person obtaining
a copy of this software and associated documentation files (the
"Software"), to deal in the Software without restriction, including
without limitation the rights to use, copy, modify, merge, publish,
distribute, sublicense, and/or sell copies of the Software, and to
permit persons to whom the Software is furnished to do so, subject to
the following conditions:
The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

13
cython/gtsam_eigency/__init__.py.in
View File

@ -1,13 +0,0 @@
import os
import numpy as np
__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}"
def get_includes(include_eigen=True):
root = os.path.dirname(__file__)
parent = os.path.join(root, "..")
path = [root, parent, np.get_include()]
if include_eigen:
path.append(os.path.join(root, __eigen_dir__))
return path

62
cython/gtsam_eigency/conversions.pxd
View File

@ -1,62 +0,0 @@
cimport numpy as np
cdef api np.ndarray[double, ndim=2] ndarray_double_C(double *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[double, ndim=2] ndarray_double_F(double *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[double, ndim=2] ndarray_copy_double_C(const double *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[double, ndim=2] ndarray_copy_double_F(const double *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[float, ndim=2] ndarray_float_C(float *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[float, ndim=2] ndarray_float_F(float *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[float, ndim=2] ndarray_copy_float_C(const float *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[float, ndim=2] ndarray_copy_float_F(const float *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[long, ndim=2] ndarray_long_C(long *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[long, ndim=2] ndarray_long_F(long *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[long, ndim=2] ndarray_copy_long_C(const long *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[long, ndim=2] ndarray_copy_long_F(const long *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned long, ndim=2] ndarray_ulong_C(unsigned long *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned long, ndim=2] ndarray_ulong_F(unsigned long *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_C(const unsigned long *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_F(const unsigned long *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[int, ndim=2] ndarray_int_C(int *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[int, ndim=2] ndarray_int_F(int *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[int, ndim=2] ndarray_copy_int_C(const int *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[int, ndim=2] ndarray_copy_int_F(const int *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned int, ndim=2] ndarray_uint_C(unsigned int *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned int, ndim=2] ndarray_uint_F(unsigned int *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_C(const unsigned int *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_F(const unsigned int *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[short, ndim=2] ndarray_short_C(short *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[short, ndim=2] ndarray_short_F(short *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[short, ndim=2] ndarray_copy_short_C(const short *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[short, ndim=2] ndarray_copy_short_F(const short *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned short, ndim=2] ndarray_ushort_C(unsigned short *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned short, ndim=2] ndarray_ushort_F(unsigned short *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_C(const unsigned short *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_F(const unsigned short *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[signed char, ndim=2] ndarray_schar_C(signed char *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[signed char, ndim=2] ndarray_schar_F(signed char *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[signed char, ndim=2] ndarray_copy_schar_C(const signed char *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[signed char, ndim=2] ndarray_copy_schar_F(const signed char *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned char, ndim=2] ndarray_uchar_C(unsigned char *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned char, ndim=2] ndarray_uchar_F(unsigned char *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_C(const unsigned char *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_F(const unsigned char *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_C(np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_F(np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_C(const np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_F(const np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_C(np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_F(np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_C(const np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride)
cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_F(const np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride)

327
cython/gtsam_eigency/conversions.pyx
View File

@ -1,327 +0,0 @@
cimport cython
import numpy as np
from numpy.lib.stride_tricks import as_strided
@cython.boundscheck(False)
cdef np.ndarray[double, ndim=2] ndarray_double_C(double *data, long rows, long cols, long row_stride, long col_stride):
cdef double[:,:] mem_view = <double[:rows,:cols]>data
dtype = 'double'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[double, ndim=2] ndarray_double_F(double *data, long rows, long cols, long row_stride, long col_stride):
cdef double[::1,:] mem_view = <double[:rows:1,:cols]>data
dtype = 'double'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[double, ndim=2] ndarray_copy_double_C(const double *data, long rows, long cols, long row_stride, long col_stride):
cdef double[:,:] mem_view = <double[:rows,:cols]>data
dtype = 'double'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[double, ndim=2] ndarray_copy_double_F(const double *data, long rows, long cols, long row_stride, long col_stride):
cdef double[::1,:] mem_view = <double[:rows:1,:cols]>data
dtype = 'double'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[float, ndim=2] ndarray_float_C(float *data, long rows, long cols, long row_stride, long col_stride):
cdef float[:,:] mem_view = <float[:rows,:cols]>data
dtype = 'float'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[float, ndim=2] ndarray_float_F(float *data, long rows, long cols, long row_stride, long col_stride):
cdef float[::1,:] mem_view = <float[:rows:1,:cols]>data
dtype = 'float'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[float, ndim=2] ndarray_copy_float_C(const float *data, long rows, long cols, long row_stride, long col_stride):
cdef float[:,:] mem_view = <float[:rows,:cols]>data
dtype = 'float'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[float, ndim=2] ndarray_copy_float_F(const float *data, long rows, long cols, long row_stride, long col_stride):
cdef float[::1,:] mem_view = <float[:rows:1,:cols]>data
dtype = 'float'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[long, ndim=2] ndarray_long_C(long *data, long rows, long cols, long row_stride, long col_stride):
cdef long[:,:] mem_view = <long[:rows,:cols]>data
dtype = 'int_'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[long, ndim=2] ndarray_long_F(long *data, long rows, long cols, long row_stride, long col_stride):
cdef long[::1,:] mem_view = <long[:rows:1,:cols]>data
dtype = 'int_'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[long, ndim=2] ndarray_copy_long_C(const long *data, long rows, long cols, long row_stride, long col_stride):
cdef long[:,:] mem_view = <long[:rows,:cols]>data
dtype = 'int_'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[long, ndim=2] ndarray_copy_long_F(const long *data, long rows, long cols, long row_stride, long col_stride):
cdef long[::1,:] mem_view = <long[:rows:1,:cols]>data
dtype = 'int_'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[unsigned long, ndim=2] ndarray_ulong_C(unsigned long *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned long[:,:] mem_view = <unsigned long[:rows,:cols]>data
dtype = 'uint'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[unsigned long, ndim=2] ndarray_ulong_F(unsigned long *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned long[::1,:] mem_view = <unsigned long[:rows:1,:cols]>data
dtype = 'uint'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_C(const unsigned long *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned long[:,:] mem_view = <unsigned long[:rows,:cols]>data
dtype = 'uint'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_F(const unsigned long *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned long[::1,:] mem_view = <unsigned long[:rows:1,:cols]>data
dtype = 'uint'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[int, ndim=2] ndarray_int_C(int *data, long rows, long cols, long row_stride, long col_stride):
cdef int[:,:] mem_view = <int[:rows,:cols]>data
dtype = 'int'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[int, ndim=2] ndarray_int_F(int *data, long rows, long cols, long row_stride, long col_stride):
cdef int[::1,:] mem_view = <int[:rows:1,:cols]>data
dtype = 'int'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[int, ndim=2] ndarray_copy_int_C(const int *data, long rows, long cols, long row_stride, long col_stride):
cdef int[:,:] mem_view = <int[:rows,:cols]>data
dtype = 'int'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[int, ndim=2] ndarray_copy_int_F(const int *data, long rows, long cols, long row_stride, long col_stride):
cdef int[::1,:] mem_view = <int[:rows:1,:cols]>data
dtype = 'int'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[unsigned int, ndim=2] ndarray_uint_C(unsigned int *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned int[:,:] mem_view = <unsigned int[:rows,:cols]>data
dtype = 'uint'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[unsigned int, ndim=2] ndarray_uint_F(unsigned int *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned int[::1,:] mem_view = <unsigned int[:rows:1,:cols]>data
dtype = 'uint'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_C(const unsigned int *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned int[:,:] mem_view = <unsigned int[:rows,:cols]>data
dtype = 'uint'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_F(const unsigned int *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned int[::1,:] mem_view = <unsigned int[:rows:1,:cols]>data
dtype = 'uint'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[short, ndim=2] ndarray_short_C(short *data, long rows, long cols, long row_stride, long col_stride):
cdef short[:,:] mem_view = <short[:rows,:cols]>data
dtype = 'short'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[short, ndim=2] ndarray_short_F(short *data, long rows, long cols, long row_stride, long col_stride):
cdef short[::1,:] mem_view = <short[:rows:1,:cols]>data
dtype = 'short'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[short, ndim=2] ndarray_copy_short_C(const short *data, long rows, long cols, long row_stride, long col_stride):
cdef short[:,:] mem_view = <short[:rows,:cols]>data
dtype = 'short'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[short, ndim=2] ndarray_copy_short_F(const short *data, long rows, long cols, long row_stride, long col_stride):
cdef short[::1,:] mem_view = <short[:rows:1,:cols]>data
dtype = 'short'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[unsigned short, ndim=2] ndarray_ushort_C(unsigned short *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned short[:,:] mem_view = <unsigned short[:rows,:cols]>data
dtype = 'ushort'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[unsigned short, ndim=2] ndarray_ushort_F(unsigned short *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned short[::1,:] mem_view = <unsigned short[:rows:1,:cols]>data
dtype = 'ushort'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_C(const unsigned short *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned short[:,:] mem_view = <unsigned short[:rows,:cols]>data
dtype = 'ushort'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_F(const unsigned short *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned short[::1,:] mem_view = <unsigned short[:rows:1,:cols]>data
dtype = 'ushort'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[signed char, ndim=2] ndarray_schar_C(signed char *data, long rows, long cols, long row_stride, long col_stride):
cdef signed char[:,:] mem_view = <signed char[:rows,:cols]>data
dtype = 'int8'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[signed char, ndim=2] ndarray_schar_F(signed char *data, long rows, long cols, long row_stride, long col_stride):
cdef signed char[::1,:] mem_view = <signed char[:rows:1,:cols]>data
dtype = 'int8'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[signed char, ndim=2] ndarray_copy_schar_C(const signed char *data, long rows, long cols, long row_stride, long col_stride):
cdef signed char[:,:] mem_view = <signed char[:rows,:cols]>data
dtype = 'int8'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[signed char, ndim=2] ndarray_copy_schar_F(const signed char *data, long rows, long cols, long row_stride, long col_stride):
cdef signed char[::1,:] mem_view = <signed char[:rows:1,:cols]>data
dtype = 'int8'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[unsigned char, ndim=2] ndarray_uchar_C(unsigned char *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned char[:,:] mem_view = <unsigned char[:rows,:cols]>data
dtype = 'uint8'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[unsigned char, ndim=2] ndarray_uchar_F(unsigned char *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned char[::1,:] mem_view = <unsigned char[:rows:1,:cols]>data
dtype = 'uint8'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_C(const unsigned char *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned char[:,:] mem_view = <unsigned char[:rows,:cols]>data
dtype = 'uint8'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_F(const unsigned char *data, long rows, long cols, long row_stride, long col_stride):
cdef unsigned char[::1,:] mem_view = <unsigned char[:rows:1,:cols]>data
dtype = 'uint8'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_C(np.complex128_t *data, long rows, long cols, long row_stride, long col_stride):
cdef np.complex128_t[:,:] mem_view = <np.complex128_t[:rows,:cols]>data
dtype = 'complex128'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_F(np.complex128_t *data, long rows, long cols, long row_stride, long col_stride):
cdef np.complex128_t[::1,:] mem_view = <np.complex128_t[:rows:1,:cols]>data
dtype = 'complex128'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_C(const np.complex128_t *data, long rows, long cols, long row_stride, long col_stride):
cdef np.complex128_t[:,:] mem_view = <np.complex128_t[:rows,:cols]>data
dtype = 'complex128'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_F(const np.complex128_t *data, long rows, long cols, long row_stride, long col_stride):
cdef np.complex128_t[::1,:] mem_view = <np.complex128_t[:rows:1,:cols]>data
dtype = 'complex128'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_C(np.complex64_t *data, long rows, long cols, long row_stride, long col_stride):
cdef np.complex64_t[:,:] mem_view = <np.complex64_t[:rows,:cols]>data
dtype = 'complex64'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_F(np.complex64_t *data, long rows, long cols, long row_stride, long col_stride):
cdef np.complex64_t[::1,:] mem_view = <np.complex64_t[:rows:1,:cols]>data
dtype = 'complex64'
cdef int itemsize = np.dtype(dtype).itemsize
return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
@cython.boundscheck(False)
cdef np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_C(const np.complex64_t *data, long rows, long cols, long row_stride, long col_stride):
cdef np.complex64_t[:,:] mem_view = <np.complex64_t[:rows,:cols]>data
dtype = 'complex64'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
@cython.boundscheck(False)
cdef np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_F(const np.complex64_t *data, long rows, long cols, long row_stride, long col_stride):
cdef np.complex64_t[::1,:] mem_view = <np.complex64_t[:rows:1,:cols]>data
dtype = 'complex64'
cdef int itemsize = np.dtype(dtype).itemsize
return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))

917
cython/gtsam_eigency/core.pxd
View File

@ -1,917 +0,0 @@
cimport cython
cimport numpy as np
ctypedef signed char schar;
ctypedef unsigned char uchar;
ctypedef fused dtype:
uchar
schar
short
int
long
float
double
ctypedef fused DenseType:
Matrix
Array
ctypedef fused Rows:
_1
_2
_3
_4
_5
_6
_7
_8
_9
_10
_11
_12
_13
_14
_15
_16
_17
_18
_19
_20
_21
_22
_23
_24
_25
_26
_27
_28
_29
_30
_31
_32
Dynamic
ctypedef Rows Cols
ctypedef Rows StrideOuter
ctypedef Rows StrideInner
ctypedef fused DenseTypeShort:
Vector1i
Vector2i
Vector3i
Vector4i
VectorXi
RowVector1i
RowVector2i
RowVector3i
RowVector4i
RowVectorXi
Matrix1i
Matrix2i
Matrix3i
Matrix4i
MatrixXi
Vector1f
Vector2f
Vector3f
Vector4f
VectorXf
RowVector1f
RowVector2f
RowVector3f
RowVector4f
RowVectorXf
Matrix1f
Matrix2f
Matrix3f
Matrix4f
MatrixXf
Vector1d
Vector2d
Vector3d
Vector4d
VectorXd
RowVector1d
RowVector2d
RowVector3d
RowVector4d
RowVectorXd
Matrix1d
Matrix2d
Matrix3d
Matrix4d
MatrixXd
Vector1cf
Vector2cf
Vector3cf
Vector4cf
VectorXcf
RowVector1cf
RowVector2cf
RowVector3cf
RowVector4cf
RowVectorXcf
Matrix1cf
Matrix2cf
Matrix3cf
Matrix4cf
MatrixXcf
Vector1cd
Vector2cd
Vector3cd
Vector4cd
VectorXcd
RowVector1cd
RowVector2cd
RowVector3cd
RowVector4cd
RowVectorXcd
Matrix1cd
Matrix2cd
Matrix3cd
Matrix4cd
MatrixXcd
Array22i
Array23i
Array24i
Array2Xi
Array32i
Array33i
Array34i
Array3Xi
Array42i
Array43i
Array44i
Array4Xi
ArrayX2i
ArrayX3i
ArrayX4i
ArrayXXi
Array2i
Array3i
Array4i
ArrayXi
Array22f
Array23f
Array24f
Array2Xf
Array32f
Array33f
Array34f
Array3Xf
Array42f
Array43f
Array44f
Array4Xf
ArrayX2f
ArrayX3f
ArrayX4f
ArrayXXf
Array2f
Array3f
Array4f
ArrayXf
Array22d
Array23d
Array24d
Array2Xd
Array32d
Array33d
Array34d
Array3Xd
Array42d
Array43d
Array44d
Array4Xd
ArrayX2d
ArrayX3d
ArrayX4d
ArrayXXd
Array2d
Array3d
Array4d
ArrayXd
Array22cf
Array23cf
Array24cf
Array2Xcf
Array32cf
Array33cf
Array34cf
Array3Xcf
Array42cf
Array43cf
Array44cf
Array4Xcf
ArrayX2cf
ArrayX3cf
ArrayX4cf
ArrayXXcf
Array2cf
Array3cf
Array4cf
ArrayXcf
Array22cd
Array23cd
Array24cd
Array2Xcd
Array32cd
Array33cd
Array34cd
Array3Xcd
Array42cd
Array43cd
Array44cd
Array4Xcd
ArrayX2cd
ArrayX3cd
ArrayX4cd
ArrayXXcd
Array2cd
Array3cd
Array4cd
ArrayXcd
ctypedef fused StorageOrder:
RowMajor
ColMajor
ctypedef fused MapOptions:
Aligned
Unaligned
cdef extern from "eigency_cpp.h" namespace "eigency":
cdef cppclass _1 "1":
pass
cdef cppclass _2 "2":
pass
cdef cppclass _3 "3":
pass
cdef cppclass _4 "4":
pass
cdef cppclass _5 "5":
pass
cdef cppclass _6 "6":
pass
cdef cppclass _7 "7":
pass
cdef cppclass _8 "8":
pass
cdef cppclass _9 "9":
pass
cdef cppclass _10 "10":
pass
cdef cppclass _11 "11":
pass
cdef cppclass _12 "12":
pass
cdef cppclass _13 "13":
pass
cdef cppclass _14 "14":
pass
cdef cppclass _15 "15":
pass
cdef cppclass _16 "16":
pass
cdef cppclass _17 "17":
pass
cdef cppclass _18 "18":
pass
cdef cppclass _19 "19":
pass
cdef cppclass _20 "20":
pass
cdef cppclass _21 "21":
pass
cdef cppclass _22 "22":
pass
cdef cppclass _23 "23":
pass
cdef cppclass _24 "24":
pass
cdef cppclass _25 "25":
pass
cdef cppclass _26 "26":
pass
cdef cppclass _27 "27":
pass
cdef cppclass _28 "28":
pass
cdef cppclass _29 "29":
pass
cdef cppclass _30 "30":
pass
cdef cppclass _31 "31":
pass
cdef cppclass _32 "32":
pass
cdef cppclass PlainObjectBase:
pass
cdef cppclass Map[DenseTypeShort](PlainObjectBase):
Map() except +
Map(np.ndarray array) except +
cdef cppclass FlattenedMap[DenseType, dtype, Rows, Cols]:
FlattenedMap() except +
FlattenedMap(np.ndarray array) except +
cdef cppclass FlattenedMapWithOrder "eigency::FlattenedMap" [DenseType, dtype, Rows, Cols, StorageOrder]:
FlattenedMapWithOrder() except +
FlattenedMapWithOrder(np.ndarray array) except +
cdef cppclass FlattenedMapWithStride "eigency::FlattenedMap" [DenseType, dtype, Rows, Cols, StorageOrder, MapOptions, StrideOuter, StrideInner]:
FlattenedMapWithStride() except +
FlattenedMapWithStride(np.ndarray array) except +
cdef np.ndarray ndarray_view(PlainObjectBase &)
cdef np.ndarray ndarray_copy(PlainObjectBase &)
cdef np.ndarray ndarray(PlainObjectBase &)
cdef extern from "eigency_cpp.h" namespace "Eigen":
cdef cppclass Dynamic:
pass
cdef cppclass RowMajor:
pass
cdef cppclass ColMajor:
pass
cdef cppclass Aligned:
pass
cdef cppclass Unaligned:
pass
cdef cppclass Matrix(PlainObjectBase):
pass
cdef cppclass Array(PlainObjectBase):
pass
cdef cppclass VectorXd(PlainObjectBase):
pass
cdef cppclass Vector1i(PlainObjectBase):
pass
cdef cppclass Vector2i(PlainObjectBase):
pass
cdef cppclass Vector3i(PlainObjectBase):
pass
cdef cppclass Vector4i(PlainObjectBase):
pass
cdef cppclass VectorXi(PlainObjectBase):
pass
cdef cppclass RowVector1i(PlainObjectBase):
pass
cdef cppclass RowVector2i(PlainObjectBase):
pass
cdef cppclass RowVector3i(PlainObjectBase):
pass
cdef cppclass RowVector4i(PlainObjectBase):
pass
cdef cppclass RowVectorXi(PlainObjectBase):
pass
cdef cppclass Matrix1i(PlainObjectBase):
pass
cdef cppclass Matrix2i(PlainObjectBase):
pass
cdef cppclass Matrix3i(PlainObjectBase):
pass
cdef cppclass Matrix4i(PlainObjectBase):
pass
cdef cppclass MatrixXi(PlainObjectBase):
pass
cdef cppclass Vector1f(PlainObjectBase):
pass
cdef cppclass Vector2f(PlainObjectBase):
pass
cdef cppclass Vector3f(PlainObjectBase):
pass
cdef cppclass Vector4f(PlainObjectBase):
pass
cdef cppclass VectorXf(PlainObjectBase):
pass
cdef cppclass RowVector1f(PlainObjectBase):
pass
cdef cppclass RowVector2f(PlainObjectBase):
pass
cdef cppclass RowVector3f(PlainObjectBase):
pass
cdef cppclass RowVector4f(PlainObjectBase):
pass
cdef cppclass RowVectorXf(PlainObjectBase):
pass
cdef cppclass Matrix1f(PlainObjectBase):
pass
cdef cppclass Matrix2f(PlainObjectBase):
pass
cdef cppclass Matrix3f(PlainObjectBase):
pass
cdef cppclass Matrix4f(PlainObjectBase):
pass
cdef cppclass MatrixXf(PlainObjectBase):
pass
cdef cppclass Vector1d(PlainObjectBase):
pass
cdef cppclass Vector2d(PlainObjectBase):
pass
cdef cppclass Vector3d(PlainObjectBase):
pass
cdef cppclass Vector4d(PlainObjectBase):
pass
cdef cppclass VectorXd(PlainObjectBase):
pass
cdef cppclass RowVector1d(PlainObjectBase):
pass
cdef cppclass RowVector2d(PlainObjectBase):
pass
cdef cppclass RowVector3d(PlainObjectBase):
pass
cdef cppclass RowVector4d(PlainObjectBase):
pass
cdef cppclass RowVectorXd(PlainObjectBase):
pass
cdef cppclass Matrix1d(PlainObjectBase):
pass
cdef cppclass Matrix2d(PlainObjectBase):
pass
cdef cppclass Matrix3d(PlainObjectBase):
pass
cdef cppclass Matrix4d(PlainObjectBase):
pass
cdef cppclass MatrixXd(PlainObjectBase):
pass
cdef cppclass Vector1cf(PlainObjectBase):
pass
cdef cppclass Vector2cf(PlainObjectBase):
pass
cdef cppclass Vector3cf(PlainObjectBase):
pass
cdef cppclass Vector4cf(PlainObjectBase):
pass
cdef cppclass VectorXcf(PlainObjectBase):
pass
cdef cppclass RowVector1cf(PlainObjectBase):
pass
cdef cppclass RowVector2cf(PlainObjectBase):
pass
cdef cppclass RowVector3cf(PlainObjectBase):
pass
cdef cppclass RowVector4cf(PlainObjectBase):
pass
cdef cppclass RowVectorXcf(PlainObjectBase):
pass
cdef cppclass Matrix1cf(PlainObjectBase):
pass
cdef cppclass Matrix2cf(PlainObjectBase):
pass
cdef cppclass Matrix3cf(PlainObjectBase):
pass
cdef cppclass Matrix4cf(PlainObjectBase):
pass
cdef cppclass MatrixXcf(PlainObjectBase):
pass
cdef cppclass Vector1cd(PlainObjectBase):
pass
cdef cppclass Vector2cd(PlainObjectBase):
pass
cdef cppclass Vector3cd(PlainObjectBase):
pass
cdef cppclass Vector4cd(PlainObjectBase):
pass
cdef cppclass VectorXcd(PlainObjectBase):
pass
cdef cppclass RowVector1cd(PlainObjectBase):
pass
cdef cppclass RowVector2cd(PlainObjectBase):
pass
cdef cppclass RowVector3cd(PlainObjectBase):
pass
cdef cppclass RowVector4cd(PlainObjectBase):
pass
cdef cppclass RowVectorXcd(PlainObjectBase):
pass
cdef cppclass Matrix1cd(PlainObjectBase):
pass
cdef cppclass Matrix2cd(PlainObjectBase):
pass
cdef cppclass Matrix3cd(PlainObjectBase):
pass
cdef cppclass Matrix4cd(PlainObjectBase):
pass
cdef cppclass MatrixXcd(PlainObjectBase):
pass
cdef cppclass Array22i(PlainObjectBase):
pass
cdef cppclass Array23i(PlainObjectBase):
pass
cdef cppclass Array24i(PlainObjectBase):
pass
cdef cppclass Array2Xi(PlainObjectBase):
pass
cdef cppclass Array32i(PlainObjectBase):
pass
cdef cppclass Array33i(PlainObjectBase):
pass
cdef cppclass Array34i(PlainObjectBase):
pass
cdef cppclass Array3Xi(PlainObjectBase):
pass
cdef cppclass Array42i(PlainObjectBase):
pass
cdef cppclass Array43i(PlainObjectBase):
pass
cdef cppclass Array44i(PlainObjectBase):
pass
cdef cppclass Array4Xi(PlainObjectBase):
pass
cdef cppclass ArrayX2i(PlainObjectBase):
pass
cdef cppclass ArrayX3i(PlainObjectBase):
pass
cdef cppclass ArrayX4i(PlainObjectBase):
pass
cdef cppclass ArrayXXi(PlainObjectBase):
pass
cdef cppclass Array2i(PlainObjectBase):
pass
cdef cppclass Array3i(PlainObjectBase):
pass
cdef cppclass Array4i(PlainObjectBase):
pass
cdef cppclass ArrayXi(PlainObjectBase):
pass
cdef cppclass Array22f(PlainObjectBase):
pass
cdef cppclass Array23f(PlainObjectBase):
pass
cdef cppclass Array24f(PlainObjectBase):
pass
cdef cppclass Array2Xf(PlainObjectBase):
pass
cdef cppclass Array32f(PlainObjectBase):
pass
cdef cppclass Array33f(PlainObjectBase):
pass
cdef cppclass Array34f(PlainObjectBase):
pass
cdef cppclass Array3Xf(PlainObjectBase):
pass
cdef cppclass Array42f(PlainObjectBase):
pass
cdef cppclass Array43f(PlainObjectBase):
pass
cdef cppclass Array44f(PlainObjectBase):
pass
cdef cppclass Array4Xf(PlainObjectBase):
pass
cdef cppclass ArrayX2f(PlainObjectBase):
pass
cdef cppclass ArrayX3f(PlainObjectBase):
pass
cdef cppclass ArrayX4f(PlainObjectBase):
pass
cdef cppclass ArrayXXf(PlainObjectBase):
pass
cdef cppclass Array2f(PlainObjectBase):
pass
cdef cppclass Array3f(PlainObjectBase):
pass
cdef cppclass Array4f(PlainObjectBase):
pass
cdef cppclass ArrayXf(PlainObjectBase):
pass
cdef cppclass Array22d(PlainObjectBase):
pass
cdef cppclass Array23d(PlainObjectBase):
pass
cdef cppclass Array24d(PlainObjectBase):
pass
cdef cppclass Array2Xd(PlainObjectBase):
pass
cdef cppclass Array32d(PlainObjectBase):
pass
cdef cppclass Array33d(PlainObjectBase):
pass
cdef cppclass Array34d(PlainObjectBase):
pass
cdef cppclass Array3Xd(PlainObjectBase):
pass
cdef cppclass Array42d(PlainObjectBase):
pass
cdef cppclass Array43d(PlainObjectBase):
pass
cdef cppclass Array44d(PlainObjectBase):
pass
cdef cppclass Array4Xd(PlainObjectBase):
pass
cdef cppclass ArrayX2d(PlainObjectBase):
pass
cdef cppclass ArrayX3d(PlainObjectBase):
pass
cdef cppclass ArrayX4d(PlainObjectBase):
pass
cdef cppclass ArrayXXd(PlainObjectBase):
pass
cdef cppclass Array2d(PlainObjectBase):
pass
cdef cppclass Array3d(PlainObjectBase):
pass
cdef cppclass Array4d(PlainObjectBase):
pass
cdef cppclass ArrayXd(PlainObjectBase):
pass
cdef cppclass Array22cf(PlainObjectBase):
pass
cdef cppclass Array23cf(PlainObjectBase):
pass
cdef cppclass Array24cf(PlainObjectBase):
pass
cdef cppclass Array2Xcf(PlainObjectBase):
pass
cdef cppclass Array32cf(PlainObjectBase):
pass
cdef cppclass Array33cf(PlainObjectBase):
pass
cdef cppclass Array34cf(PlainObjectBase):
pass
cdef cppclass Array3Xcf(PlainObjectBase):
pass
cdef cppclass Array42cf(PlainObjectBase):
pass
cdef cppclass Array43cf(PlainObjectBase):
pass
cdef cppclass Array44cf(PlainObjectBase):
pass
cdef cppclass Array4Xcf(PlainObjectBase):
pass
cdef cppclass ArrayX2cf(PlainObjectBase):
pass
cdef cppclass ArrayX3cf(PlainObjectBase):
pass
cdef cppclass ArrayX4cf(PlainObjectBase):
pass
cdef cppclass ArrayXXcf(PlainObjectBase):
pass
cdef cppclass Array2cf(PlainObjectBase):
pass
cdef cppclass Array3cf(PlainObjectBase):
pass
cdef cppclass Array4cf(PlainObjectBase):
pass
cdef cppclass ArrayXcf(PlainObjectBase):
pass
cdef cppclass Array22cd(PlainObjectBase):
pass
cdef cppclass Array23cd(PlainObjectBase):
pass
cdef cppclass Array24cd(PlainObjectBase):
pass
cdef cppclass Array2Xcd(PlainObjectBase):
pass
cdef cppclass Array32cd(PlainObjectBase):
pass
cdef cppclass Array33cd(PlainObjectBase):
pass
cdef cppclass Array34cd(PlainObjectBase):
pass
cdef cppclass Array3Xcd(PlainObjectBase):
pass
cdef cppclass Array42cd(PlainObjectBase):
pass
cdef cppclass Array43cd(PlainObjectBase):
pass
cdef cppclass Array44cd(PlainObjectBase):
pass
cdef cppclass Array4Xcd(PlainObjectBase):
pass
cdef cppclass ArrayX2cd(PlainObjectBase):
pass
cdef cppclass ArrayX3cd(PlainObjectBase):
pass
cdef cppclass ArrayX4cd(PlainObjectBase):
pass
cdef cppclass ArrayXXcd(PlainObjectBase):
pass
cdef cppclass Array2cd(PlainObjectBase):
pass
cdef cppclass Array3cd(PlainObjectBase):
pass
cdef cppclass Array4cd(PlainObjectBase):
pass
cdef cppclass ArrayXcd(PlainObjectBase):
pass

1
cython/gtsam_eigency/core.pyx
View File

@ -1 +0,0 @@

504
cython/gtsam_eigency/eigency_cpp.h
View File

@ -1,504 +0,0 @@
#include <Eigen/Dense>
#include <iostream>
#include <stdexcept>
#include <complex>
typedef ::std::complex< double > __pyx_t_double_complex;
typedef ::std::complex< float > __pyx_t_float_complex;
#include "conversions_api.h"
#ifndef EIGENCY_CPP
#define EIGENCY_CPP
namespace eigency {
template<typename Scalar>
inline PyArrayObject *_ndarray_view(Scalar *, long rows, long cols, bool is_row_major, long outer_stride=0, long inner_stride=0);
template<typename Scalar>
inline PyArrayObject *_ndarray_copy(const Scalar *, long rows, long cols, bool is_row_major, long outer_stride=0, long inner_stride=0);
// Strides:
// Eigen and numpy differ in their way of dealing with strides. Eigen has the concept of outer and
// inner strides, which are dependent on whether the array/matrix is row-major of column-major:
// Inner stride: denotes the offset between succeeding elements in each row (row-major) or column (column-major).
// Outer stride: denotes the offset between succeeding rows (row-major) or succeeding columns (column-major).
// In contrast, numpy's stride is simply a measure of how fast each dimension should be incremented.
// Consequently, a switch in numpy storage order from row-major to column-major involves a switch
// in strides, while it does not affect the stride in Eigen.
template<>
inline PyArrayObject *_ndarray_view<double>(double *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major) {
// Eigen row-major mode: row_stride=outer_stride, and col_stride=inner_stride
// If no stride is given, the row_stride is set to the number of columns.
return ndarray_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
} else {
// Eigen column-major mode: row_stride=outer_stride, and col_stride=inner_stride
// If no stride is given, the cow_stride is set to the number of rows.
return ndarray_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
}
template<>
inline PyArrayObject *_ndarray_copy<double>(const double *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<float>(float *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<float>(const float *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<long>(long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_long_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_long_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<long>(const long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_long_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_long_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<unsigned long>(unsigned long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_ulong_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_ulong_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<unsigned long>(const unsigned long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_ulong_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_ulong_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<int>(int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_int_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_int_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<int>(const int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_int_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_int_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<unsigned int>(unsigned int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_uint_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_uint_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<unsigned int>(const unsigned int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_uint_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_uint_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<short>(short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_short_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_short_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<short>(const short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_short_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_short_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<unsigned short>(unsigned short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_ushort_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_ushort_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<unsigned short>(const unsigned short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_ushort_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_ushort_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<signed char>(signed char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_schar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_schar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<signed char>(const signed char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_schar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_schar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<unsigned char>(unsigned char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_uchar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_uchar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<unsigned char>(const unsigned char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_uchar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_uchar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<std::complex<double> >(std::complex<double> *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_complex_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_complex_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<std::complex<double> >(const std::complex<double> *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_complex_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_complex_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_view<std::complex<float> >(std::complex<float> *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_complex_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_complex_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template<>
inline PyArrayObject *_ndarray_copy<std::complex<float> >(const std::complex<float> *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
if (is_row_major)
return ndarray_copy_complex_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
else
return ndarray_copy_complex_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
}
template <typename Derived>
inline PyArrayObject *ndarray(Eigen::PlainObjectBase<Derived> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor);
}
// If C++11 is available, check if m is an r-value reference, in
// which case a copy should always be made
#if __cplusplus >= 201103L
template <typename Derived>
inline PyArrayObject *ndarray(Eigen::PlainObjectBase<Derived> &&m) {
import_gtsam_eigency__conversions();
return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor);
}
#endif
template <typename Derived>
inline PyArrayObject *ndarray(const Eigen::PlainObjectBase<Derived> &m) {
import_gtsam_eigency__conversions();
return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor);
}
template <typename Derived>
inline PyArrayObject *ndarray_view(Eigen::PlainObjectBase<Derived> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor);
}
template <typename Derived>
inline PyArrayObject *ndarray_view(const Eigen::PlainObjectBase<Derived> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(const_cast<typename Derived::Scalar*>(m.data()), m.rows(), m.cols(), m.IsRowMajor);
}
template <typename Derived>
inline PyArrayObject *ndarray_copy(const Eigen::PlainObjectBase<Derived> &m) {
import_gtsam_eigency__conversions();
return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor);
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray(Eigen::Map<Derived, MapOptions, Stride> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray(const Eigen::Map<Derived, MapOptions, Stride> &m) {
import_gtsam_eigency__conversions();
// Since this is a map, we assume that ownership is correctly taken care
// of, and we avoid taking a copy
return _ndarray_view(const_cast<typename Derived::Scalar*>(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray_view(Eigen::Map<Derived, MapOptions, Stride> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray_view(const Eigen::Map<Derived, MapOptions, Stride> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(const_cast<typename Derived::Scalar*>(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray_copy(const Eigen::Map<Derived, MapOptions, Stride> &m) {
import_gtsam_eigency__conversions();
return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename MatrixType,
int _MapOptions = Eigen::Unaligned,
typename _StrideType=Eigen::Stride<0,0> >
class MapBase: public Eigen::Map<MatrixType, _MapOptions, _StrideType> {
public:
typedef Eigen::Map<MatrixType, _MapOptions, _StrideType> Base;
typedef typename Base::Scalar Scalar;
MapBase(Scalar* data,
long rows,
long cols,
_StrideType stride=_StrideType())
: Base(data,
// If both dimensions are dynamic or dimensions match, accept dimensions as they are
((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) ||
(Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols))
? rows
// otherwise, test if swapping them makes them fit
: ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows)
? cols
: rows),
((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) ||
(Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols))
? cols
: ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows)
? rows
: cols),
stride
) {}
MapBase &operator=(const MatrixType &other) {
Base::operator=(other);
return *this;
}
virtual ~MapBase() { }
};
template <template<class,int,int,int,int,int> class EigencyDenseBase,
typename Scalar,
int _Rows, int _Cols,
int _Options = Eigen::AutoAlign |
#if defined(__GNUC__) && __GNUC__==3 && __GNUC_MINOR__==4
// workaround a bug in at least gcc 3.4.6
// the innermost ?: ternary operator is misparsed. We write it slightly
// differently and this makes gcc 3.4.6 happy, but it's ugly.
// The error would only show up with EIGEN_DEFAULT_TO_ROW_MAJOR is defined
// (when EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION is RowMajor)
( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19
#if EIGEN_VERSION_AT_LEAST(3,2,90)
: !(_Cols==1 && _Rows!=1) ? EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION
#else
: !(_Cols==1 && _Rows!=1) ? Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION
#endif
: ColMajor ),
#else
( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
: (_Cols==1 && _Rows!=1) ? Eigen::ColMajor
// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19
#if EIGEN_VERSION_AT_LEAST(3,2,90)
: EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
#else
: Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
#endif
#endif
int _MapOptions = Eigen::Unaligned,
int _StrideOuter=0, int _StrideInner=0,
int _MaxRows = _Rows,
int _MaxCols = _Cols>
class FlattenedMap: public MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > {
public:
typedef MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base;
FlattenedMap()
: Base(NULL, 0, 0),
object_(NULL) {}
FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0)
: Base(data, rows, cols,
Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)),
object_(NULL) {
}
FlattenedMap(PyArrayObject *object)
: Base((Scalar *)((PyArrayObject*)object)->data,
// : Base(_from_numpy<Scalar>((PyArrayObject*)object),
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0],
Eigen::Stride<_StrideOuter, _StrideInner>(_StrideOuter != Eigen::Dynamic ? _StrideOuter : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
_StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])),
object_(object) {
if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
Py_XINCREF(object_);
}
FlattenedMap &operator=(const FlattenedMap &other) {
if (other.object_) {
new (this) FlattenedMap(other.object_);
} else {
// Replace the memory that we point to (not a memory allocation)
new (this) FlattenedMap(const_cast<Scalar*>(other.data()),
other.rows(),
other.cols(),
other.outerStride(),
other.innerStride());
}
return *this;
}
operator Base() const {
return static_cast<Base>(*this);
}
operator Base&() const {
return static_cast<Base&>(*this);
}
operator EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>() const {
return EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>(static_cast<Base>(*this));
}
virtual ~FlattenedMap() {
Py_XDECREF(object_);
}
private:
PyArrayObject * const object_;
};
template <typename MatrixType>
class Map: public MapBase<MatrixType> {
public:
typedef MapBase<MatrixType> Base;
typedef typename MatrixType::Scalar Scalar;
enum {
RowsAtCompileTime = Base::Base::RowsAtCompileTime,
ColsAtCompileTime = Base::Base::ColsAtCompileTime
};
Map()
: Base(NULL,
(RowsAtCompileTime == Eigen::Dynamic) ? 0 : RowsAtCompileTime,
(ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime),
object_(NULL) {
}
Map(Scalar *data, long rows, long cols)
: Base(data, rows, cols),
object_(NULL) {}
Map(PyArrayObject *object)
: Base((PyObject*)object == Py_None? NULL: (Scalar *)object->data,
// ROW: If array is in row-major order, transpose (see README)
(PyObject*)object == Py_None? 0 :
(!PyArray_IS_F_CONTIGUOUS(object)
? ((object->nd == 1)
? 1 // ROW: If 1D row-major numpy array, set to 1 (row vector)
: object->dimensions[1])
: object->dimensions[0]),
// COLUMN: If array is in row-major order: transpose (see README)
(PyObject*)object == Py_None? 0 :
(!PyArray_IS_F_CONTIGUOUS(object)
? object->dimensions[0]
: ((object->nd == 1)
? 1 // COLUMN: If 1D col-major numpy array, set to length (column vector)
: object->dimensions[1]))),
object_(object) {
if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
Py_XINCREF(object_);
}
Map &operator=(const Map &other) {
if (other.object_) {
new (this) Map(other.object_);
} else {
// Replace the memory that we point to (not a memory allocation)
new (this) Map(const_cast<Scalar*>(other.data()),
other.rows(),
other.cols());
}
return *this;
}
Map &operator=(const MatrixType &other) {
MapBase<MatrixType>::operator=(other);
return *this;
}
operator Base() const {
return static_cast<Base>(*this);
}
operator Base&() const {
return static_cast<Base&>(*this);
}
operator MatrixType() const {
return MatrixType(static_cast<Base>(*this));
}
virtual ~Map() {
Py_XDECREF(object_);
}
private:
PyArrayObject * const object_;
};
}
#endif

3
cython/requirements.txt
View File

@ -1,3 +0,0 @@
Cython>=0.25.2
backports_abc>=0.5
numpy>=1.11.0

4
examples/CameraResectioning.cpp
View File

@ -46,8 +46,8 @@ public:
}
/// evaluate the error
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const override {
PinholeCamera<Cal3_S2> camera(pose, *K_);
return camera.project(P_, H, boost::none, boost::none) - p_;
}

12
examples/Data/Klaus3.g2o
View File

@ -1,6 +1,6 @@
VERTEX_SE3:QUAT 0 -3.865747774038187 0.06639337702667497 -0.16064874691945374 0.024595211709139555 0.49179523413089893 -0.06279232989379242 0.8680954132776109
VERTEX_SE3:QUAT 1 -3.614793159814815 0.04774490041587656 -0.2837650367985949 0.00991721787943912 0.4854918961891193 -0.042343290945895576 0.8731588132957809
VERTEX_SE3:QUAT 2 -3.255096913553434 0.013296754286114112 -0.5339792269680574 -0.027851108010665374 0.585478168397957 -0.05088341463532465 0.8086102325762403
EDGE_SE3:QUAT 0 1 0.2509546142233723 -0.01864847661079841 -0.12311628987914114 -0.022048798853273946 -0.01796327847857683 0.010210006313668573 0.9995433591728293 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 0 2 0.6106508604847534 -0.05309662274056086 -0.3733304800486037 -0.054972994022992064 0.10432547598981769 -0.02221474884651081 0.9927742290779572 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 1 2 0.3596962462613811 -0.03444814612976245 -0.25021419016946256 -0.03174661848656213 0.11646825423134777 -0.02951742735854383 0.9922479626852876 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
VERTEX_SE3:QUAT 0 -1.6618596980158338 -0.5736497760548741 -3.3319774096611026 -0.02676080288219576 -0.024497002638379624 -0.015064701622500615 0.9992281076190063
VERTEX_SE3:QUAT 1 -1.431820463019384 -0.549139761976065 -3.160677992237872 -0.049543805396343954 -0.03232420352077356 -0.004386230477751116 0.998239108728862
VERTEX_SE3:QUAT 2 -1.0394840214436651 -0.5268841046291037 -2.972143862665523 -0.07993768981394891 0.0825062894866454 -0.04088089479075661 0.9925378735259738
EDGE_SE3:QUAT 0 1 0.23003923499644974 0.02451001407880915 0.17129941742323052 -0.022048798853273946 -0.01796327847857683 0.010210006313668573 0.9995433591728293 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 0 2 0.6223756765721686 0.04676567142577037 0.35983354699557957 -0.054972994022992064 0.10432547598981769 -0.02221474884651081 0.9927742290779572 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 1 2 0.3923364415757189 0.022255657346961222 0.18853412957234905 -0.03174661848656213 0.11646825423134777 -0.02951742735854383 0.9922479626852876 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0

16
examples/Data/example_with_vertices.g2o Normal file
View File

@ -0,0 +1,16 @@
VERTEX_SE3:QUAT 0 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162
VERTEX_SE3:QUAT 1 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508
VERTEX_SE3:QUAT 2 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10
VERTEX_SE3:QUAT 3 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508
VERTEX_SE3:QUAT 4 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162
VERTEX_SE3:QUAT 5 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567
VERTEX_SE3:QUAT 6 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412
VERTEX_SE3:QUAT 7 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567
VERTEX_TRACKXYZ 0 10 10 10
VERTEX_TRACKXYZ 1 -10 10 10
VERTEX_TRACKXYZ 2 -10 -10 10
VERTEX_TRACKXYZ 3 10 -10 10
VERTEX_TRACKXYZ 4 10 10 -10
VERTEX_TRACKXYZ 5 -10 10 -10
VERTEX_TRACKXYZ 6 -10 -10 -10
VERTEX_TRACKXYZ 7 10 -10 -10

2
examples/IMUKittiExampleGPS.cpp
View File

@ -287,7 +287,7 @@ int main(int argc, char* argv[]) {
new_values.insert(current_pose_key, gps_pose);
printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
gps_pose.translation().print();
cout << gps_pose.translation();
printf("\n\n");
} else {
new_values.insert(current_pose_key, current_pose_global);

4
examples/LocalizationExample.cpp
View File

@ -85,7 +85,7 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
// 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 {
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
@ -99,7 +99,7 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
// The second is a 'clone' function that allows the factor to be copied. Under most
// circumstances, the following code that employs the default copy constructor should
// work fine.
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }

125
examples/ShonanAveragingCLI.cpp Normal file
View File

@ -0,0 +1,125 @@
/* ----------------------------------------------------------------------------
* 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 ShonanAveragingCLI.cpp
* @brief Run Shonan Rotation Averaging Algorithm on a file or example dataset
* @author Frank Dellaert
* @date August, 2020
*
* Example usage:
*
* Running without arguments will run on tiny 3D example pose3example-grid
* ./ShonanAveragingCLI
*
* Read 2D dataset w10000 (in examples/data) and output to w10000-rotations.g2o
* ./ShonanAveragingCLI -d 2 -n w10000 -o w10000-rotations.g2o
*
* Read 3D dataset sphere25000.txt and output to shonan.g2o (default)
* ./ShonanAveragingCLI -i spere2500.txt
*
*/
#include <gtsam/base/timing.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/slam/InitializePose.h>
#include <gtsam/slam/dataset.h>
#include <boost/program_options.hpp>
using namespace std;
using namespace gtsam;
namespace po = boost::program_options;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
string datasetName;
string inputFile;
string outputFile;
int d, seed;
po::options_description desc(
"Shonan Rotation Averaging CLI reads a *pose* graph, extracts the "
"rotation constraints, and runs the Shonan algorithm.");
desc.add_options()("help", "Print help message")(
"named_dataset,n",
po::value<string>(&datasetName)->default_value("pose3example-grid"),
"Find and read frome example dataset file")(
"input_file,i", po::value<string>(&inputFile)->default_value(""),
"Read pose constraints graph from the specified file")(
"output_file,o",
po::value<string>(&outputFile)->default_value("shonan.g2o"),
"Write solution to the specified file")(
"dimension,d", po::value<int>(&d)->default_value(3),
"Optimize over 2D or 3D rotations")(
"seed,s", po::value<int>(&seed)->default_value(42),
"Random seed for initial estimate");
po::variables_map vm;
po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
po::notify(vm);
if (vm.count("help")) {
cout << desc << "\n";
return 1;
}
// Get input file
if (inputFile.empty()) {
if (datasetName.empty()) {
cout << "You must either specify a named dataset or an input file\n"
<< desc << endl;
return 1;
}
inputFile = findExampleDataFile(datasetName);
}
// Seed random number generator
static std::mt19937 rng(seed);
NonlinearFactorGraph::shared_ptr inputGraph;
Values::shared_ptr posesInFile;
Values poses;
if (d == 2) {
cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
ShonanAveraging2 shonan(inputFile);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load2D(inputFile);
auto priorModel = noiseModel::Unit::Create(3);
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
cout << "recovering 2D translations" << endl;
auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
poses = initialize::computePoses<Pose2>(result.first, &poseGraph);
} else if (d == 3) {
cout << "Running Shonan averaging for SO(3) on " << inputFile << endl;
ShonanAveraging3 shonan(inputFile);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load3D(inputFile);
auto priorModel = noiseModel::Unit::Create(6);
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
cout << "recovering 3D translations" << endl;
auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
poses = initialize::computePoses<Pose3>(result.first, &poseGraph);
} else {
cout << "Can only run SO(2) or SO(3) averaging\n" << desc << endl;
return 1;
}
cout << "Writing result to " << outputFile << endl;
writeG2o(NonlinearFactorGraph(), poses, outputFile);
return 0;
}
/* ************************************************************************* */

6
examples/SolverComparer.cpp
View File

@ -50,11 +50,11 @@
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/random.hpp>
#include <boost/serialization/export.hpp>
#include <fstream>
#include <iostream>
#include <random>
#ifdef GTSAM_USE_TBB
#include <tbb/task_arena.h> // tbb::task_arena
@ -554,8 +554,8 @@ void runCompare()
void runPerturb()
{
// Set up random number generator
boost::mt19937 rng;
boost::normal_distribution<double> normal(0.0, perturbationNoise);
std::mt19937 rng;
std::normal_distribution<double> normal(0.0, perturbationNoise);
// Perturb values
VectorValues noise;

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

@ -1,7 +1,8 @@
# install CCOLAMD headers
# install CCOLAMD and SuiteSparse_config headers
install(FILES CCOLAMD/Include/ccolamd.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/CCOLAMD)
install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/SuiteSparse_config)
# install Eigen unless System Eigen is used
if(NOT GTSAM_USE_SYSTEM_EIGEN)
# Find plain .h files
file(GLOB_RECURSE eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/Eigen/*.h")

482
gtsam/3rdparty/Spectra/GenEigsBase.h vendored Normal file
View File

@ -0,0 +1,482 @@
// Copyright (C) 2018-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef GEN_EIGS_BASE_H
#define GEN_EIGS_BASE_H
#include <Eigen/Core>
#include <vector> // std::vector
#include <cmath> // std::abs, std::pow, std::sqrt
#include <algorithm> // std::min, std::copy
#include <complex> // std::complex, std::conj, std::norm, std::abs
#include <stdexcept> // std::invalid_argument
#include "Util/TypeTraits.h"
#include "Util/SelectionRule.h"
#include "Util/CompInfo.h"
#include "Util/SimpleRandom.h"
#include "MatOp/internal/ArnoldiOp.h"
#include "LinAlg/UpperHessenbergQR.h"
#include "LinAlg/DoubleShiftQR.h"
#include "LinAlg/UpperHessenbergEigen.h"
#include "LinAlg/Arnoldi.h"
namespace Spectra {
///
/// \ingroup EigenSolver
///
/// This is the base class for general eigen solvers, mainly for internal use.
/// It is kept here to provide the documentation for member functions of concrete eigen solvers
/// such as GenEigsSolver and GenEigsRealShiftSolver.
///
template <typename Scalar,
int SelectionRule,
typename OpType,
typename BOpType>
class GenEigsBase
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Array<Scalar, Eigen::Dynamic, 1> Array;
typedef Eigen::Array<bool, Eigen::Dynamic, 1> BoolArray;
typedef Eigen::Map<Matrix> MapMat;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::Map<const Vector> MapConstVec;
typedef std::complex<Scalar> Complex;
typedef Eigen::Matrix<Complex, Eigen::Dynamic, Eigen::Dynamic> ComplexMatrix;
typedef Eigen::Matrix<Complex, Eigen::Dynamic, 1> ComplexVector;
typedef ArnoldiOp<Scalar, OpType, BOpType> ArnoldiOpType;
typedef Arnoldi<Scalar, ArnoldiOpType> ArnoldiFac;
protected:
// clang-format off
OpType* m_op; // object to conduct matrix operation,
// e.g. matrix-vector product
const Index m_n; // dimension of matrix A
const Index m_nev; // number of eigenvalues requested
const Index m_ncv; // dimension of Krylov subspace in the Arnoldi method
Index m_nmatop; // number of matrix operations called
Index m_niter; // number of restarting iterations
ArnoldiFac m_fac; // Arnoldi factorization
ComplexVector m_ritz_val; // Ritz values
ComplexMatrix m_ritz_vec; // Ritz vectors
ComplexVector m_ritz_est; // last row of m_ritz_vec
private:
BoolArray m_ritz_conv; // indicator of the convergence of Ritz values
int m_info; // status of the computation
const Scalar m_near_0; // a very small value, but 1.0 / m_near_0 does not overflow
// ~= 1e-307 for the "double" type
const Scalar m_eps; // the machine precision, ~= 1e-16 for the "double" type
const Scalar m_eps23; // m_eps^(2/3), used to test the convergence
// clang-format on
// Real Ritz values calculated from UpperHessenbergEigen have exact zero imaginary part
// Complex Ritz values have exact conjugate pairs
// So we use exact tests here
static bool is_complex(const Complex& v) { return v.imag() != Scalar(0); }
static bool is_conj(const Complex& v1, const Complex& v2) { return v1 == Eigen::numext::conj(v2); }
// Implicitly restarted Arnoldi factorization
void restart(Index k)
{
using std::norm;
if (k >= m_ncv)
return;
DoubleShiftQR<Scalar> decomp_ds(m_ncv);
UpperHessenbergQR<Scalar> decomp_hb(m_ncv);
Matrix Q = Matrix::Identity(m_ncv, m_ncv);
for (Index i = k; i < m_ncv; i++)
{
if (is_complex(m_ritz_val[i]) && is_conj(m_ritz_val[i], m_ritz_val[i + 1]))
{
// H - mu * I = Q1 * R1
// H <- R1 * Q1 + mu * I = Q1' * H * Q1
// H - conj(mu) * I = Q2 * R2
// H <- R2 * Q2 + conj(mu) * I = Q2' * H * Q2
//
// (H - mu * I) * (H - conj(mu) * I) = Q1 * Q2 * R2 * R1 = Q * R
const Scalar s = Scalar(2) * m_ritz_val[i].real();
const Scalar t = norm(m_ritz_val[i]);
decomp_ds.compute(m_fac.matrix_H(), s, t);
// Q -> Q * Qi
decomp_ds.apply_YQ(Q);
// H -> Q'HQ
// Matrix Q = Matrix::Identity(m_ncv, m_ncv);
// decomp_ds.apply_YQ(Q);
// m_fac_H = Q.transpose() * m_fac_H * Q;
m_fac.compress_H(decomp_ds);
i++;
}
else
{
// QR decomposition of H - mu * I, mu is real
decomp_hb.compute(m_fac.matrix_H(), m_ritz_val[i].real());
// Q -> Q * Qi
decomp_hb.apply_YQ(Q);
// H -> Q'HQ = RQ + mu * I
m_fac.compress_H(decomp_hb);
}
}
m_fac.compress_V(Q);
m_fac.factorize_from(k, m_ncv, m_nmatop);
retrieve_ritzpair();
}
// Calculates the number of converged Ritz values
Index num_converged(Scalar tol)
{
// thresh = tol * max(m_eps23, abs(theta)), theta for Ritz value
Array thresh = tol * m_ritz_val.head(m_nev).array().abs().max(m_eps23);
Array resid = m_ritz_est.head(m_nev).array().abs() * m_fac.f_norm();
// Converged "wanted" Ritz values
m_ritz_conv = (resid < thresh);
return m_ritz_conv.cast<Index>().sum();
}
// Returns the adjusted nev for restarting
Index nev_adjusted(Index nconv)
{
using std::abs;
Index nev_new = m_nev;
for (Index i = m_nev; i < m_ncv; i++)
if (abs(m_ritz_est[i]) < m_near_0)
nev_new++;
// Adjust nev_new, according to dnaup2.f line 660~674 in ARPACK
nev_new += std::min(nconv, (m_ncv - nev_new) / 2);
if (nev_new == 1 && m_ncv >= 6)
nev_new = m_ncv / 2;
else if (nev_new == 1 && m_ncv > 3)
nev_new = 2;
if (nev_new > m_ncv - 2)
nev_new = m_ncv - 2;
// Increase nev by one if ritz_val[nev - 1] and
// ritz_val[nev] are conjugate pairs
if (is_complex(m_ritz_val[nev_new - 1]) &&
is_conj(m_ritz_val[nev_new - 1], m_ritz_val[nev_new]))
{
nev_new++;
}
return nev_new;
}
// Retrieves and sorts Ritz values and Ritz vectors
void retrieve_ritzpair()
{
UpperHessenbergEigen<Scalar> decomp(m_fac.matrix_H());
const ComplexVector& evals = decomp.eigenvalues();
ComplexMatrix evecs = decomp.eigenvectors();
SortEigenvalue<Complex, SelectionRule> sorting(evals.data(), evals.size());
std::vector<int> ind = sorting.index();
// Copy the Ritz values and vectors to m_ritz_val and m_ritz_vec, respectively
for (Index i = 0; i < m_ncv; i++)
{
m_ritz_val[i] = evals[ind[i]];
m_ritz_est[i] = evecs(m_ncv - 1, ind[i]);
}
for (Index i = 0; i < m_nev; i++)
{
m_ritz_vec.col(i).noalias() = evecs.col(ind[i]);
}
}
protected:
// Sorts the first nev Ritz pairs in the specified order
// This is used to return the final results
virtual void sort_ritzpair(int sort_rule)
{
// First make sure that we have a valid index vector
SortEigenvalue<Complex, LARGEST_MAGN> sorting(m_ritz_val.data(), m_nev);
std::vector<int> ind = sorting.index();
switch (sort_rule)
{
case LARGEST_MAGN:
break;
case LARGEST_REAL:
{
SortEigenvalue<Complex, LARGEST_REAL> sorting(m_ritz_val.data(), m_nev);
ind = sorting.index();
break;
}
case LARGEST_IMAG:
{
SortEigenvalue<Complex, LARGEST_IMAG> sorting(m_ritz_val.data(), m_nev);
ind = sorting.index();
break;
}
case SMALLEST_MAGN:
{
SortEigenvalue<Complex, SMALLEST_MAGN> sorting(m_ritz_val.data(), m_nev);
ind = sorting.index();
break;
}
case SMALLEST_REAL:
{
SortEigenvalue<Complex, SMALLEST_REAL> sorting(m_ritz_val.data(), m_nev);
ind = sorting.index();
break;
}
case SMALLEST_IMAG:
{
SortEigenvalue<Complex, SMALLEST_IMAG> sorting(m_ritz_val.data(), m_nev);
ind = sorting.index();
break;
}
default:
throw std::invalid_argument("unsupported sorting rule");
}
ComplexVector new_ritz_val(m_ncv);
ComplexMatrix new_ritz_vec(m_ncv, m_nev);
BoolArray new_ritz_conv(m_nev);
for (Index i = 0; i < m_nev; i++)
{
new_ritz_val[i] = m_ritz_val[ind[i]];
new_ritz_vec.col(i).noalias() = m_ritz_vec.col(ind[i]);
new_ritz_conv[i] = m_ritz_conv[ind[i]];
}
m_ritz_val.swap(new_ritz_val);
m_ritz_vec.swap(new_ritz_vec);
m_ritz_conv.swap(new_ritz_conv);
}
public:
/// \cond
GenEigsBase(OpType* op, BOpType* Bop, Index nev, Index ncv) :
m_op(op),
m_n(m_op->rows()),
m_nev(nev),
m_ncv(ncv > m_n ? m_n : ncv),
m_nmatop(0),
m_niter(0),
m_fac(ArnoldiOpType(op, Bop), m_ncv),
m_info(NOT_COMPUTED),
m_near_0(TypeTraits<Scalar>::min() * Scalar(10)),
m_eps(Eigen::NumTraits<Scalar>::epsilon()),
m_eps23(Eigen::numext::pow(m_eps, Scalar(2.0) / 3))
{
if (nev < 1 || nev > m_n - 2)
throw std::invalid_argument("nev must satisfy 1 <= nev <= n - 2, n is the size of matrix");
if (ncv < nev + 2 || ncv > m_n)
throw std::invalid_argument("ncv must satisfy nev + 2 <= ncv <= n, n is the size of matrix");
}
///
/// Virtual destructor
///
virtual ~GenEigsBase() {}
/// \endcond
///
/// Initializes the solver by providing an initial residual vector.
///
/// \param init_resid Pointer to the initial residual vector.
///
/// **Spectra** (and also **ARPACK**) uses an iterative algorithm
/// to find eigenvalues. This function allows the user to provide the initial
/// residual vector.
///
void init(const Scalar* init_resid)
{
// Reset all matrices/vectors to zero
m_ritz_val.resize(m_ncv);
m_ritz_vec.resize(m_ncv, m_nev);
m_ritz_est.resize(m_ncv);
m_ritz_conv.resize(m_nev);
m_ritz_val.setZero();
m_ritz_vec.setZero();
m_ritz_est.setZero();
m_ritz_conv.setZero();
m_nmatop = 0;
m_niter = 0;
// Initialize the Arnoldi factorization
MapConstVec v0(init_resid, m_n);
m_fac.init(v0, m_nmatop);
}
///
/// Initializes the solver by providing a random initial residual vector.
///
/// This overloaded function generates a random initial residual vector
/// (with a fixed random seed) for the algorithm. Elements in the vector
/// follow independent Uniform(-0.5, 0.5) distribution.
///
void init()
{
SimpleRandom<Scalar> rng(0);
Vector init_resid = rng.random_vec(m_n);
init(init_resid.data());
}
///
/// Conducts the major computation procedure.
///
/// \param maxit Maximum number of iterations allowed in the algorithm.
/// \param tol Precision parameter for the calculated eigenvalues.
/// \param sort_rule Rule to sort the eigenvalues and eigenvectors.
/// Supported values are
/// `Spectra::LARGEST_MAGN`, `Spectra::LARGEST_REAL`,
/// `Spectra::LARGEST_IMAG`, `Spectra::SMALLEST_MAGN`,
/// `Spectra::SMALLEST_REAL` and `Spectra::SMALLEST_IMAG`,
/// for example `LARGEST_MAGN` indicates that eigenvalues
/// with largest magnitude come first.
/// Note that this argument is only used to
/// **sort** the final result, and the **selection** rule
/// (e.g. selecting the largest or smallest eigenvalues in the
/// full spectrum) is specified by the template parameter
/// `SelectionRule` of GenEigsSolver.
///
/// \return Number of converged eigenvalues.
///
Index compute(Index maxit = 1000, Scalar tol = 1e-10, int sort_rule = LARGEST_MAGN)
{
// The m-step Arnoldi factorization
m_fac.factorize_from(1, m_ncv, m_nmatop);
retrieve_ritzpair();
// Restarting
Index i, nconv = 0, nev_adj;
for (i = 0; i < maxit; i++)
{
nconv = num_converged(tol);
if (nconv >= m_nev)
break;
nev_adj = nev_adjusted(nconv);
restart(nev_adj);
}
// Sorting results
sort_ritzpair(sort_rule);
m_niter += i + 1;
m_info = (nconv >= m_nev) ? SUCCESSFUL : NOT_CONVERGING;
return std::min(m_nev, nconv);
}
///
/// Returns the status of the computation.
/// The full list of enumeration values can be found in \ref Enumerations.
///
int info() const { return m_info; }
///
/// Returns the number of iterations used in the computation.
///
Index num_iterations() const { return m_niter; }
///
/// Returns the number of matrix operations used in the computation.
///
Index num_operations() const { return m_nmatop; }
///
/// Returns the converged eigenvalues.
///
/// \return A complex-valued vector containing the eigenvalues.
/// Returned vector type will be `Eigen::Vector<std::complex<Scalar>, ...>`, depending on
/// the template parameter `Scalar` defined.
///
ComplexVector eigenvalues() const
{
const Index nconv = m_ritz_conv.cast<Index>().sum();
ComplexVector res(nconv);
if (!nconv)
return res;
Index j = 0;
for (Index i = 0; i < m_nev; i++)
{
if (m_ritz_conv[i])
{
res[j] = m_ritz_val[i];
j++;
}
}
return res;
}
///
/// Returns the eigenvectors associated with the converged eigenvalues.
///
/// \param nvec The number of eigenvectors to return.
///
/// \return A complex-valued matrix containing the eigenvectors.
/// Returned matrix type will be `Eigen::Matrix<std::complex<Scalar>, ...>`,
/// depending on the template parameter `Scalar` defined.
///
ComplexMatrix eigenvectors(Index nvec) const
{
const Index nconv = m_ritz_conv.cast<Index>().sum();
nvec = std::min(nvec, nconv);
ComplexMatrix res(m_n, nvec);
if (!nvec)
return res;
ComplexMatrix ritz_vec_conv(m_ncv, nvec);
Index j = 0;
for (Index i = 0; i < m_nev && j < nvec; i++)
{
if (m_ritz_conv[i])
{
ritz_vec_conv.col(j).noalias() = m_ritz_vec.col(i);
j++;
}
}
res.noalias() = m_fac.matrix_V() * ritz_vec_conv;
return res;
}
///
/// Returns all converged eigenvectors.
///
ComplexMatrix eigenvectors() const
{
return eigenvectors(m_nev);
}
};
} // namespace Spectra
#endif // GEN_EIGS_BASE_H

157
gtsam/3rdparty/Spectra/GenEigsComplexShiftSolver.h vendored Normal file
View File

@ -0,0 +1,157 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef GEN_EIGS_COMPLEX_SHIFT_SOLVER_H
#define GEN_EIGS_COMPLEX_SHIFT_SOLVER_H
#include <Eigen/Core>
#include "GenEigsBase.h"
#include "Util/SelectionRule.h"
#include "MatOp/DenseGenComplexShiftSolve.h"
namespace Spectra {
///
/// \ingroup EigenSolver
///
/// This class implements the eigen solver for general real matrices with
/// a complex shift value in the **shift-and-invert mode**. The background
/// knowledge of the shift-and-invert mode can be found in the documentation
/// of the SymEigsShiftSolver class.
///
/// \tparam Scalar The element type of the matrix.
/// Currently supported types are `float`, `double` and `long double`.
/// \tparam SelectionRule An enumeration value indicating the selection rule of
/// the shifted-and-inverted eigenvalues.
/// The full list of enumeration values can be found in
/// \ref Enumerations.
/// \tparam OpType The name of the matrix operation class. Users could either
/// use the DenseGenComplexShiftSolve wrapper class, or define their
/// own that implements all the public member functions as in
/// DenseGenComplexShiftSolve.
///
template <typename Scalar = double,
int SelectionRule = LARGEST_MAGN,
typename OpType = DenseGenComplexShiftSolve<double> >
class GenEigsComplexShiftSolver : public GenEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>
{
private:
typedef Eigen::Index Index;
typedef std::complex<Scalar> Complex;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Matrix<Complex, Eigen::Dynamic, 1> ComplexVector;
const Scalar m_sigmar;
const Scalar m_sigmai;
// First transform back the Ritz values, and then sort
void sort_ritzpair(int sort_rule)
{
using std::abs;
using std::sqrt;
using std::norm;
// The eigenvalues we get from the iteration is
// nu = 0.5 * (1 / (lambda - sigma) + 1 / (lambda - conj(sigma)))
// So the eigenvalues of the original problem is
// 1 \pm sqrt(1 - 4 * nu^2 * sigmai^2)
// lambda = sigmar + -----------------------------------
// 2 * nu
// We need to pick the correct root
// Let (lambdaj, vj) be the j-th eigen pair, then A * vj = lambdaj * vj
// and inv(A - r * I) * vj = 1 / (lambdaj - r) * vj
// where r is any shift value.
// We can use this identity to determine lambdaj
//
// op(v) computes Re(inv(A - r * I) * v) for any real v
// If r is real, then op(v) is also real. Let a = Re(vj), b = Im(vj),
// then op(vj) = op(a) + op(b) * i
// By comparing op(vj) and [1 / (lambdaj - r) * vj], we can determine
// which one is the correct root
// Select a random shift value
SimpleRandom<Scalar> rng(0);
const Scalar shiftr = rng.random() * m_sigmar + rng.random();
const Complex shift = Complex(shiftr, Scalar(0));
this->m_op->set_shift(shiftr, Scalar(0));
// Calculate inv(A - r * I) * vj
Vector v_real(this->m_n), v_imag(this->m_n), OPv_real(this->m_n), OPv_imag(this->m_n);
const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
for (Index i = 0; i < this->m_nev; i++)
{
v_real.noalias() = this->m_fac.matrix_V() * this->m_ritz_vec.col(i).real();
v_imag.noalias() = this->m_fac.matrix_V() * this->m_ritz_vec.col(i).imag();
this->m_op->perform_op(v_real.data(), OPv_real.data());
this->m_op->perform_op(v_imag.data(), OPv_imag.data());
// Two roots computed from the quadratic equation
const Complex nu = this->m_ritz_val[i];
const Complex root_part1 = m_sigmar + Scalar(0.5) / nu;
const Complex root_part2 = Scalar(0.5) * sqrt(Scalar(1) - Scalar(4) * m_sigmai * m_sigmai * (nu * nu)) / nu;
const Complex root1 = root_part1 + root_part2;
const Complex root2 = root_part1 - root_part2;
// Test roots
Scalar err1 = Scalar(0), err2 = Scalar(0);
for (int k = 0; k < this->m_n; k++)
{
const Complex rhs1 = Complex(v_real[k], v_imag[k]) / (root1 - shift);
const Complex rhs2 = Complex(v_real[k], v_imag[k]) / (root2 - shift);
const Complex OPv = Complex(OPv_real[k], OPv_imag[k]);
err1 += norm(OPv - rhs1);
err2 += norm(OPv - rhs2);
}
const Complex lambdaj = (err1 < err2) ? root1 : root2;
this->m_ritz_val[i] = lambdaj;
if (abs(Eigen::numext::imag(lambdaj)) > eps)
{
this->m_ritz_val[i + 1] = Eigen::numext::conj(lambdaj);
i++;
}
else
{
this->m_ritz_val[i] = Complex(Eigen::numext::real(lambdaj), Scalar(0));
}
}
GenEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>::sort_ritzpair(sort_rule);
}
public:
///
/// Constructor to create a eigen solver object using the shift-and-invert mode.
///
/// \param op Pointer to the matrix operation object. This class should implement
/// the complex shift-solve operation of \f$A\f$: calculating
/// \f$\mathrm{Re}\{(A-\sigma I)^{-1}v\}\f$ for any vector \f$v\f$. Users could either
/// create the object from the DenseGenComplexShiftSolve wrapper class, or
/// define their own that implements all the public member functions
/// as in DenseGenComplexShiftSolve.
/// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-2\f$,
/// where \f$n\f$ is the size of matrix.
/// \param ncv Parameter that controls the convergence speed of the algorithm.
/// Typically a larger `ncv` means faster convergence, but it may
/// also result in greater memory use and more matrix operations
/// in each iteration. This parameter must satisfy \f$nev+2 \le ncv \le n\f$,
/// and is advised to take \f$ncv \ge 2\cdot nev + 1\f$.
/// \param sigmar The real part of the shift.
/// \param sigmai The imaginary part of the shift.
///
GenEigsComplexShiftSolver(OpType* op, Index nev, Index ncv, const Scalar& sigmar, const Scalar& sigmai) :
GenEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>(op, NULL, nev, ncv),
m_sigmar(sigmar), m_sigmai(sigmai)
{
this->m_op->set_shift(m_sigmar, m_sigmai);
}
};
} // namespace Spectra
#endif // GEN_EIGS_COMPLEX_SHIFT_SOLVER_H

89
gtsam/3rdparty/Spectra/GenEigsRealShiftSolver.h vendored Normal file
View File

@ -0,0 +1,89 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef GEN_EIGS_REAL_SHIFT_SOLVER_H
#define GEN_EIGS_REAL_SHIFT_SOLVER_H
#include <Eigen/Core>
#include "GenEigsBase.h"
#include "Util/SelectionRule.h"
#include "MatOp/DenseGenRealShiftSolve.h"
namespace Spectra {
///
/// \ingroup EigenSolver
///
/// This class implements the eigen solver for general real matrices with
/// a real shift value in the **shift-and-invert mode**. The background
/// knowledge of the shift-and-invert mode can be found in the documentation
/// of the SymEigsShiftSolver class.
///
/// \tparam Scalar The element type of the matrix.
/// Currently supported types are `float`, `double` and `long double`.
/// \tparam SelectionRule An enumeration value indicating the selection rule of
/// the shifted-and-inverted eigenvalues.
/// The full list of enumeration values can be found in
/// \ref Enumerations.
/// \tparam OpType The name of the matrix operation class. Users could either
/// use the wrapper classes such as DenseGenRealShiftSolve and
/// SparseGenRealShiftSolve, or define their
/// own that implements all the public member functions as in
/// DenseGenRealShiftSolve.
///
template <typename Scalar = double,
int SelectionRule = LARGEST_MAGN,
typename OpType = DenseGenRealShiftSolve<double> >
class GenEigsRealShiftSolver : public GenEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>
{
private:
typedef Eigen::Index Index;
typedef std::complex<Scalar> Complex;
typedef Eigen::Array<Complex, Eigen::Dynamic, 1> ComplexArray;
const Scalar m_sigma;
// First transform back the Ritz values, and then sort
void sort_ritzpair(int sort_rule)
{
// The eigenvalues we get from the iteration is nu = 1 / (lambda - sigma)
// So the eigenvalues of the original problem is lambda = 1 / nu + sigma
ComplexArray ritz_val_org = Scalar(1.0) / this->m_ritz_val.head(this->m_nev).array() + m_sigma;
this->m_ritz_val.head(this->m_nev) = ritz_val_org;
GenEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>::sort_ritzpair(sort_rule);
}
public:
///
/// Constructor to create a eigen solver object using the shift-and-invert mode.
///
/// \param op Pointer to the matrix operation object. This class should implement
/// the shift-solve operation of \f$A\f$: calculating
/// \f$(A-\sigma I)^{-1}v\f$ for any vector \f$v\f$. Users could either
/// create the object from the wrapper class such as DenseGenRealShiftSolve, or
/// define their own that implements all the public member functions
/// as in DenseGenRealShiftSolve.
/// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-2\f$,
/// where \f$n\f$ is the size of matrix.
/// \param ncv Parameter that controls the convergence speed of the algorithm.
/// Typically a larger `ncv` means faster convergence, but it may
/// also result in greater memory use and more matrix operations
/// in each iteration. This parameter must satisfy \f$nev+2 \le ncv \le n\f$,
/// and is advised to take \f$ncv \ge 2\cdot nev + 1\f$.
/// \param sigma The real-valued shift.
///
GenEigsRealShiftSolver(OpType* op, Index nev, Index ncv, Scalar sigma) :
GenEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>(op, NULL, nev, ncv),
m_sigma(sigma)
{
this->m_op->set_shift(m_sigma);
}
};
} // namespace Spectra
#endif // GEN_EIGS_REAL_SHIFT_SOLVER_H

158
gtsam/3rdparty/Spectra/GenEigsSolver.h vendored Normal file
View File

@ -0,0 +1,158 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef GEN_EIGS_SOLVER_H
#define GEN_EIGS_SOLVER_H
#include <Eigen/Core>
#include "GenEigsBase.h"
#include "Util/SelectionRule.h"
#include "MatOp/DenseGenMatProd.h"
namespace Spectra {
///
/// \ingroup EigenSolver
///
/// This class implements the eigen solver for general real matrices, i.e.,
/// to solve \f$Ax=\lambda x\f$ for a possibly non-symmetric \f$A\f$ matrix.
///
/// Most of the background information documented in the SymEigsSolver class
/// also applies to the GenEigsSolver class here, except that the eigenvalues
/// and eigenvectors of a general matrix can now be complex-valued.
///
/// \tparam Scalar The element type of the matrix.
/// Currently supported types are `float`, `double` and `long double`.
/// \tparam SelectionRule An enumeration value indicating the selection rule of
/// the requested eigenvalues, for example `LARGEST_MAGN`
/// to retrieve eigenvalues with the largest magnitude.
/// The full list of enumeration values can be found in
/// \ref Enumerations.
/// \tparam OpType The name of the matrix operation class. Users could either
/// use the wrapper classes such as DenseGenMatProd and
/// SparseGenMatProd, or define their
/// own that implements all the public member functions as in
/// DenseGenMatProd.
///
/// An example that illustrates the usage of GenEigsSolver is give below:
///
/// \code{.cpp}
/// #include <Eigen/Core>
/// #include <Spectra/GenEigsSolver.h>
/// // <Spectra/MatOp/DenseGenMatProd.h> is implicitly included
/// #include <iostream>
///
/// using namespace Spectra;
///
/// int main()
/// {
/// // We are going to calculate the eigenvalues of M
/// Eigen::MatrixXd M = Eigen::MatrixXd::Random(10, 10);
///
/// // Construct matrix operation object using the wrapper class
/// DenseGenMatProd<double> op(M);
///
/// // Construct eigen solver object, requesting the largest
/// // (in magnitude, or norm) three eigenvalues
/// GenEigsSolver< double, LARGEST_MAGN, DenseGenMatProd<double> > eigs(&op, 3, 6);
///
/// // Initialize and compute
/// eigs.init();
/// int nconv = eigs.compute();
///
/// // Retrieve results
/// Eigen::VectorXcd evalues;
/// if(eigs.info() == SUCCESSFUL)
/// evalues = eigs.eigenvalues();
///
/// std::cout << "Eigenvalues found:\n" << evalues << std::endl;
///
/// return 0;
/// }
/// \endcode
///
/// And also an example for sparse matrices:
///
/// \code{.cpp}
/// #include <Eigen/Core>
/// #include <Eigen/SparseCore>
/// #include <Spectra/GenEigsSolver.h>
/// #include <Spectra/MatOp/SparseGenMatProd.h>
/// #include <iostream>
///
/// using namespace Spectra;
///
/// int main()
/// {
/// // A band matrix with 1 on the main diagonal, 2 on the below-main subdiagonal,
/// // and 3 on the above-main subdiagonal
/// const int n = 10;
/// Eigen::SparseMatrix<double> M(n, n);
/// M.reserve(Eigen::VectorXi::Constant(n, 3));
/// for(int i = 0; i < n; i++)
/// {
/// M.insert(i, i) = 1.0;
/// if(i > 0)
/// M.insert(i - 1, i) = 3.0;
/// if(i < n - 1)
/// M.insert(i + 1, i) = 2.0;
/// }
///
/// // Construct matrix operation object using the wrapper class SparseGenMatProd
/// SparseGenMatProd<double> op(M);
///
/// // Construct eigen solver object, requesting the largest three eigenvalues
/// GenEigsSolver< double, LARGEST_MAGN, SparseGenMatProd<double> > eigs(&op, 3, 6);
///
/// // Initialize and compute
/// eigs.init();
/// int nconv = eigs.compute();
///
/// // Retrieve results
/// Eigen::VectorXcd evalues;
/// if(eigs.info() == SUCCESSFUL)
/// evalues = eigs.eigenvalues();
///
/// std::cout << "Eigenvalues found:\n" << evalues << std::endl;
///
/// return 0;
/// }
/// \endcode
template <typename Scalar = double,
int SelectionRule = LARGEST_MAGN,
typename OpType = DenseGenMatProd<double> >
class GenEigsSolver : public GenEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>
{
private:
typedef Eigen::Index Index;
public:
///
/// Constructor to create a solver object.
///
/// \param op Pointer to the matrix operation object, which should implement
/// the matrix-vector multiplication operation of \f$A\f$:
/// calculating \f$Av\f$ for any vector \f$v\f$. Users could either
/// create the object from the wrapper class such as DenseGenMatProd, or
/// define their own that implements all the public member functions
/// as in DenseGenMatProd.
/// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-2\f$,
/// where \f$n\f$ is the size of matrix.
/// \param ncv Parameter that controls the convergence speed of the algorithm.
/// Typically a larger `ncv` means faster convergence, but it may
/// also result in greater memory use and more matrix operations
/// in each iteration. This parameter must satisfy \f$nev+2 \le ncv \le n\f$,
/// and is advised to take \f$ncv \ge 2\cdot nev + 1\f$.
///
GenEigsSolver(OpType* op, Index nev, Index ncv) :
GenEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>(op, NULL, nev, ncv)
{}
};
} // namespace Spectra
#endif // GEN_EIGS_SOLVER_H

284
gtsam/3rdparty/Spectra/LinAlg/Arnoldi.h vendored Normal file
View File

@ -0,0 +1,284 @@
// Copyright (C) 2018-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef ARNOLDI_H
#define ARNOLDI_H
#include <Eigen/Core>
#include <cmath> // std::sqrt
#include <stdexcept> // std::invalid_argument
#include <sstream> // std::stringstream
#include "../MatOp/internal/ArnoldiOp.h"
#include "../Util/TypeTraits.h"
#include "../Util/SimpleRandom.h"
#include "UpperHessenbergQR.h"
#include "DoubleShiftQR.h"
namespace Spectra {
// Arnoldi factorization A * V = V * H + f * e'
// A: n x n
// V: n x k
// H: k x k
// f: n x 1
// e: [0, ..., 0, 1]
// V and H are allocated of dimension m, so the maximum value of k is m
template <typename Scalar, typename ArnoldiOpType>
class Arnoldi
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<Matrix> MapMat;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::Map<const Matrix> MapConstMat;
typedef Eigen::Map<const Vector> MapConstVec;
protected:
// clang-format off
ArnoldiOpType m_op; // Operators for the Arnoldi factorization
const Index m_n; // dimension of A
const Index m_m; // maximum dimension of subspace V
Index m_k; // current dimension of subspace V
Matrix m_fac_V; // V matrix in the Arnoldi factorization
Matrix m_fac_H; // H matrix in the Arnoldi factorization
Vector m_fac_f; // residual in the Arnoldi factorization
Scalar m_beta; // ||f||, B-norm of f
const Scalar m_near_0; // a very small value, but 1.0 / m_near_0 does not overflow
// ~= 1e-307 for the "double" type
const Scalar m_eps; // the machine precision, ~= 1e-16 for the "double" type
// clang-format on
// Given orthonormal basis functions V, find a nonzero vector f such that V'Bf = 0
// Assume that f has been properly allocated
void expand_basis(MapConstMat& V, const Index seed, Vector& f, Scalar& fnorm)
{
using std::sqrt;
const Scalar thresh = m_eps * sqrt(Scalar(m_n));
Vector Vf(V.cols());
for (Index iter = 0; iter < 5; iter++)
{
// Randomly generate a new vector and orthogonalize it against V
SimpleRandom<Scalar> rng(seed + 123 * iter);
f.noalias() = rng.random_vec(m_n);
// f <- f - V * V'Bf, so that f is orthogonal to V in B-norm
m_op.trans_product(V, f, Vf);
f.noalias() -= V * Vf;
// fnorm <- ||f||
fnorm = m_op.norm(f);
// If fnorm is too close to zero, we try a new random vector,
// otherwise return the result
if (fnorm >= thresh)
return;
}
}
public:
Arnoldi(const ArnoldiOpType& op, Index m) :
m_op(op), m_n(op.rows()), m_m(m), m_k(0),
m_near_0(TypeTraits<Scalar>::min() * Scalar(10)),
m_eps(Eigen::NumTraits<Scalar>::epsilon())
{}
virtual ~Arnoldi() {}
// Const-reference to internal structures
const Matrix& matrix_V() const { return m_fac_V; }
const Matrix& matrix_H() const { return m_fac_H; }
const Vector& vector_f() const { return m_fac_f; }
Scalar f_norm() const { return m_beta; }
Index subspace_dim() const { return m_k; }
// Initialize with an operator and an initial vector
void init(MapConstVec& v0, Index& op_counter)
{
m_fac_V.resize(m_n, m_m);
m_fac_H.resize(m_m, m_m);
m_fac_f.resize(m_n);
m_fac_H.setZero();
// Verify the initial vector
const Scalar v0norm = m_op.norm(v0);
if (v0norm < m_near_0)
throw std::invalid_argument("initial residual vector cannot be zero");
// Points to the first column of V
MapVec v(m_fac_V.data(), m_n);
// Normalize
v.noalias() = v0 / v0norm;
// Compute H and f
Vector w(m_n);
m_op.perform_op(v.data(), w.data());
op_counter++;
m_fac_H(0, 0) = m_op.inner_product(v, w);
m_fac_f.noalias() = w - v * m_fac_H(0, 0);
// In some cases f is zero in exact arithmetics, but due to rounding errors
// it may contain tiny fluctuations. When this happens, we force f to be zero
if (m_fac_f.cwiseAbs().maxCoeff() < m_eps)
{
m_fac_f.setZero();
m_beta = Scalar(0);
}
else
{
m_beta = m_op.norm(m_fac_f);
}
// Indicate that this is a step-1 factorization
m_k = 1;
}
// Arnoldi factorization starting from step-k
virtual void factorize_from(Index from_k, Index to_m, Index& op_counter)
{
using std::sqrt;
if (to_m <= from_k)
return;
if (from_k > m_k)
{
std::stringstream msg;
msg << "Arnoldi: from_k (= " << from_k << ") is larger than the current subspace dimension (= " << m_k << ")";
throw std::invalid_argument(msg.str());
}
const Scalar beta_thresh = m_eps * sqrt(Scalar(m_n));
// Pre-allocate vectors
Vector Vf(to_m);
Vector w(m_n);
// Keep the upperleft k x k submatrix of H and set other elements to 0
m_fac_H.rightCols(m_m - from_k).setZero();
m_fac_H.block(from_k, 0, m_m - from_k, from_k).setZero();
for (Index i = from_k; i <= to_m - 1; i++)
{
bool restart = false;
// If beta = 0, then the next V is not full rank
// We need to generate a new residual vector that is orthogonal
// to the current V, which we call a restart
if (m_beta < m_near_0)
{
MapConstMat V(m_fac_V.data(), m_n, i); // The first i columns
expand_basis(V, 2 * i, m_fac_f, m_beta);
restart = true;
}
// v <- f / ||f||
m_fac_V.col(i).noalias() = m_fac_f / m_beta; // The (i+1)-th column
// Note that H[i+1, i] equals to the unrestarted beta
m_fac_H(i, i - 1) = restart ? Scalar(0) : m_beta;
// w <- A * v, v = m_fac_V.col(i)
m_op.perform_op(&m_fac_V(0, i), w.data());
op_counter++;
const Index i1 = i + 1;
// First i+1 columns of V
MapConstMat Vs(m_fac_V.data(), m_n, i1);
// h = m_fac_H(0:i, i)
MapVec h(&m_fac_H(0, i), i1);
// h <- V'Bw
m_op.trans_product(Vs, w, h);
// f <- w - V * h
m_fac_f.noalias() = w - Vs * h;
m_beta = m_op.norm(m_fac_f);
if (m_beta > Scalar(0.717) * m_op.norm(h))
continue;
// f/||f|| is going to be the next column of V, so we need to test
// whether V'B(f/||f||) ~= 0
m_op.trans_product(Vs, m_fac_f, Vf.head(i1));
Scalar ortho_err = Vf.head(i1).cwiseAbs().maxCoeff();
// If not, iteratively correct the residual
int count = 0;
while (count < 5 && ortho_err > m_eps * m_beta)
{
// There is an edge case: when beta=||f|| is close to zero, f mostly consists
// of noises of rounding errors, so the test [ortho_err < eps * beta] is very
// likely to fail. In particular, if beta=0, then the test is ensured to fail.
// Hence when this happens, we force f to be zero, and then restart in the
// next iteration.
if (m_beta < beta_thresh)
{
m_fac_f.setZero();
m_beta = Scalar(0);
break;
}
// f <- f - V * Vf
m_fac_f.noalias() -= Vs * Vf.head(i1);
// h <- h + Vf
h.noalias() += Vf.head(i1);
// beta <- ||f||
m_beta = m_op.norm(m_fac_f);
m_op.trans_product(Vs, m_fac_f, Vf.head(i1));
ortho_err = Vf.head(i1).cwiseAbs().maxCoeff();
count++;
}
}
// Indicate that this is a step-m factorization
m_k = to_m;
}
// Apply H -> Q'HQ, where Q is from a double shift QR decomposition
void compress_H(const DoubleShiftQR<Scalar>& decomp)
{
decomp.matrix_QtHQ(m_fac_H);
m_k -= 2;
}
// Apply H -> Q'HQ, where Q is from an upper Hessenberg QR decomposition
void compress_H(const UpperHessenbergQR<Scalar>& decomp)
{
decomp.matrix_QtHQ(m_fac_H);
m_k--;
}
// Apply V -> VQ and compute the new f.
// Should be called after compress_H(), since m_k is updated there.
// Only need to update the first k+1 columns of V
// The first (m - k + i) elements of the i-th column of Q are non-zero,
// and the rest are zero
void compress_V(const Matrix& Q)
{
Matrix Vs(m_n, m_k + 1);
for (Index i = 0; i < m_k; i++)
{
const Index nnz = m_m - m_k + i + 1;
MapConstVec q(&Q(0, i), nnz);
Vs.col(i).noalias() = m_fac_V.leftCols(nnz) * q;
}
Vs.col(m_k).noalias() = m_fac_V * Q.col(m_k);
m_fac_V.leftCols(m_k + 1).noalias() = Vs;
Vector fk = m_fac_f * Q(m_m - 1, m_k - 1) + m_fac_V.col(m_k) * m_fac_H(m_k, m_k - 1);
m_fac_f.swap(fk);
m_beta = m_op.norm(m_fac_f);
}
};
} // namespace Spectra
#endif // ARNOLDI_H

530
gtsam/3rdparty/Spectra/LinAlg/BKLDLT.h vendored Normal file
View File

@ -0,0 +1,530 @@
// Copyright (C) 2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef BK_LDLT_H
#define BK_LDLT_H
#include <Eigen/Core>
#include <vector>
#include <stdexcept>
#include "../Util/CompInfo.h"
namespace Spectra {
// Bunch-Kaufman LDLT decomposition
// References:
// 1. Bunch, J. R., & Kaufman, L. (1977). Some stable methods for calculating inertia and solving symmetric linear systems.
// Mathematics of computation, 31(137), 163-179.
// 2. Golub, G. H., & Van Loan, C. F. (2012). Matrix computations (Vol. 3). JHU press. Section 4.4.
// 3. Bunch-Parlett diagonal pivoting <http://oz.nthu.edu.tw/~d947207/Chap13_GE3.ppt>
// 4. Ashcraft, C., Grimes, R. G., & Lewis, J. G. (1998). Accurate symmetric indefinite linear equation solvers.
// SIAM Journal on Matrix Analysis and Applications, 20(2), 513-561.
template <typename Scalar = double>
class BKLDLT
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Matrix<Index, Eigen::Dynamic, 1> IntVector;
typedef Eigen::Ref<Vector> GenericVector;
typedef Eigen::Ref<Matrix> GenericMatrix;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
typedef const Eigen::Ref<const Vector> ConstGenericVector;
Index m_n;
Vector m_data; // storage for a lower-triangular matrix
std::vector<Scalar*> m_colptr; // pointers to columns
IntVector m_perm; // [-2, -1, 3, 1, 4, 5]: 0 <-> 2, 1 <-> 1, 2 <-> 3, 3 <-> 1, 4 <-> 4, 5 <-> 5
std::vector<std::pair<Index, Index> > m_permc; // compressed version of m_perm: [(0, 2), (2, 3), (3, 1)]
bool m_computed;
int m_info;
// Access to elements
// Pointer to the k-th column
Scalar* col_pointer(Index k) { return m_colptr[k]; }
// A[i, j] -> m_colptr[j][i - j], i >= j
Scalar& coeff(Index i, Index j) { return m_colptr[j][i - j]; }
const Scalar& coeff(Index i, Index j) const { return m_colptr[j][i - j]; }
// A[i, i] -> m_colptr[i][0]
Scalar& diag_coeff(Index i) { return m_colptr[i][0]; }
const Scalar& diag_coeff(Index i) const { return m_colptr[i][0]; }
// Compute column pointers
void compute_pointer()
{
m_colptr.clear();
m_colptr.reserve(m_n);
Scalar* head = m_data.data();
for (Index i = 0; i < m_n; i++)
{
m_colptr.push_back(head);
head += (m_n - i);
}
}
// Copy mat - shift * I to m_data
void copy_data(ConstGenericMatrix& mat, int uplo, const Scalar& shift)
{
if (uplo == Eigen::Lower)
{
for (Index j = 0; j < m_n; j++)
{
const Scalar* begin = &mat.coeffRef(j, j);
const Index len = m_n - j;
std::copy(begin, begin + len, col_pointer(j));
diag_coeff(j) -= shift;
}
}
else
{
Scalar* dest = m_data.data();
for (Index i = 0; i < m_n; i++)
{
for (Index j = i; j < m_n; j++, dest++)
{
*dest = mat.coeff(i, j);
}
diag_coeff(i) -= shift;
}
}
}
// Compute compressed permutations
void compress_permutation()
{
for (Index i = 0; i < m_n; i++)
{
// Recover the permutation action
const Index perm = (m_perm[i] >= 0) ? (m_perm[i]) : (-m_perm[i] - 1);
if (perm != i)
m_permc.push_back(std::make_pair(i, perm));
}
}
// Working on the A[k:end, k:end] submatrix
// Exchange k <-> r
// Assume r >= k
void pivoting_1x1(Index k, Index r)
{
// No permutation
if (k == r)
{
m_perm[k] = r;
return;
}
// A[k, k] <-> A[r, r]
std::swap(diag_coeff(k), diag_coeff(r));
// A[(r+1):end, k] <-> A[(r+1):end, r]
std::swap_ranges(&coeff(r + 1, k), col_pointer(k + 1), &coeff(r + 1, r));
// A[(k+1):(r-1), k] <-> A[r, (k+1):(r-1)]
Scalar* src = &coeff(k + 1, k);
for (Index j = k + 1; j < r; j++, src++)
{
std::swap(*src, coeff(r, j));
}
m_perm[k] = r;
}
// Working on the A[k:end, k:end] submatrix
// Exchange [k+1, k] <-> [r, p]
// Assume p >= k, r >= k+1
void pivoting_2x2(Index k, Index r, Index p)
{
pivoting_1x1(k, p);
pivoting_1x1(k + 1, r);
// A[k+1, k] <-> A[r, k]
std::swap(coeff(k + 1, k), coeff(r, k));
// Use negative signs to indicate a 2x2 block
// Also minus one to distinguish a negative zero from a positive zero
m_perm[k] = -m_perm[k] - 1;
m_perm[k + 1] = -m_perm[k + 1] - 1;
}
// A[r1, c1:c2] <-> A[r2, c1:c2]
// Assume r2 >= r1 > c2 >= c1
void interchange_rows(Index r1, Index r2, Index c1, Index c2)
{
if (r1 == r2)
return;
for (Index j = c1; j <= c2; j++)
{
std::swap(coeff(r1, j), coeff(r2, j));
}
}
// lambda = |A[r, k]| = max{|A[k+1, k]|, ..., |A[end, k]|}
// Largest (in magnitude) off-diagonal element in the first column of the current reduced matrix
// r is the row index
// Assume k < end
Scalar find_lambda(Index k, Index& r)
{
using std::abs;
const Scalar* head = col_pointer(k); // => A[k, k]
const Scalar* end = col_pointer(k + 1);
// Start with r=k+1, lambda=A[k+1, k]
r = k + 1;
Scalar lambda = abs(head[1]);
// Scan remaining elements
for (const Scalar* ptr = head + 2; ptr < end; ptr++)
{
const Scalar abs_elem = abs(*ptr);
if (lambda < abs_elem)
{
lambda = abs_elem;
r = k + (ptr - head);
}
}
return lambda;
}
// sigma = |A[p, r]| = max {|A[k, r]|, ..., |A[end, r]|} \ {A[r, r]}
// Largest (in magnitude) off-diagonal element in the r-th column of the current reduced matrix
// p is the row index
// Assume k < r < end
Scalar find_sigma(Index k, Index r, Index& p)
{
using std::abs;
// First search A[r+1, r], ..., A[end, r], which has the same task as find_lambda()
// If r == end, we skip this search
Scalar sigma = Scalar(-1);
if (r < m_n - 1)
sigma = find_lambda(r, p);
// Then search A[k, r], ..., A[r-1, r], which maps to A[r, k], ..., A[r, r-1]
for (Index j = k; j < r; j++)
{
const Scalar abs_elem = abs(coeff(r, j));
if (sigma < abs_elem)
{
sigma = abs_elem;
p = j;
}
}
return sigma;
}
// Generate permutations and apply to A
// Return true if the resulting pivoting is 1x1, and false if 2x2
bool permutate_mat(Index k, const Scalar& alpha)
{
using std::abs;
Index r = k, p = k;
const Scalar lambda = find_lambda(k, r);
// If lambda=0, no need to interchange
if (lambda > Scalar(0))
{
const Scalar abs_akk = abs(diag_coeff(k));
// If |A[k, k]| >= alpha * lambda, no need to interchange
if (abs_akk < alpha * lambda)
{
const Scalar sigma = find_sigma(k, r, p);
// If sigma * |A[k, k]| >= alpha * lambda^2, no need to interchange
if (sigma * abs_akk < alpha * lambda * lambda)
{
if (abs_akk >= alpha * sigma)
{
// Permutation on A
pivoting_1x1(k, r);
// Permutation on L
interchange_rows(k, r, 0, k - 1);
return true;
}
else
{
// There are two versions of permutation here
// 1. A[k+1, k] <-> A[r, k]
// 2. A[k+1, k] <-> A[r, p], where p >= k and r >= k+1
//
// Version 1 and 2 are used by Ref[1] and Ref[2], respectively
// Version 1 implementation
p = k;
// Version 2 implementation
// [r, p] and [p, r] are symmetric, but we need to make sure
// p >= k and r >= k+1, so it is safe to always make r > p
// One exception is when min{r,p} == k+1, in which case we make
// r = k+1, so that only one permutation needs to be performed
/* const Index rp_min = std::min(r, p);
const Index rp_max = std::max(r, p);
if(rp_min == k + 1)
{
r = rp_min; p = rp_max;
} else {
r = rp_max; p = rp_min;
} */
// Right now we use Version 1 since it reduces the overhead of interchange
// Permutation on A
pivoting_2x2(k, r, p);
// Permutation on L
interchange_rows(k, p, 0, k - 1);
interchange_rows(k + 1, r, 0, k - 1);
return false;
}
}
}
}
return true;
}
// E = [e11, e12]
// [e21, e22]
// Overwrite E with inv(E)
void inverse_inplace_2x2(Scalar& e11, Scalar& e21, Scalar& e22) const
{
// inv(E) = [d11, d12], d11 = e22/delta, d21 = -e21/delta, d22 = e11/delta
// [d21, d22]
const Scalar delta = e11 * e22 - e21 * e21;
std::swap(e11, e22);
e11 /= delta;
e22 /= delta;
e21 = -e21 / delta;
}
// Return value is the status, SUCCESSFUL/NUMERICAL_ISSUE
int gaussian_elimination_1x1(Index k)
{
// D = 1 / A[k, k]
const Scalar akk = diag_coeff(k);
// Return NUMERICAL_ISSUE if not invertible
if (akk == Scalar(0))
return NUMERICAL_ISSUE;
diag_coeff(k) = Scalar(1) / akk;
// B -= l * l' / A[k, k], B := A[(k+1):end, (k+1):end], l := L[(k+1):end, k]
Scalar* lptr = col_pointer(k) + 1;
const Index ldim = m_n - k - 1;
MapVec l(lptr, ldim);
for (Index j = 0; j < ldim; j++)
{
MapVec(col_pointer(j + k + 1), ldim - j).noalias() -= (lptr[j] / akk) * l.tail(ldim - j);
}
// l /= A[k, k]
l /= akk;
return SUCCESSFUL;
}
// Return value is the status, SUCCESSFUL/NUMERICAL_ISSUE
int gaussian_elimination_2x2(Index k)
{
// D = inv(E)
Scalar& e11 = diag_coeff(k);
Scalar& e21 = coeff(k + 1, k);
Scalar& e22 = diag_coeff(k + 1);
// Return NUMERICAL_ISSUE if not invertible
if (e11 * e22 - e21 * e21 == Scalar(0))
return NUMERICAL_ISSUE;
inverse_inplace_2x2(e11, e21, e22);
// X = l * inv(E), l := L[(k+2):end, k:(k+1)]
Scalar* l1ptr = &coeff(k + 2, k);
Scalar* l2ptr = &coeff(k + 2, k + 1);
const Index ldim = m_n - k - 2;
MapVec l1(l1ptr, ldim), l2(l2ptr, ldim);
Eigen::Matrix<Scalar, Eigen::Dynamic, 2> X(ldim, 2);
X.col(0).noalias() = l1 * e11 + l2 * e21;
X.col(1).noalias() = l1 * e21 + l2 * e22;
// B -= l * inv(E) * l' = X * l', B = A[(k+2):end, (k+2):end]
for (Index j = 0; j < ldim; j++)
{
MapVec(col_pointer(j + k + 2), ldim - j).noalias() -= (X.col(0).tail(ldim - j) * l1ptr[j] + X.col(1).tail(ldim - j) * l2ptr[j]);
}
// l = X
l1.noalias() = X.col(0);
l2.noalias() = X.col(1);
return SUCCESSFUL;
}
public:
BKLDLT() :
m_n(0), m_computed(false), m_info(NOT_COMPUTED)
{}
// Factorize mat - shift * I
BKLDLT(ConstGenericMatrix& mat, int uplo = Eigen::Lower, const Scalar& shift = Scalar(0)) :
m_n(mat.rows()), m_computed(false), m_info(NOT_COMPUTED)
{
compute(mat, uplo, shift);
}
void compute(ConstGenericMatrix& mat, int uplo = Eigen::Lower, const Scalar& shift = Scalar(0))
{
using std::abs;
m_n = mat.rows();
if (m_n != mat.cols())
throw std::invalid_argument("BKLDLT: matrix must be square");
m_perm.setLinSpaced(m_n, 0, m_n - 1);
m_permc.clear();
// Copy data
m_data.resize((m_n * (m_n + 1)) / 2);
compute_pointer();
copy_data(mat, uplo, shift);
const Scalar alpha = (1.0 + std::sqrt(17.0)) / 8.0;
Index k = 0;
for (k = 0; k < m_n - 1; k++)
{
// 1. Interchange rows and columns of A, and save the result to m_perm
bool is_1x1 = permutate_mat(k, alpha);
// 2. Gaussian elimination
if (is_1x1)
{
m_info = gaussian_elimination_1x1(k);
}
else
{
m_info = gaussian_elimination_2x2(k);
k++;
}
// 3. Check status
if (m_info != SUCCESSFUL)
break;
}
// Invert the last 1x1 block if it exists
if (k == m_n - 1)
{
const Scalar akk = diag_coeff(k);
if (akk == Scalar(0))
m_info = NUMERICAL_ISSUE;
diag_coeff(k) = Scalar(1) / diag_coeff(k);
}
compress_permutation();
m_computed = true;
}
// Solve Ax=b
void solve_inplace(GenericVector b) const
{
if (!m_computed)
throw std::logic_error("BKLDLT: need to call compute() first");
// PAP' = LDL'
// 1. b -> Pb
Scalar* x = b.data();
MapVec res(x, m_n);
Index npermc = m_permc.size();
for (Index i = 0; i < npermc; i++)
{
std::swap(x[m_permc[i].first], x[m_permc[i].second]);
}
// 2. Lz = Pb
// If m_perm[end] < 0, then end with m_n - 3, otherwise end with m_n - 2
const Index end = (m_perm[m_n - 1] < 0) ? (m_n - 3) : (m_n - 2);
for (Index i = 0; i <= end; i++)
{
const Index b1size = m_n - i - 1;
const Index b2size = b1size - 1;
if (m_perm[i] >= 0)
{
MapConstVec l(&coeff(i + 1, i), b1size);
res.segment(i + 1, b1size).noalias() -= l * x[i];
}
else
{
MapConstVec l1(&coeff(i + 2, i), b2size);
MapConstVec l2(&coeff(i + 2, i + 1), b2size);
res.segment(i + 2, b2size).noalias() -= (l1 * x[i] + l2 * x[i + 1]);
i++;
}
}
// 3. Dw = z
for (Index i = 0; i < m_n; i++)
{
const Scalar e11 = diag_coeff(i);
if (m_perm[i] >= 0)
{
x[i] *= e11;
}
else
{
const Scalar e21 = coeff(i + 1, i), e22 = diag_coeff(i + 1);
const Scalar wi = x[i] * e11 + x[i + 1] * e21;
x[i + 1] = x[i] * e21 + x[i + 1] * e22;
x[i] = wi;
i++;
}
}
// 4. L'y = w
// If m_perm[end] < 0, then start with m_n - 3, otherwise start with m_n - 2
Index i = (m_perm[m_n - 1] < 0) ? (m_n - 3) : (m_n - 2);
for (; i >= 0; i--)
{
const Index ldim = m_n - i - 1;
MapConstVec l(&coeff(i + 1, i), ldim);
x[i] -= res.segment(i + 1, ldim).dot(l);
if (m_perm[i] < 0)
{
MapConstVec l2(&coeff(i + 1, i - 1), ldim);
x[i - 1] -= res.segment(i + 1, ldim).dot(l2);
i--;
}
}
// 5. x = P'y
for (Index i = npermc - 1; i >= 0; i--)
{
std::swap(x[m_permc[i].first], x[m_permc[i].second]);
}
}
Vector solve(ConstGenericVector& b) const
{
Vector res = b;
solve_inplace(res);
return res;
}
int info() const { return m_info; }
};
} // namespace Spectra
#endif // BK_LDLT_H

384
gtsam/3rdparty/Spectra/LinAlg/DoubleShiftQR.h vendored Normal file
View File

@ -0,0 +1,384 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef DOUBLE_SHIFT_QR_H
#define DOUBLE_SHIFT_QR_H
#include <Eigen/Core>
#include <vector> // std::vector
#include <algorithm> // std::min, std::fill, std::copy
#include <cmath> // std::abs, std::sqrt, std::pow
#include <stdexcept> // std::invalid_argument, std::logic_error
#include "../Util/TypeTraits.h"
namespace Spectra {
template <typename Scalar = double>
class DoubleShiftQR
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3X;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Array<unsigned char, Eigen::Dynamic, 1> IntArray;
typedef Eigen::Ref<Matrix> GenericMatrix;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
Index m_n; // Dimension of the matrix
Matrix m_mat_H; // A copy of the matrix to be factorized
Scalar m_shift_s; // Shift constant
Scalar m_shift_t; // Shift constant
Matrix3X m_ref_u; // Householder reflectors
IntArray m_ref_nr; // How many rows does each reflector affects
// 3 - A general reflector
// 2 - A Givens rotation
// 1 - An identity transformation
const Scalar m_near_0; // a very small value, but 1.0 / m_safe_min does not overflow
// ~= 1e-307 for the "double" type
const Scalar m_eps; // the machine precision,
// e.g. ~= 1e-16 for the "double" type
const Scalar m_eps_rel;
const Scalar m_eps_abs;
bool m_computed; // Whether matrix has been factorized
void compute_reflector(const Scalar& x1, const Scalar& x2, const Scalar& x3, Index ind)
{
using std::abs;
Scalar* u = &m_ref_u.coeffRef(0, ind);
unsigned char* nr = m_ref_nr.data();
// In general case the reflector affects 3 rows
nr[ind] = 3;
Scalar x2x3 = Scalar(0);
// If x3 is zero, decrease nr by 1
if (abs(x3) < m_near_0)
{
// If x2 is also zero, nr will be 1, and we can exit this function
if (abs(x2) < m_near_0)
{
nr[ind] = 1;
return;
}
else
{
nr[ind] = 2;
}
x2x3 = abs(x2);
}
else
{
x2x3 = Eigen::numext::hypot(x2, x3);
}
// x1' = x1 - rho * ||x||
// rho = -sign(x1), if x1 == 0, we choose rho = 1
Scalar x1_new = x1 - ((x1 <= 0) - (x1 > 0)) * Eigen::numext::hypot(x1, x2x3);
Scalar x_norm = Eigen::numext::hypot(x1_new, x2x3);
// Double check the norm of new x
if (x_norm < m_near_0)
{
nr[ind] = 1;
return;
}
u[0] = x1_new / x_norm;
u[1] = x2 / x_norm;
u[2] = x3 / x_norm;
}
void compute_reflector(const Scalar* x, Index ind)
{
compute_reflector(x[0], x[1], x[2], ind);
}
// Update the block X = H(il:iu, il:iu)
void update_block(Index il, Index iu)
{
// Block size
const Index bsize = iu - il + 1;
// If block size == 1, there is no need to apply reflectors
if (bsize == 1)
{
m_ref_nr.coeffRef(il) = 1;
return;
}
const Scalar x00 = m_mat_H.coeff(il, il),
x01 = m_mat_H.coeff(il, il + 1),
x10 = m_mat_H.coeff(il + 1, il),
x11 = m_mat_H.coeff(il + 1, il + 1);
// m00 = x00 * (x00 - s) + x01 * x10 + t
const Scalar m00 = x00 * (x00 - m_shift_s) + x01 * x10 + m_shift_t;
// m10 = x10 * (x00 + x11 - s)
const Scalar m10 = x10 * (x00 + x11 - m_shift_s);
// For block size == 2, do a Givens rotation on M = X * X - s * X + t * I
if (bsize == 2)
{
// This causes nr=2
compute_reflector(m00, m10, 0, il);
// Apply the reflector to X
apply_PX(m_mat_H.block(il, il, 2, m_n - il), m_n, il);
apply_XP(m_mat_H.block(0, il, il + 2, 2), m_n, il);
m_ref_nr.coeffRef(il + 1) = 1;
return;
}
// For block size >=3, use the regular strategy
// m20 = x21 * x10
const Scalar m20 = m_mat_H.coeff(il + 2, il + 1) * m_mat_H.coeff(il + 1, il);
compute_reflector(m00, m10, m20, il);
// Apply the first reflector
apply_PX(m_mat_H.block(il, il, 3, m_n - il), m_n, il);
apply_XP(m_mat_H.block(0, il, il + std::min(bsize, Index(4)), 3), m_n, il);
// Calculate the following reflectors
// If entering this loop, block size is at least 4.
for (Index i = 1; i < bsize - 2; i++)
{
compute_reflector(&m_mat_H.coeffRef(il + i, il + i - 1), il + i);
// Apply the reflector to X
apply_PX(m_mat_H.block(il + i, il + i - 1, 3, m_n - il - i + 1), m_n, il + i);
apply_XP(m_mat_H.block(0, il + i, il + std::min(bsize, Index(i + 4)), 3), m_n, il + i);
}
// The last reflector
// This causes nr=2
compute_reflector(m_mat_H.coeff(iu - 1, iu - 2), m_mat_H.coeff(iu, iu - 2), 0, iu - 1);
// Apply the reflector to X
apply_PX(m_mat_H.block(iu - 1, iu - 2, 2, m_n - iu + 2), m_n, iu - 1);
apply_XP(m_mat_H.block(0, iu - 1, il + bsize, 2), m_n, iu - 1);
m_ref_nr.coeffRef(iu) = 1;
}
// P = I - 2 * u * u' = P'
// PX = X - 2 * u * (u'X)
void apply_PX(GenericMatrix X, Index stride, Index u_ind) const
{
const Index nr = m_ref_nr.coeff(u_ind);
if (nr == 1)
return;
const Scalar u0 = m_ref_u.coeff(0, u_ind),
u1 = m_ref_u.coeff(1, u_ind);
const Scalar u0_2 = Scalar(2) * u0,
u1_2 = Scalar(2) * u1;
const Index nrow = X.rows();
const Index ncol = X.cols();
Scalar* xptr = X.data();
if (nr == 2 || nrow == 2)
{
for (Index i = 0; i < ncol; i++, xptr += stride)
{
const Scalar tmp = u0_2 * xptr[0] + u1_2 * xptr[1];
xptr[0] -= tmp * u0;
xptr[1] -= tmp * u1;
}
}
else
{
const Scalar u2 = m_ref_u.coeff(2, u_ind);
const Scalar u2_2 = Scalar(2) * u2;
for (Index i = 0; i < ncol; i++, xptr += stride)
{
const Scalar tmp = u0_2 * xptr[0] + u1_2 * xptr[1] + u2_2 * xptr[2];
xptr[0] -= tmp * u0;
xptr[1] -= tmp * u1;
xptr[2] -= tmp * u2;
}
}
}
// x is a pointer to a vector
// Px = x - 2 * dot(x, u) * u
void apply_PX(Scalar* x, Index u_ind) const
{
const Index nr = m_ref_nr.coeff(u_ind);
if (nr == 1)
return;
const Scalar u0 = m_ref_u.coeff(0, u_ind),
u1 = m_ref_u.coeff(1, u_ind),
u2 = m_ref_u.coeff(2, u_ind);
// When the reflector only contains two elements, u2 has been set to zero
const bool nr_is_2 = (nr == 2);
const Scalar dot2 = Scalar(2) * (x[0] * u0 + x[1] * u1 + (nr_is_2 ? 0 : (x[2] * u2)));
x[0] -= dot2 * u0;
x[1] -= dot2 * u1;
if (!nr_is_2)
x[2] -= dot2 * u2;
}
// XP = X - 2 * (X * u) * u'
void apply_XP(GenericMatrix X, Index stride, Index u_ind) const
{
const Index nr = m_ref_nr.coeff(u_ind);
if (nr == 1)
return;
const Scalar u0 = m_ref_u.coeff(0, u_ind),
u1 = m_ref_u.coeff(1, u_ind);
const Scalar u0_2 = Scalar(2) * u0,
u1_2 = Scalar(2) * u1;
const int nrow = X.rows();
const int ncol = X.cols();
Scalar *X0 = X.data(), *X1 = X0 + stride; // X0 => X.col(0), X1 => X.col(1)
if (nr == 2 || ncol == 2)
{
// tmp = 2 * u0 * X0 + 2 * u1 * X1
// X0 => X0 - u0 * tmp
// X1 => X1 - u1 * tmp
for (Index i = 0; i < nrow; i++)
{
const Scalar tmp = u0_2 * X0[i] + u1_2 * X1[i];
X0[i] -= tmp * u0;
X1[i] -= tmp * u1;
}
}
else
{
Scalar* X2 = X1 + stride; // X2 => X.col(2)
const Scalar u2 = m_ref_u.coeff(2, u_ind);
const Scalar u2_2 = Scalar(2) * u2;
for (Index i = 0; i < nrow; i++)
{
const Scalar tmp = u0_2 * X0[i] + u1_2 * X1[i] + u2_2 * X2[i];
X0[i] -= tmp * u0;
X1[i] -= tmp * u1;
X2[i] -= tmp * u2;
}
}
}
public:
DoubleShiftQR(Index size) :
m_n(size),
m_near_0(TypeTraits<Scalar>::min() * Scalar(10)),
m_eps(Eigen::NumTraits<Scalar>::epsilon()),
m_eps_rel(m_eps),
m_eps_abs(m_near_0 * (m_n / m_eps)),
m_computed(false)
{}
DoubleShiftQR(ConstGenericMatrix& mat, const Scalar& s, const Scalar& t) :
m_n(mat.rows()),
m_mat_H(m_n, m_n),
m_shift_s(s),
m_shift_t(t),
m_ref_u(3, m_n),
m_ref_nr(m_n),
m_near_0(TypeTraits<Scalar>::min() * Scalar(10)),
m_eps(Eigen::NumTraits<Scalar>::epsilon()),
m_eps_rel(m_eps),
m_eps_abs(m_near_0 * (m_n / m_eps)),
m_computed(false)
{
compute(mat, s, t);
}
void compute(ConstGenericMatrix& mat, const Scalar& s, const Scalar& t)
{
using std::abs;
m_n = mat.rows();
if (m_n != mat.cols())
throw std::invalid_argument("DoubleShiftQR: matrix must be square");
m_mat_H.resize(m_n, m_n);
m_shift_s = s;
m_shift_t = t;
m_ref_u.resize(3, m_n);
m_ref_nr.resize(m_n);
// Make a copy of mat
std::copy(mat.data(), mat.data() + mat.size(), m_mat_H.data());
// Obtain the indices of zero elements in the subdiagonal,
// so that H can be divided into several blocks
std::vector<int> zero_ind;
zero_ind.reserve(m_n - 1);
zero_ind.push_back(0);
Scalar* Hii = m_mat_H.data();
for (Index i = 0; i < m_n - 2; i++, Hii += (m_n + 1))
{
// Hii[1] => m_mat_H(i + 1, i)
const Scalar h = abs(Hii[1]);
if (h <= 0 || h <= m_eps_rel * (abs(Hii[0]) + abs(Hii[m_n + 1])))
{
Hii[1] = 0;
zero_ind.push_back(i + 1);
}
// Make sure m_mat_H is upper Hessenberg
// Zero the elements below m_mat_H(i + 1, i)
std::fill(Hii + 2, Hii + m_n - i, Scalar(0));
}
zero_ind.push_back(m_n);
for (std::vector<int>::size_type i = 0; i < zero_ind.size() - 1; i++)
{
const Index start = zero_ind[i];
const Index end = zero_ind[i + 1] - 1;
// Compute refelctors and update each block
update_block(start, end);
}
m_computed = true;
}
void matrix_QtHQ(Matrix& dest) const
{
if (!m_computed)
throw std::logic_error("DoubleShiftQR: need to call compute() first");
dest.noalias() = m_mat_H;
}
// Q = P0 * P1 * ...
// Q'y = P_{n-2} * ... * P1 * P0 * y
void apply_QtY(Vector& y) const
{
if (!m_computed)
throw std::logic_error("DoubleShiftQR: need to call compute() first");
Scalar* y_ptr = y.data();
const Index n1 = m_n - 1;
for (Index i = 0; i < n1; i++, y_ptr++)
{
apply_PX(y_ptr, i);
}
}
// Q = P0 * P1 * ...
// YQ = Y * P0 * P1 * ...
void apply_YQ(GenericMatrix Y) const
{
if (!m_computed)
throw std::logic_error("DoubleShiftQR: need to call compute() first");
const Index nrow = Y.rows();
const Index n2 = m_n - 2;
for (Index i = 0; i < n2; i++)
{
apply_XP(Y.block(0, i, nrow, 3), nrow, i);
}
apply_XP(Y.block(0, n2, nrow, 2), nrow, n2);
}
};
} // namespace Spectra
#endif // DOUBLE_SHIFT_QR_H

167
gtsam/3rdparty/Spectra/LinAlg/Lanczos.h vendored Normal file
View File

@ -0,0 +1,167 @@
// Copyright (C) 2018-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef LANCZOS_H
#define LANCZOS_H
#include <Eigen/Core>
#include <cmath> // std::sqrt
#include <stdexcept> // std::invalid_argument
#include <sstream> // std::stringstream
#include "Arnoldi.h"
namespace Spectra {
// Lanczos factorization A * V = V * H + f * e'
// A: n x n
// V: n x k
// H: k x k
// f: n x 1
// e: [0, ..., 0, 1]
// V and H are allocated of dimension m, so the maximum value of k is m
template <typename Scalar, typename ArnoldiOpType>
class Lanczos : public Arnoldi<Scalar, ArnoldiOpType>
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<Matrix> MapMat;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::Map<const Matrix> MapConstMat;
typedef Eigen::Map<const Vector> MapConstVec;
using Arnoldi<Scalar, ArnoldiOpType>::m_op;
using Arnoldi<Scalar, ArnoldiOpType>::m_n;
using Arnoldi<Scalar, ArnoldiOpType>::m_m;
using Arnoldi<Scalar, ArnoldiOpType>::m_k;
using Arnoldi<Scalar, ArnoldiOpType>::m_fac_V;
using Arnoldi<Scalar, ArnoldiOpType>::m_fac_H;
using Arnoldi<Scalar, ArnoldiOpType>::m_fac_f;
using Arnoldi<Scalar, ArnoldiOpType>::m_beta;
using Arnoldi<Scalar, ArnoldiOpType>::m_near_0;
using Arnoldi<Scalar, ArnoldiOpType>::m_eps;
public:
Lanczos(const ArnoldiOpType& op, Index m) :
Arnoldi<Scalar, ArnoldiOpType>(op, m)
{}
// Lanczos factorization starting from step-k
void factorize_from(Index from_k, Index to_m, Index& op_counter)
{
using std::sqrt;
if (to_m <= from_k)
return;
if (from_k > m_k)
{
std::stringstream msg;
msg << "Lanczos: from_k (= " << from_k << ") is larger than the current subspace dimension (= " << m_k << ")";
throw std::invalid_argument(msg.str());
}
const Scalar beta_thresh = m_eps * sqrt(Scalar(m_n));
// Pre-allocate vectors
Vector Vf(to_m);
Vector w(m_n);
// Keep the upperleft k x k submatrix of H and set other elements to 0
m_fac_H.rightCols(m_m - from_k).setZero();
m_fac_H.block(from_k, 0, m_m - from_k, from_k).setZero();
for (Index i = from_k; i <= to_m - 1; i++)
{
bool restart = false;
// If beta = 0, then the next V is not full rank
// We need to generate a new residual vector that is orthogonal
// to the current V, which we call a restart
if (m_beta < m_near_0)
{
MapConstMat V(m_fac_V.data(), m_n, i); // The first i columns
this->expand_basis(V, 2 * i, m_fac_f, m_beta);
restart = true;
}
// v <- f / ||f||
MapVec v(&m_fac_V(0, i), m_n); // The (i+1)-th column
v.noalias() = m_fac_f / m_beta;
// Note that H[i+1, i] equals to the unrestarted beta
m_fac_H(i, i - 1) = restart ? Scalar(0) : m_beta;
// w <- A * v
m_op.perform_op(v.data(), w.data());
op_counter++;
// H[i+1, i+1] = <v, w> = v'Bw
m_fac_H(i - 1, i) = m_fac_H(i, i - 1); // Due to symmetry
m_fac_H(i, i) = m_op.inner_product(v, w);
// f <- w - V * V'Bw = w - H[i+1, i] * V{i} - H[i+1, i+1] * V{i+1}
// If restarting, we know that H[i+1, i] = 0
if (restart)
m_fac_f.noalias() = w - m_fac_H(i, i) * v;
else
m_fac_f.noalias() = w - m_fac_H(i, i - 1) * m_fac_V.col(i - 1) - m_fac_H(i, i) * v;
m_beta = m_op.norm(m_fac_f);
// f/||f|| is going to be the next column of V, so we need to test
// whether V'B(f/||f||) ~= 0
const Index i1 = i + 1;
MapMat Vs(m_fac_V.data(), m_n, i1); // The first (i+1) columns
m_op.trans_product(Vs, m_fac_f, Vf.head(i1));
Scalar ortho_err = Vf.head(i1).cwiseAbs().maxCoeff();
// If not, iteratively correct the residual
int count = 0;
while (count < 5 && ortho_err > m_eps * m_beta)
{
// There is an edge case: when beta=||f|| is close to zero, f mostly consists
// of noises of rounding errors, so the test [ortho_err < eps * beta] is very
// likely to fail. In particular, if beta=0, then the test is ensured to fail.
// Hence when this happens, we force f to be zero, and then restart in the
// next iteration.
if (m_beta < beta_thresh)
{
m_fac_f.setZero();
m_beta = Scalar(0);
break;
}
// f <- f - V * Vf
m_fac_f.noalias() -= Vs * Vf.head(i1);
// h <- h + Vf
m_fac_H(i - 1, i) += Vf[i - 1];
m_fac_H(i, i - 1) = m_fac_H(i - 1, i);
m_fac_H(i, i) += Vf[i];
// beta <- ||f||
m_beta = m_op.norm(m_fac_f);
m_op.trans_product(Vs, m_fac_f, Vf.head(i1));
ortho_err = Vf.head(i1).cwiseAbs().maxCoeff();
count++;
}
}
// Indicate that this is a step-m factorization
m_k = to_m;
}
// Apply H -> Q'HQ, where Q is from a tridiagonal QR decomposition
void compress_H(const TridiagQR<Scalar>& decomp)
{
decomp.matrix_QtHQ(m_fac_H);
m_k--;
}
};
} // namespace Spectra
#endif // LANCZOS_H

219
gtsam/3rdparty/Spectra/LinAlg/TridiagEigen.h vendored Normal file
View File

@ -0,0 +1,219 @@
// The code was adapted from Eigen/src/Eigenvaleus/SelfAdjointEigenSolver.h
//
// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef TRIDIAG_EIGEN_H
#define TRIDIAG_EIGEN_H
#include <Eigen/Core>
#include <Eigen/Jacobi>
#include <stdexcept>
#include "../Util/TypeTraits.h"
namespace Spectra {
template <typename Scalar = double>
class TridiagEigen
{
private:
typedef Eigen::Index Index;
// For convenience in adapting the tridiagonal_qr_step() function
typedef Scalar RealScalar;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Ref<Matrix> GenericMatrix;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
Index m_n;
Vector m_main_diag; // Main diagonal elements of the matrix
Vector m_sub_diag; // Sub-diagonal elements of the matrix
Matrix m_evecs; // To store eigenvectors
bool m_computed;
const Scalar m_near_0; // a very small value, ~= 1e-307 for the "double" type
// Adapted from Eigen/src/Eigenvaleus/SelfAdjointEigenSolver.h
static void tridiagonal_qr_step(RealScalar* diag,
RealScalar* subdiag, Index start,
Index end, Scalar* matrixQ,
Index n)
{
using std::abs;
RealScalar td = (diag[end - 1] - diag[end]) * RealScalar(0.5);
RealScalar e = subdiag[end - 1];
// Note that thanks to scaling, e^2 or td^2 cannot overflow, however they can still
// underflow thus leading to inf/NaN values when using the following commented code:
// RealScalar e2 = numext::abs2(subdiag[end-1]);
// RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2));
// This explain the following, somewhat more complicated, version:
RealScalar mu = diag[end];
if (td == Scalar(0))
mu -= abs(e);
else
{
RealScalar e2 = Eigen::numext::abs2(subdiag[end - 1]);
RealScalar h = Eigen::numext::hypot(td, e);
if (e2 == RealScalar(0))
mu -= (e / (td + (td > RealScalar(0) ? RealScalar(1) : RealScalar(-1)))) * (e / h);
else
mu -= e2 / (td + (td > RealScalar(0) ? h : -h));
}
RealScalar x = diag[start] - mu;
RealScalar z = subdiag[start];
Eigen::Map<Matrix> q(matrixQ, n, n);
for (Index k = start; k < end; ++k)
{
Eigen::JacobiRotation<RealScalar> rot;
rot.makeGivens(x, z);
const RealScalar s = rot.s();
const RealScalar c = rot.c();
// do T = G' T G
RealScalar sdk = s * diag[k] + c * subdiag[k];
RealScalar dkp1 = s * subdiag[k] + c * diag[k + 1];
diag[k] = c * (c * diag[k] - s * subdiag[k]) - s * (c * subdiag[k] - s * diag[k + 1]);
diag[k + 1] = s * sdk + c * dkp1;
subdiag[k] = c * sdk - s * dkp1;
if (k > start)
subdiag[k - 1] = c * subdiag[k - 1] - s * z;
x = subdiag[k];
if (k < end - 1)
{
z = -s * subdiag[k + 1];
subdiag[k + 1] = c * subdiag[k + 1];
}
// apply the givens rotation to the unit matrix Q = Q * G
if (matrixQ)
q.applyOnTheRight(k, k + 1, rot);
}
}
public:
TridiagEigen() :
m_n(0), m_computed(false),
m_near_0(TypeTraits<Scalar>::min() * Scalar(10))
{}
TridiagEigen(ConstGenericMatrix& mat) :
m_n(mat.rows()), m_computed(false),
m_near_0(TypeTraits<Scalar>::min() * Scalar(10))
{
compute(mat);
}
void compute(ConstGenericMatrix& mat)
{
using std::abs;
m_n = mat.rows();
if (m_n != mat.cols())
throw std::invalid_argument("TridiagEigen: matrix must be square");
m_main_diag.resize(m_n);
m_sub_diag.resize(m_n - 1);
m_evecs.resize(m_n, m_n);
m_evecs.setIdentity();
// Scale matrix to improve stability
const Scalar scale = std::max(mat.diagonal().cwiseAbs().maxCoeff(),
mat.diagonal(-1).cwiseAbs().maxCoeff());
// If scale=0, mat is a zero matrix, so we can early stop
if (scale < m_near_0)
{
// m_main_diag contains eigenvalues
m_main_diag.setZero();
// m_evecs has been set identity
// m_evecs.setIdentity();
m_computed = true;
return;
}
m_main_diag.noalias() = mat.diagonal() / scale;
m_sub_diag.noalias() = mat.diagonal(-1) / scale;
Scalar* diag = m_main_diag.data();
Scalar* subdiag = m_sub_diag.data();
Index end = m_n - 1;
Index start = 0;
Index iter = 0; // total number of iterations
int info = 0; // 0 for success, 1 for failure
const Scalar considerAsZero = TypeTraits<Scalar>::min();
const Scalar precision = Scalar(2) * Eigen::NumTraits<Scalar>::epsilon();
while (end > 0)
{
for (Index i = start; i < end; i++)
if (abs(subdiag[i]) <= considerAsZero ||
abs(subdiag[i]) <= (abs(diag[i]) + abs(diag[i + 1])) * precision)
subdiag[i] = 0;
// find the largest unreduced block
while (end > 0 && subdiag[end - 1] == Scalar(0))
end--;
if (end <= 0)
break;
// if we spent too many iterations, we give up
iter++;
if (iter > 30 * m_n)
{
info = 1;
break;
}
start = end - 1;
while (start > 0 && subdiag[start - 1] != Scalar(0))
start--;
tridiagonal_qr_step(diag, subdiag, start, end, m_evecs.data(), m_n);
}
if (info > 0)
throw std::runtime_error("TridiagEigen: eigen decomposition failed");
// Scale eigenvalues back
m_main_diag *= scale;
m_computed = true;
}
const Vector& eigenvalues() const
{
if (!m_computed)
throw std::logic_error("TridiagEigen: need to call compute() first");
// After calling compute(), main_diag will contain the eigenvalues.
return m_main_diag;
}
const Matrix& eigenvectors() const
{
if (!m_computed)
throw std::logic_error("TridiagEigen: need to call compute() first");
return m_evecs;
}
};
} // namespace Spectra
#endif // TRIDIAG_EIGEN_H

319
gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergEigen.h vendored Normal file
View File

@ -0,0 +1,319 @@
// The code was adapted from Eigen/src/Eigenvaleus/EigenSolver.h
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2010,2012 Jitse Niesen <jitse@maths.leeds.ac.uk>
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef UPPER_HESSENBERG_EIGEN_H
#define UPPER_HESSENBERG_EIGEN_H
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <stdexcept>
namespace Spectra {
template <typename Scalar = double>
class UpperHessenbergEigen
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Ref<Matrix> GenericMatrix;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
typedef std::complex<Scalar> Complex;
typedef Eigen::Matrix<Complex, Eigen::Dynamic, Eigen::Dynamic> ComplexMatrix;
typedef Eigen::Matrix<Complex, Eigen::Dynamic, 1> ComplexVector;
Index m_n; // Size of the matrix
Eigen::RealSchur<Matrix> m_realSchur; // Schur decomposition solver
Matrix m_matT; // Schur T matrix
Matrix m_eivec; // Storing eigenvectors
ComplexVector m_eivalues; // Eigenvalues
bool m_computed;
void doComputeEigenvectors()
{
using std::abs;
const Index size = m_eivec.cols();
const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
// inefficient! this is already computed in RealSchur
Scalar norm(0);
for (Index j = 0; j < size; ++j)
{
norm += m_matT.row(j).segment((std::max)(j - 1, Index(0)), size - (std::max)(j - 1, Index(0))).cwiseAbs().sum();
}
// Backsubstitute to find vectors of upper triangular form
if (norm == Scalar(0))
return;
for (Index n = size - 1; n >= 0; n--)
{
Scalar p = m_eivalues.coeff(n).real();
Scalar q = m_eivalues.coeff(n).imag();
// Scalar vector
if (q == Scalar(0))
{
Scalar lastr(0), lastw(0);
Index l = n;
m_matT.coeffRef(n, n) = Scalar(1);
for (Index i = n - 1; i >= 0; i--)
{
Scalar w = m_matT.coeff(i, i) - p;
Scalar r = m_matT.row(i).segment(l, n - l + 1).dot(m_matT.col(n).segment(l, n - l + 1));
if (m_eivalues.coeff(i).imag() < Scalar(0))
{
lastw = w;
lastr = r;
}
else
{
l = i;
if (m_eivalues.coeff(i).imag() == Scalar(0))
{
if (w != Scalar(0))
m_matT.coeffRef(i, n) = -r / w;
else
m_matT.coeffRef(i, n) = -r / (eps * norm);
}
else // Solve real equations
{
Scalar x = m_matT.coeff(i, i + 1);
Scalar y = m_matT.coeff(i + 1, i);
Scalar denom = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag();
Scalar t = (x * lastr - lastw * r) / denom;
m_matT.coeffRef(i, n) = t;
if (abs(x) > abs(lastw))
m_matT.coeffRef(i + 1, n) = (-r - w * t) / x;
else
m_matT.coeffRef(i + 1, n) = (-lastr - y * t) / lastw;
}
// Overflow control
Scalar t = abs(m_matT.coeff(i, n));
if ((eps * t) * t > Scalar(1))
m_matT.col(n).tail(size - i) /= t;
}
}
}
else if (q < Scalar(0) && n > 0)
{ // Complex vector
Scalar lastra(0), lastsa(0), lastw(0);
Index l = n - 1;
// Last vector component imaginary so matrix is triangular
if (abs(m_matT.coeff(n, n - 1)) > abs(m_matT.coeff(n - 1, n)))
{
m_matT.coeffRef(n - 1, n - 1) = q / m_matT.coeff(n, n - 1);
m_matT.coeffRef(n - 1, n) = -(m_matT.coeff(n, n) - p) / m_matT.coeff(n, n - 1);
}
else
{
Complex cc = Complex(Scalar(0), -m_matT.coeff(n - 1, n)) / Complex(m_matT.coeff(n - 1, n - 1) - p, q);
m_matT.coeffRef(n - 1, n - 1) = Eigen::numext::real(cc);
m_matT.coeffRef(n - 1, n) = Eigen::numext::imag(cc);
}
m_matT.coeffRef(n, n - 1) = Scalar(0);
m_matT.coeffRef(n, n) = Scalar(1);
for (Index i = n - 2; i >= 0; i--)
{
Scalar ra = m_matT.row(i).segment(l, n - l + 1).dot(m_matT.col(n - 1).segment(l, n - l + 1));
Scalar sa = m_matT.row(i).segment(l, n - l + 1).dot(m_matT.col(n).segment(l, n - l + 1));
Scalar w = m_matT.coeff(i, i) - p;
if (m_eivalues.coeff(i).imag() < Scalar(0))
{
lastw = w;
lastra = ra;
lastsa = sa;
}
else
{
l = i;
if (m_eivalues.coeff(i).imag() == Scalar(0))
{
Complex cc = Complex(-ra, -sa) / Complex(w, q);
m_matT.coeffRef(i, n - 1) = Eigen::numext::real(cc);
m_matT.coeffRef(i, n) = Eigen::numext::imag(cc);
}
else
{
// Solve complex equations
Scalar x = m_matT.coeff(i, i + 1);
Scalar y = m_matT.coeff(i + 1, i);
Scalar vr = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag() - q * q;
Scalar vi = (m_eivalues.coeff(i).real() - p) * Scalar(2) * q;
if ((vr == Scalar(0)) && (vi == Scalar(0)))
vr = eps * norm * (abs(w) + abs(q) + abs(x) + abs(y) + abs(lastw));
Complex cc = Complex(x * lastra - lastw * ra + q * sa, x * lastsa - lastw * sa - q * ra) / Complex(vr, vi);
m_matT.coeffRef(i, n - 1) = Eigen::numext::real(cc);
m_matT.coeffRef(i, n) = Eigen::numext::imag(cc);
if (abs(x) > (abs(lastw) + abs(q)))
{
m_matT.coeffRef(i + 1, n - 1) = (-ra - w * m_matT.coeff(i, n - 1) + q * m_matT.coeff(i, n)) / x;
m_matT.coeffRef(i + 1, n) = (-sa - w * m_matT.coeff(i, n) - q * m_matT.coeff(i, n - 1)) / x;
}
else
{
cc = Complex(-lastra - y * m_matT.coeff(i, n - 1), -lastsa - y * m_matT.coeff(i, n)) / Complex(lastw, q);
m_matT.coeffRef(i + 1, n - 1) = Eigen::numext::real(cc);
m_matT.coeffRef(i + 1, n) = Eigen::numext::imag(cc);
}
}
// Overflow control
Scalar t = std::max(abs(m_matT.coeff(i, n - 1)), abs(m_matT.coeff(i, n)));
if ((eps * t) * t > Scalar(1))
m_matT.block(i, n - 1, size - i, 2) /= t;
}
}
// We handled a pair of complex conjugate eigenvalues, so need to skip them both
n--;
}
}
// Back transformation to get eigenvectors of original matrix
Vector m_tmp(size);
for (Index j = size - 1; j >= 0; j--)
{
m_tmp.noalias() = m_eivec.leftCols(j + 1) * m_matT.col(j).segment(0, j + 1);
m_eivec.col(j) = m_tmp;
}
}
public:
UpperHessenbergEigen() :
m_n(0), m_computed(false)
{}
UpperHessenbergEigen(ConstGenericMatrix& mat) :
m_n(mat.rows()), m_computed(false)
{
compute(mat);
}
void compute(ConstGenericMatrix& mat)
{
using std::abs;
using std::sqrt;
if (mat.rows() != mat.cols())
throw std::invalid_argument("UpperHessenbergEigen: matrix must be square");
m_n = mat.rows();
// Scale matrix prior to the Schur decomposition
const Scalar scale = mat.cwiseAbs().maxCoeff();
// Reduce to real Schur form
Matrix Q = Matrix::Identity(m_n, m_n);
m_realSchur.computeFromHessenberg(mat / scale, Q, true);
if (m_realSchur.info() != Eigen::Success)
throw std::runtime_error("UpperHessenbergEigen: eigen decomposition failed");
m_matT = m_realSchur.matrixT();
m_eivec = m_realSchur.matrixU();
// Compute eigenvalues from matT
m_eivalues.resize(m_n);
Index i = 0;
while (i < m_n)
{
// Real eigenvalue
if (i == m_n - 1 || m_matT.coeff(i + 1, i) == Scalar(0))
{
m_eivalues.coeffRef(i) = m_matT.coeff(i, i);
++i;
}
else // Complex eigenvalues
{
Scalar p = Scalar(0.5) * (m_matT.coeff(i, i) - m_matT.coeff(i + 1, i + 1));
Scalar z;
// Compute z = sqrt(abs(p * p + m_matT.coeff(i+1, i) * m_matT.coeff(i, i+1)));
// without overflow
{
Scalar t0 = m_matT.coeff(i + 1, i);
Scalar t1 = m_matT.coeff(i, i + 1);
Scalar maxval = std::max(abs(p), std::max(abs(t0), abs(t1)));
t0 /= maxval;
t1 /= maxval;
Scalar p0 = p / maxval;
z = maxval * sqrt(abs(p0 * p0 + t0 * t1));
}
m_eivalues.coeffRef(i) = Complex(m_matT.coeff(i + 1, i + 1) + p, z);
m_eivalues.coeffRef(i + 1) = Complex(m_matT.coeff(i + 1, i + 1) + p, -z);
i += 2;
}
}
// Compute eigenvectors
doComputeEigenvectors();
// Scale eigenvalues back
m_eivalues *= scale;
m_computed = true;
}
const ComplexVector& eigenvalues() const
{
if (!m_computed)
throw std::logic_error("UpperHessenbergEigen: need to call compute() first");
return m_eivalues;
}
ComplexMatrix eigenvectors()
{
using std::abs;
if (!m_computed)
throw std::logic_error("UpperHessenbergEigen: need to call compute() first");
Index n = m_eivec.cols();
ComplexMatrix matV(n, n);
for (Index j = 0; j < n; ++j)
{
// imaginary part of real eigenvalue is already set to exact zero
if (Eigen::numext::imag(m_eivalues.coeff(j)) == Scalar(0) || j + 1 == n)
{
// we have a real eigen value
matV.col(j) = m_eivec.col(j).template cast<Complex>();
matV.col(j).normalize();
}
else
{
// we have a pair of complex eigen values
for (Index i = 0; i < n; ++i)
{
matV.coeffRef(i, j) = Complex(m_eivec.coeff(i, j), m_eivec.coeff(i, j + 1));
matV.coeffRef(i, j + 1) = Complex(m_eivec.coeff(i, j), -m_eivec.coeff(i, j + 1));
}
matV.col(j).normalize();
matV.col(j + 1).normalize();
++j;
}
}
return matV;
}
};
} // namespace Spectra
#endif // UPPER_HESSENBERG_EIGEN_H

670
gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergQR.h vendored Normal file
View File

@ -0,0 +1,670 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef UPPER_HESSENBERG_QR_H
#define UPPER_HESSENBERG_QR_H
#include <Eigen/Core>
#include <cmath> // std::sqrt
#include <algorithm> // std::fill, std::copy
#include <stdexcept> // std::logic_error
namespace Spectra {
///
/// \defgroup Internals Internal Classes
///
/// Classes for internal use. May be useful to developers.
///
///
/// \ingroup Internals
/// @{
///
///
/// \defgroup LinearAlgebra Linear Algebra
///
/// A number of classes for linear algebra operations.
///
///
/// \ingroup LinearAlgebra
///
/// Perform the QR decomposition of an upper Hessenberg matrix.
///
/// \tparam Scalar The element type of the matrix.
/// Currently supported types are `float`, `double` and `long double`.
///
template <typename Scalar = double>
class UpperHessenbergQR
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Matrix<Scalar, 1, Eigen::Dynamic> RowVector;
typedef Eigen::Array<Scalar, Eigen::Dynamic, 1> Array;
typedef Eigen::Ref<Matrix> GenericMatrix;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
Matrix m_mat_T;
protected:
Index m_n;
// Gi = [ cos[i] sin[i]]
// [-sin[i] cos[i]]
// Q = G1 * G2 * ... * G_{n-1}
Scalar m_shift;
Array m_rot_cos;
Array m_rot_sin;
bool m_computed;
// Given x and y, compute 1) r = sqrt(x^2 + y^2), 2) c = x / r, 3) s = -y / r
// If both x and y are zero, set c = 1 and s = 0
// We must implement it in a numerically stable way
static void compute_rotation(const Scalar& x, const Scalar& y, Scalar& r, Scalar& c, Scalar& s)
{
using std::sqrt;
const Scalar xsign = (x > Scalar(0)) - (x < Scalar(0));
const Scalar ysign = (y > Scalar(0)) - (y < Scalar(0));
const Scalar xabs = x * xsign;
const Scalar yabs = y * ysign;
if (xabs > yabs)
{
// In this case xabs != 0
const Scalar ratio = yabs / xabs; // so that 0 <= ratio < 1
const Scalar common = sqrt(Scalar(1) + ratio * ratio);
c = xsign / common;
r = xabs * common;
s = -y / r;
}
else
{
if (yabs == Scalar(0))
{
r = Scalar(0);
c = Scalar(1);
s = Scalar(0);
return;
}
const Scalar ratio = xabs / yabs; // so that 0 <= ratio <= 1
const Scalar common = sqrt(Scalar(1) + ratio * ratio);
s = -ysign / common;
r = yabs * common;
c = x / r;
}
}
public:
///
/// Constructor to preallocate memory. Computation can
/// be performed later by calling the compute() method.
///
UpperHessenbergQR(Index size) :
m_n(size),
m_rot_cos(m_n - 1),
m_rot_sin(m_n - 1),
m_computed(false)
{}
///
/// Constructor to create an object that performs and stores the
/// QR decomposition of an upper Hessenberg matrix `mat`, with an
/// optional shift: \f$H-sI=QR\f$. Here \f$H\f$ stands for the matrix
/// `mat`, and \f$s\f$ is the shift.
///
/// \param mat Matrix type can be `Eigen::Matrix<Scalar, ...>` (e.g.
/// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
/// Only the upper triangular and the lower subdiagonal parts of
/// the matrix are used.
///
UpperHessenbergQR(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0)) :
m_n(mat.rows()),
m_shift(shift),
m_rot_cos(m_n - 1),
m_rot_sin(m_n - 1),
m_computed(false)
{
compute(mat, shift);
}
///
/// Virtual destructor.
///
virtual ~UpperHessenbergQR(){};
///
/// Conduct the QR factorization of an upper Hessenberg matrix with
/// an optional shift.
///
/// \param mat Matrix type can be `Eigen::Matrix<Scalar, ...>` (e.g.
/// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
/// Only the upper triangular and the lower subdiagonal parts of
/// the matrix are used.
///
virtual void compute(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0))
{
m_n = mat.rows();
if (m_n != mat.cols())
throw std::invalid_argument("UpperHessenbergQR: matrix must be square");
m_shift = shift;
m_mat_T.resize(m_n, m_n);
m_rot_cos.resize(m_n - 1);
m_rot_sin.resize(m_n - 1);
// Make a copy of mat - s * I
std::copy(mat.data(), mat.data() + mat.size(), m_mat_T.data());
m_mat_T.diagonal().array() -= m_shift;
Scalar xi, xj, r, c, s;
Scalar *Tii, *ptr;
const Index n1 = m_n - 1;
for (Index i = 0; i < n1; i++)
{
Tii = &m_mat_T.coeffRef(i, i);
// Make sure mat_T is upper Hessenberg
// Zero the elements below mat_T(i + 1, i)
std::fill(Tii + 2, Tii + m_n - i, Scalar(0));
xi = Tii[0]; // mat_T(i, i)
xj = Tii[1]; // mat_T(i + 1, i)
compute_rotation(xi, xj, r, c, s);
m_rot_cos[i] = c;
m_rot_sin[i] = s;
// For a complete QR decomposition,
// we first obtain the rotation matrix
// G = [ cos sin]
// [-sin cos]
// and then do T[i:(i + 1), i:(n - 1)] = G' * T[i:(i + 1), i:(n - 1)]
// Gt << c, -s, s, c;
// m_mat_T.block(i, i, 2, m_n - i) = Gt * m_mat_T.block(i, i, 2, m_n - i);
Tii[0] = r; // m_mat_T(i, i) => r
Tii[1] = 0; // m_mat_T(i + 1, i) => 0
ptr = Tii + m_n; // m_mat_T(i, k), k = i+1, i+2, ..., n-1
for (Index j = i + 1; j < m_n; j++, ptr += m_n)
{
Scalar tmp = ptr[0];
ptr[0] = c * tmp - s * ptr[1];
ptr[1] = s * tmp + c * ptr[1];
}
// If we do not need to calculate the R matrix, then
// only the cos and sin sequences are required.
// In such case we only update T[i + 1, (i + 1):(n - 1)]
// m_mat_T.block(i + 1, i + 1, 1, m_n - i - 1) *= c;
// m_mat_T.block(i + 1, i + 1, 1, m_n - i - 1) += s * mat_T.block(i, i + 1, 1, m_n - i - 1);
}
m_computed = true;
}
///
/// Return the \f$R\f$ matrix in the QR decomposition, which is an
/// upper triangular matrix.
///
/// \return Returned matrix type will be `Eigen::Matrix<Scalar, ...>`, depending on
/// the template parameter `Scalar` defined.
///
virtual Matrix matrix_R() const
{
if (!m_computed)
throw std::logic_error("UpperHessenbergQR: need to call compute() first");
return m_mat_T;
}
///
/// Overwrite `dest` with \f$Q'HQ = RQ + sI\f$, where \f$H\f$ is the input matrix `mat`,
/// and \f$s\f$ is the shift. The result is an upper Hessenberg matrix.
///
/// \param mat The matrix to be overwritten, whose type should be `Eigen::Matrix<Scalar, ...>`,
/// depending on the template parameter `Scalar` defined.
///
virtual void matrix_QtHQ(Matrix& dest) const
{
if (!m_computed)
throw std::logic_error("UpperHessenbergQR: need to call compute() first");
// Make a copy of the R matrix
dest.resize(m_n, m_n);
std::copy(m_mat_T.data(), m_mat_T.data() + m_mat_T.size(), dest.data());
// Compute the RQ matrix
const Index n1 = m_n - 1;
for (Index i = 0; i < n1; i++)
{
const Scalar c = m_rot_cos.coeff(i);
const Scalar s = m_rot_sin.coeff(i);
// RQ[, i:(i + 1)] = RQ[, i:(i + 1)] * Gi
// Gi = [ cos[i] sin[i]]
// [-sin[i] cos[i]]
Scalar *Yi, *Yi1;
Yi = &dest.coeffRef(0, i);
Yi1 = Yi + m_n; // RQ(0, i + 1)
const Index i2 = i + 2;
for (Index j = 0; j < i2; j++)
{
const Scalar tmp = Yi[j];
Yi[j] = c * tmp - s * Yi1[j];
Yi1[j] = s * tmp + c * Yi1[j];
}
/* Vector dest = RQ.block(0, i, i + 2, 1);
dest.block(0, i, i + 2, 1) = c * Yi - s * dest.block(0, i + 1, i + 2, 1);
dest.block(0, i + 1, i + 2, 1) = s * Yi + c * dest.block(0, i + 1, i + 2, 1); */
}
// Add the shift to the diagonal
dest.diagonal().array() += m_shift;
}
///
/// Apply the \f$Q\f$ matrix to a vector \f$y\f$.
///
/// \param Y A vector that will be overwritten by the matrix product \f$Qy\f$.
///
/// Vector type can be `Eigen::Vector<Scalar, ...>`, depending on
/// the template parameter `Scalar` defined.
///
// Y -> QY = G1 * G2 * ... * Y
void apply_QY(Vector& Y) const
{
if (!m_computed)
throw std::logic_error("UpperHessenbergQR: need to call compute() first");
for (Index i = m_n - 2; i >= 0; i--)
{
const Scalar c = m_rot_cos.coeff(i);
const Scalar s = m_rot_sin.coeff(i);
// Y[i:(i + 1)] = Gi * Y[i:(i + 1)]
// Gi = [ cos[i] sin[i]]
// [-sin[i] cos[i]]
const Scalar tmp = Y[i];
Y[i] = c * tmp + s * Y[i + 1];
Y[i + 1] = -s * tmp + c * Y[i + 1];
}
}
///
/// Apply the \f$Q\f$ matrix to a vector \f$y\f$.
///
/// \param Y A vector that will be overwritten by the matrix product \f$Q'y\f$.
///
/// Vector type can be `Eigen::Vector<Scalar, ...>`, depending on
/// the template parameter `Scalar` defined.
///
// Y -> Q'Y = G_{n-1}' * ... * G2' * G1' * Y
void apply_QtY(Vector& Y) const
{
if (!m_computed)
throw std::logic_error("UpperHessenbergQR: need to call compute() first");
const Index n1 = m_n - 1;
for (Index i = 0; i < n1; i++)
{
const Scalar c = m_rot_cos.coeff(i);
const Scalar s = m_rot_sin.coeff(i);
// Y[i:(i + 1)] = Gi' * Y[i:(i + 1)]
// Gi = [ cos[i] sin[i]]
// [-sin[i] cos[i]]
const Scalar tmp = Y[i];
Y[i] = c * tmp - s * Y[i + 1];
Y[i + 1] = s * tmp + c * Y[i + 1];
}
}
///
/// Apply the \f$Q\f$ matrix to another matrix \f$Y\f$.
///
/// \param Y A matrix that will be overwritten by the matrix product \f$QY\f$.
///
/// Matrix type can be `Eigen::Matrix<Scalar, ...>` (e.g.
/// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
// Y -> QY = G1 * G2 * ... * Y
void apply_QY(GenericMatrix Y) const
{
if (!m_computed)
throw std::logic_error("UpperHessenbergQR: need to call compute() first");
RowVector Yi(Y.cols()), Yi1(Y.cols());
for (Index i = m_n - 2; i >= 0; i--)
{
const Scalar c = m_rot_cos.coeff(i);
const Scalar s = m_rot_sin.coeff(i);
// Y[i:(i + 1), ] = Gi * Y[i:(i + 1), ]
// Gi = [ cos[i] sin[i]]
// [-sin[i] cos[i]]
Yi.noalias() = Y.row(i);
Yi1.noalias() = Y.row(i + 1);
Y.row(i) = c * Yi + s * Yi1;
Y.row(i + 1) = -s * Yi + c * Yi1;
}
}
///
/// Apply the \f$Q\f$ matrix to another matrix \f$Y\f$.
///
/// \param Y A matrix that will be overwritten by the matrix product \f$Q'Y\f$.
///
/// Matrix type can be `Eigen::Matrix<Scalar, ...>` (e.g.
/// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
// Y -> Q'Y = G_{n-1}' * ... * G2' * G1' * Y
void apply_QtY(GenericMatrix Y) const
{
if (!m_computed)
throw std::logic_error("UpperHessenbergQR: need to call compute() first");
RowVector Yi(Y.cols()), Yi1(Y.cols());
const Index n1 = m_n - 1;
for (Index i = 0; i < n1; i++)
{
const Scalar c = m_rot_cos.coeff(i);
const Scalar s = m_rot_sin.coeff(i);
// Y[i:(i + 1), ] = Gi' * Y[i:(i + 1), ]
// Gi = [ cos[i] sin[i]]
// [-sin[i] cos[i]]
Yi.noalias() = Y.row(i);
Yi1.noalias() = Y.row(i + 1);
Y.row(i) = c * Yi - s * Yi1;
Y.row(i + 1) = s * Yi + c * Yi1;
}
}
///
/// Apply the \f$Q\f$ matrix to another matrix \f$Y\f$.
///
/// \param Y A matrix that will be overwritten by the matrix product \f$YQ\f$.
///
/// Matrix type can be `Eigen::Matrix<Scalar, ...>` (e.g.
/// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
// Y -> YQ = Y * G1 * G2 * ...
void apply_YQ(GenericMatrix Y) const
{
if (!m_computed)
throw std::logic_error("UpperHessenbergQR: need to call compute() first");
/*Vector Yi(Y.rows());
for(Index i = 0; i < m_n - 1; i++)
{
const Scalar c = m_rot_cos.coeff(i);
const Scalar s = m_rot_sin.coeff(i);
// Y[, i:(i + 1)] = Y[, i:(i + 1)] * Gi
// Gi = [ cos[i] sin[i]]
// [-sin[i] cos[i]]
Yi.noalias() = Y.col(i);
Y.col(i) = c * Yi - s * Y.col(i + 1);
Y.col(i + 1) = s * Yi + c * Y.col(i + 1);
}*/
Scalar *Y_col_i, *Y_col_i1;
const Index n1 = m_n - 1;
const Index nrow = Y.rows();
for (Index i = 0; i < n1; i++)
{
const Scalar c = m_rot_cos.coeff(i);
const Scalar s = m_rot_sin.coeff(i);
Y_col_i = &Y.coeffRef(0, i);
Y_col_i1 = &Y.coeffRef(0, i + 1);
for (Index j = 0; j < nrow; j++)
{
Scalar tmp = Y_col_i[j];
Y_col_i[j] = c * tmp - s * Y_col_i1[j];
Y_col_i1[j] = s * tmp + c * Y_col_i1[j];
}
}
}
///
/// Apply the \f$Q\f$ matrix to another matrix \f$Y\f$.
///
/// \param Y A matrix that will be overwritten by the matrix product \f$YQ'\f$.
///
/// Matrix type can be `Eigen::Matrix<Scalar, ...>` (e.g.
/// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
// Y -> YQ' = Y * G_{n-1}' * ... * G2' * G1'
void apply_YQt(GenericMatrix Y) const
{
if (!m_computed)
throw std::logic_error("UpperHessenbergQR: need to call compute() first");
Vector Yi(Y.rows());
for (Index i = m_n - 2; i >= 0; i--)
{
const Scalar c = m_rot_cos.coeff(i);
const Scalar s = m_rot_sin.coeff(i);
// Y[, i:(i + 1)] = Y[, i:(i + 1)] * Gi'
// Gi = [ cos[i] sin[i]]
// [-sin[i] cos[i]]
Yi.noalias() = Y.col(i);
Y.col(i) = c * Yi + s * Y.col(i + 1);
Y.col(i + 1) = -s * Yi + c * Y.col(i + 1);
}
}
};
///
/// \ingroup LinearAlgebra
///
/// Perform the QR decomposition of a tridiagonal matrix, a special
/// case of upper Hessenberg matrices.
///
/// \tparam Scalar The element type of the matrix.
/// Currently supported types are `float`, `double` and `long double`.
///
template <typename Scalar = double>
class TridiagQR : public UpperHessenbergQR<Scalar>
{
private:
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
typedef typename Matrix::Index Index;
Vector m_T_diag; // diagonal elements of T
Vector m_T_lsub; // lower subdiagonal of T
Vector m_T_usub; // upper subdiagonal of T
Vector m_T_usub2; // 2nd upper subdiagonal of T
public:
///
/// Constructor to preallocate memory. Computation can
/// be performed later by calling the compute() method.
///
TridiagQR(Index size) :
UpperHessenbergQR<Scalar>(size)
{}
///
/// Constructor to create an object that performs and stores the
/// QR decomposition of an upper Hessenberg matrix `mat`, with an
/// optional shift: \f$H-sI=QR\f$. Here \f$H\f$ stands for the matrix
/// `mat`, and \f$s\f$ is the shift.
///
/// \param mat Matrix type can be `Eigen::Matrix<Scalar, ...>` (e.g.
/// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
/// Only the major- and sub- diagonal parts of
/// the matrix are used.
///
TridiagQR(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0)) :
UpperHessenbergQR<Scalar>(mat.rows())
{
this->compute(mat, shift);
}
///
/// Conduct the QR factorization of a tridiagonal matrix with an
/// optional shift.
///
/// \param mat Matrix type can be `Eigen::Matrix<Scalar, ...>` (e.g.
/// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
/// Only the major- and sub- diagonal parts of
/// the matrix are used.
///
void compute(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0))
{
this->m_n = mat.rows();
if (this->m_n != mat.cols())
throw std::invalid_argument("TridiagQR: matrix must be square");
this->m_shift = shift;
m_T_diag.resize(this->m_n);
m_T_lsub.resize(this->m_n - 1);
m_T_usub.resize(this->m_n - 1);
m_T_usub2.resize(this->m_n - 2);
this->m_rot_cos.resize(this->m_n - 1);
this->m_rot_sin.resize(this->m_n - 1);
m_T_diag.array() = mat.diagonal().array() - this->m_shift;
m_T_lsub.noalias() = mat.diagonal(-1);
m_T_usub.noalias() = m_T_lsub;
// A number of pointers to avoid repeated address calculation
Scalar *c = this->m_rot_cos.data(), // pointer to the cosine vector
*s = this->m_rot_sin.data(), // pointer to the sine vector
r;
const Index n1 = this->m_n - 1;
for (Index i = 0; i < n1; i++)
{
// diag[i] == T[i, i]
// lsub[i] == T[i + 1, i]
// r = sqrt(T[i, i]^2 + T[i + 1, i]^2)
// c = T[i, i] / r, s = -T[i + 1, i] / r
this->compute_rotation(m_T_diag.coeff(i), m_T_lsub.coeff(i), r, *c, *s);
// For a complete QR decomposition,
// we first obtain the rotation matrix
// G = [ cos sin]
// [-sin cos]
// and then do T[i:(i + 1), i:(i + 2)] = G' * T[i:(i + 1), i:(i + 2)]
// Update T[i, i] and T[i + 1, i]
// The updated value of T[i, i] is known to be r
// The updated value of T[i + 1, i] is known to be 0
m_T_diag.coeffRef(i) = r;
m_T_lsub.coeffRef(i) = Scalar(0);
// Update T[i, i + 1] and T[i + 1, i + 1]
// usub[i] == T[i, i + 1]
// diag[i + 1] == T[i + 1, i + 1]
const Scalar tmp = m_T_usub.coeff(i);
m_T_usub.coeffRef(i) = (*c) * tmp - (*s) * m_T_diag.coeff(i + 1);
m_T_diag.coeffRef(i + 1) = (*s) * tmp + (*c) * m_T_diag.coeff(i + 1);
// Update T[i, i + 2] and T[i + 1, i + 2]
// usub2[i] == T[i, i + 2]
// usub[i + 1] == T[i + 1, i + 2]
if (i < n1 - 1)
{
m_T_usub2.coeffRef(i) = -(*s) * m_T_usub.coeff(i + 1);
m_T_usub.coeffRef(i + 1) *= (*c);
}
c++;
s++;
// If we do not need to calculate the R matrix, then
// only the cos and sin sequences are required.
// In such case we only update T[i + 1, (i + 1):(i + 2)]
// T[i + 1, i + 1] = c * T[i + 1, i + 1] + s * T[i, i + 1];
// T[i + 1, i + 2] *= c;
}
this->m_computed = true;
}
///
/// Return the \f$R\f$ matrix in the QR decomposition, which is an
/// upper triangular matrix.
///
/// \return Returned matrix type will be `Eigen::Matrix<Scalar, ...>`, depending on
/// the template parameter `Scalar` defined.
///
Matrix matrix_R() const
{
if (!this->m_computed)
throw std::logic_error("TridiagQR: need to call compute() first");
Matrix R = Matrix::Zero(this->m_n, this->m_n);
R.diagonal().noalias() = m_T_diag;
R.diagonal(1).noalias() = m_T_usub;
R.diagonal(2).noalias() = m_T_usub2;
return R;
}
///
/// Overwrite `dest` with \f$Q'HQ = RQ + sI\f$, where \f$H\f$ is the input matrix `mat`,
/// and \f$s\f$ is the shift. The result is a tridiagonal matrix.
///
/// \param mat The matrix to be overwritten, whose type should be `Eigen::Matrix<Scalar, ...>`,
/// depending on the template parameter `Scalar` defined.
///
void matrix_QtHQ(Matrix& dest) const
{
if (!this->m_computed)
throw std::logic_error("TridiagQR: need to call compute() first");
// Make a copy of the R matrix
dest.resize(this->m_n, this->m_n);
dest.setZero();
dest.diagonal().noalias() = m_T_diag;
// The upper diagonal refers to m_T_usub
// The 2nd upper subdiagonal will be zero in RQ
// Compute the RQ matrix
// [m11 m12] points to RQ[i:(i+1), i:(i+1)]
// [0 m22]
//
// Gi = [ cos[i] sin[i]]
// [-sin[i] cos[i]]
const Index n1 = this->m_n - 1;
for (Index i = 0; i < n1; i++)
{
const Scalar c = this->m_rot_cos.coeff(i);
const Scalar s = this->m_rot_sin.coeff(i);
const Scalar m11 = dest.coeff(i, i),
m12 = m_T_usub.coeff(i),
m22 = m_T_diag.coeff(i + 1);
// Update the diagonal and the lower subdiagonal of dest
dest.coeffRef(i, i) = c * m11 - s * m12;
dest.coeffRef(i + 1, i) = -s * m22;
dest.coeffRef(i + 1, i + 1) = c * m22;
}
// Copy the lower subdiagonal to upper subdiagonal
dest.diagonal(1).noalias() = dest.diagonal(-1);
// Add the shift to the diagonal
dest.diagonal().array() += this->m_shift;
}
};
///
/// @}
///
} // namespace Spectra
#endif // UPPER_HESSENBERG_QR_H

108
gtsam/3rdparty/Spectra/MatOp/DenseCholesky.h vendored Normal file
View File

@ -0,0 +1,108 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef DENSE_CHOLESKY_H
#define DENSE_CHOLESKY_H
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <stdexcept>
#include "../Util/CompInfo.h"
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the operations related to Cholesky decomposition on a
/// positive definite matrix, \f$B=LL'\f$, where \f$L\f$ is a lower triangular
/// matrix. It is mainly used in the SymGEigsSolver generalized eigen solver
/// in the Cholesky decomposition mode.
///
template <typename Scalar, int Uplo = Eigen::Lower>
class DenseCholesky
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Matrix> MapConstMat;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
const Index m_n;
Eigen::LLT<Matrix, Uplo> m_decomp;
int m_info; // status of the decomposition
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** matrix object, whose type can be
/// `Eigen::Matrix<Scalar, ...>` (e.g. `Eigen::MatrixXd` and
/// `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
DenseCholesky(ConstGenericMatrix& mat) :
m_n(mat.rows()), m_info(NOT_COMPUTED)
{
if (mat.rows() != mat.cols())
throw std::invalid_argument("DenseCholesky: matrix must be square");
m_decomp.compute(mat);
m_info = (m_decomp.info() == Eigen::Success) ?
SUCCESSFUL :
NUMERICAL_ISSUE;
}
///
/// Returns the number of rows of the underlying matrix.
///
Index rows() const { return m_n; }
///
/// Returns the number of columns of the underlying matrix.
///
Index cols() const { return m_n; }
///
/// Returns the status of the computation.
/// The full list of enumeration values can be found in \ref Enumerations.
///
int info() const { return m_info; }
///
/// Performs the lower triangular solving operation \f$y=L^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(L) * x_in
void lower_triangular_solve(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_decomp.matrixL().solve(x);
}
///
/// Performs the upper triangular solving operation \f$y=(L')^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(L') * x_in
void upper_triangular_solve(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_decomp.matrixU().solve(x);
}
};
} // namespace Spectra
#endif // DENSE_CHOLESKY_H

102
gtsam/3rdparty/Spectra/MatOp/DenseGenComplexShiftSolve.h vendored Normal file
View File

@ -0,0 +1,102 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef DENSE_GEN_COMPLEX_SHIFT_SOLVE_H
#define DENSE_GEN_COMPLEX_SHIFT_SOLVE_H
#include <Eigen/Core>
#include <Eigen/LU>
#include <stdexcept>
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the complex shift-solve operation on a general real matrix \f$A\f$,
/// i.e., calculating \f$y=\mathrm{Re}\{(A-\sigma I)^{-1}x\}\f$ for any complex-valued
/// \f$\sigma\f$ and real-valued vector \f$x\f$. It is mainly used in the
/// GenEigsComplexShiftSolver eigen solver.
///
template <typename Scalar>
class DenseGenComplexShiftSolve
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
typedef std::complex<Scalar> Complex;
typedef Eigen::Matrix<Complex, Eigen::Dynamic, Eigen::Dynamic> ComplexMatrix;
typedef Eigen::Matrix<Complex, Eigen::Dynamic, 1> ComplexVector;
typedef Eigen::PartialPivLU<ComplexMatrix> ComplexSolver;
ConstGenericMatrix m_mat;
const Index m_n;
ComplexSolver m_solver;
ComplexVector m_x_cache;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** matrix object, whose type can be
/// `Eigen::Matrix<Scalar, ...>` (e.g. `Eigen::MatrixXd` and
/// `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
DenseGenComplexShiftSolve(ConstGenericMatrix& mat) :
m_mat(mat), m_n(mat.rows())
{
if (mat.rows() != mat.cols())
throw std::invalid_argument("DenseGenComplexShiftSolve: matrix must be square");
}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_n; }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_n; }
///
/// Set the complex shift \f$\sigma\f$.
///
/// \param sigmar Real part of \f$\sigma\f$.
/// \param sigmai Imaginary part of \f$\sigma\f$.
///
void set_shift(Scalar sigmar, Scalar sigmai)
{
m_solver.compute(m_mat.template cast<Complex>() - Complex(sigmar, sigmai) * ComplexMatrix::Identity(m_n, m_n));
m_x_cache.resize(m_n);
m_x_cache.setZero();
}
///
/// Perform the complex shift-solve operation
/// \f$y=\mathrm{Re}\{(A-\sigma I)^{-1}x\}\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = Re( inv(A - sigma * I) * x_in )
void perform_op(const Scalar* x_in, Scalar* y_out)
{
m_x_cache.real() = MapConstVec(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_solver.solve(m_x_cache).real();
}
};
} // namespace Spectra
#endif // DENSE_GEN_COMPLEX_SHIFT_SOLVE_H

80
gtsam/3rdparty/Spectra/MatOp/DenseGenMatProd.h vendored Normal file
View File

@ -0,0 +1,80 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef DENSE_GEN_MAT_PROD_H
#define DENSE_GEN_MAT_PROD_H
#include <Eigen/Core>
namespace Spectra {
///
/// \defgroup MatOp Matrix Operations
///
/// Define matrix operations on existing matrix objects
///
///
/// \ingroup MatOp
///
/// This class defines the matrix-vector multiplication operation on a
/// general real matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector
/// \f$x\f$. It is mainly used in the GenEigsSolver and
/// SymEigsSolver eigen solvers.
///
template <typename Scalar>
class DenseGenMatProd
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
ConstGenericMatrix m_mat;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** matrix object, whose type can be
/// `Eigen::Matrix<Scalar, ...>` (e.g. `Eigen::MatrixXd` and
/// `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
DenseGenMatProd(ConstGenericMatrix& mat) :
m_mat(mat)
{}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_mat.rows(); }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_mat.cols(); }
///
/// Perform the matrix-vector multiplication operation \f$y=Ax\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = A * x_in
void perform_op(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_mat.cols());
MapVec y(y_out, m_mat.rows());
y.noalias() = m_mat * x;
}
};
} // namespace Spectra
#endif // DENSE_GEN_MAT_PROD_H

88
gtsam/3rdparty/Spectra/MatOp/DenseGenRealShiftSolve.h vendored Normal file
View File

@ -0,0 +1,88 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef DENSE_GEN_REAL_SHIFT_SOLVE_H
#define DENSE_GEN_REAL_SHIFT_SOLVE_H
#include <Eigen/Core>
#include <Eigen/LU>
#include <stdexcept>
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the shift-solve operation on a general real matrix \f$A\f$,
/// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and
/// vector \f$x\f$. It is mainly used in the GenEigsRealShiftSolver eigen solver.
///
template <typename Scalar>
class DenseGenRealShiftSolve
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
ConstGenericMatrix m_mat;
const Index m_n;
Eigen::PartialPivLU<Matrix> m_solver;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** matrix object, whose type can be
/// `Eigen::Matrix<Scalar, ...>` (e.g. `Eigen::MatrixXd` and
/// `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
DenseGenRealShiftSolve(ConstGenericMatrix& mat) :
m_mat(mat), m_n(mat.rows())
{
if (mat.rows() != mat.cols())
throw std::invalid_argument("DenseGenRealShiftSolve: matrix must be square");
}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_n; }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_n; }
///
/// Set the real shift \f$\sigma\f$.
///
void set_shift(Scalar sigma)
{
m_solver.compute(m_mat - sigma * Matrix::Identity(m_n, m_n));
}
///
/// Perform the shift-solve operation \f$y=(A-\sigma I)^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(A - sigma * I) * x_in
void perform_op(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_solver.solve(x);
}
};
} // namespace Spectra
#endif // DENSE_GEN_REAL_SHIFT_SOLVE_H

73
gtsam/3rdparty/Spectra/MatOp/DenseSymMatProd.h vendored Normal file
View File

@ -0,0 +1,73 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef DENSE_SYM_MAT_PROD_H
#define DENSE_SYM_MAT_PROD_H
#include <Eigen/Core>
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the matrix-vector multiplication operation on a
/// symmetric real matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector
/// \f$x\f$. It is mainly used in the SymEigsSolver eigen solver.
///
template <typename Scalar, int Uplo = Eigen::Lower>
class DenseSymMatProd
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
ConstGenericMatrix m_mat;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** matrix object, whose type can be
/// `Eigen::Matrix<Scalar, ...>` (e.g. `Eigen::MatrixXd` and
/// `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
DenseSymMatProd(ConstGenericMatrix& mat) :
m_mat(mat)
{}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_mat.rows(); }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_mat.cols(); }
///
/// Perform the matrix-vector multiplication operation \f$y=Ax\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = A * x_in
void perform_op(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_mat.cols());
MapVec y(y_out, m_mat.rows());
y.noalias() = m_mat.template selfadjointView<Uplo>() * x;
}
};
} // namespace Spectra
#endif // DENSE_SYM_MAT_PROD_H

92
gtsam/3rdparty/Spectra/MatOp/DenseSymShiftSolve.h vendored Normal file
View File

@ -0,0 +1,92 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef DENSE_SYM_SHIFT_SOLVE_H
#define DENSE_SYM_SHIFT_SOLVE_H
#include <Eigen/Core>
#include <stdexcept>
#include "../LinAlg/BKLDLT.h"
#include "../Util/CompInfo.h"
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the shift-solve operation on a real symmetric matrix \f$A\f$,
/// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and
/// vector \f$x\f$. It is mainly used in the SymEigsShiftSolver eigen solver.
///
template <typename Scalar, int Uplo = Eigen::Lower>
class DenseSymShiftSolve
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef const Eigen::Ref<const Matrix> ConstGenericMatrix;
ConstGenericMatrix m_mat;
const int m_n;
BKLDLT<Scalar> m_solver;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** matrix object, whose type can be
/// `Eigen::Matrix<Scalar, ...>` (e.g. `Eigen::MatrixXd` and
/// `Eigen::MatrixXf`), or its mapped version
/// (e.g. `Eigen::Map<Eigen::MatrixXd>`).
///
DenseSymShiftSolve(ConstGenericMatrix& mat) :
m_mat(mat), m_n(mat.rows())
{
if (mat.rows() != mat.cols())
throw std::invalid_argument("DenseSymShiftSolve: matrix must be square");
}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_n; }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_n; }
///
/// Set the real shift \f$\sigma\f$.
///
void set_shift(Scalar sigma)
{
m_solver.compute(m_mat, Uplo, sigma);
if (m_solver.info() != SUCCESSFUL)
throw std::invalid_argument("DenseSymShiftSolve: factorization failed with the given shift");
}
///
/// Perform the shift-solve operation \f$y=(A-\sigma I)^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(A - sigma * I) * x_in
void perform_op(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_solver.solve(x);
}
};
} // namespace Spectra
#endif // DENSE_SYM_SHIFT_SOLVE_H

109
gtsam/3rdparty/Spectra/MatOp/SparseCholesky.h vendored Normal file
View File

@ -0,0 +1,109 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SPARSE_CHOLESKY_H
#define SPARSE_CHOLESKY_H
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseCholesky>
#include <stdexcept>
#include "../Util/CompInfo.h"
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the operations related to Cholesky decomposition on a
/// sparse positive definite matrix, \f$B=LL'\f$, where \f$L\f$ is a lower triangular
/// matrix. It is mainly used in the SymGEigsSolver generalized eigen solver
/// in the Cholesky decomposition mode.
///
template <typename Scalar, int Uplo = Eigen::Lower, int Flags = 0, typename StorageIndex = int>
class SparseCholesky
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::SparseMatrix<Scalar, Flags, StorageIndex> SparseMatrix;
typedef const Eigen::Ref<const SparseMatrix> ConstGenericSparseMatrix;
const Index m_n;
Eigen::SimplicialLLT<SparseMatrix, Uplo> m_decomp;
int m_info; // status of the decomposition
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** sparse matrix object, whose type can be
/// `Eigen::SparseMatrix<Scalar, ...>` or its mapped version
/// `Eigen::Map<Eigen::SparseMatrix<Scalar, ...> >`.
///
SparseCholesky(ConstGenericSparseMatrix& mat) :
m_n(mat.rows())
{
if (mat.rows() != mat.cols())
throw std::invalid_argument("SparseCholesky: matrix must be square");
m_decomp.compute(mat);
m_info = (m_decomp.info() == Eigen::Success) ?
SUCCESSFUL :
NUMERICAL_ISSUE;
}
///
/// Returns the number of rows of the underlying matrix.
///
Index rows() const { return m_n; }
///
/// Returns the number of columns of the underlying matrix.
///
Index cols() const { return m_n; }
///
/// Returns the status of the computation.
/// The full list of enumeration values can be found in \ref Enumerations.
///
int info() const { return m_info; }
///
/// Performs the lower triangular solving operation \f$y=L^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(L) * x_in
void lower_triangular_solve(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_decomp.permutationP() * x;
m_decomp.matrixL().solveInPlace(y);
}
///
/// Performs the upper triangular solving operation \f$y=(L')^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(L') * x_in
void upper_triangular_solve(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_decomp.matrixU().solve(x);
y = m_decomp.permutationPinv() * y;
}
};
} // namespace Spectra
#endif // SPARSE_CHOLESKY_H

74
gtsam/3rdparty/Spectra/MatOp/SparseGenMatProd.h vendored Normal file
View File

@ -0,0 +1,74 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SPARSE_GEN_MAT_PROD_H
#define SPARSE_GEN_MAT_PROD_H
#include <Eigen/Core>
#include <Eigen/SparseCore>
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the matrix-vector multiplication operation on a
/// sparse real matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector
/// \f$x\f$. It is mainly used in the GenEigsSolver and SymEigsSolver
/// eigen solvers.
///
template <typename Scalar, int Flags = 0, typename StorageIndex = int>
class SparseGenMatProd
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::SparseMatrix<Scalar, Flags, StorageIndex> SparseMatrix;
typedef const Eigen::Ref<const SparseMatrix> ConstGenericSparseMatrix;
ConstGenericSparseMatrix m_mat;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** sparse matrix object, whose type can be
/// `Eigen::SparseMatrix<Scalar, ...>` or its mapped version
/// `Eigen::Map<Eigen::SparseMatrix<Scalar, ...> >`.
///
SparseGenMatProd(ConstGenericSparseMatrix& mat) :
m_mat(mat)
{}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_mat.rows(); }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_mat.cols(); }
///
/// Perform the matrix-vector multiplication operation \f$y=Ax\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = A * x_in
void perform_op(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_mat.cols());
MapVec y(y_out, m_mat.rows());
y.noalias() = m_mat * x;
}
};
} // namespace Spectra
#endif // SPARSE_GEN_MAT_PROD_H

93
gtsam/3rdparty/Spectra/MatOp/SparseGenRealShiftSolve.h vendored Normal file
View File

@ -0,0 +1,93 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SPARSE_GEN_REAL_SHIFT_SOLVE_H
#define SPARSE_GEN_REAL_SHIFT_SOLVE_H
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <stdexcept>
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the shift-solve operation on a sparse real matrix \f$A\f$,
/// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and
/// vector \f$x\f$. It is mainly used in the GenEigsRealShiftSolver eigen solver.
///
template <typename Scalar, int Flags = 0, typename StorageIndex = int>
class SparseGenRealShiftSolve
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::SparseMatrix<Scalar, Flags, StorageIndex> SparseMatrix;
typedef const Eigen::Ref<const SparseMatrix> ConstGenericSparseMatrix;
ConstGenericSparseMatrix m_mat;
const int m_n;
Eigen::SparseLU<SparseMatrix> m_solver;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** sparse matrix object, whose type can be
/// `Eigen::SparseMatrix<Scalar, ...>` or its mapped version
/// `Eigen::Map<Eigen::SparseMatrix<Scalar, ...> >`.
///
SparseGenRealShiftSolve(ConstGenericSparseMatrix& mat) :
m_mat(mat), m_n(mat.rows())
{
if (mat.rows() != mat.cols())
throw std::invalid_argument("SparseGenRealShiftSolve: matrix must be square");
}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_n; }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_n; }
///
/// Set the real shift \f$\sigma\f$.
///
void set_shift(Scalar sigma)
{
SparseMatrix I(m_n, m_n);
I.setIdentity();
m_solver.compute(m_mat - sigma * I);
if (m_solver.info() != Eigen::Success)
throw std::invalid_argument("SparseGenRealShiftSolve: factorization failed with the given shift");
}
///
/// Perform the shift-solve operation \f$y=(A-\sigma I)^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(A - sigma * I) * x_in
void perform_op(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_solver.solve(x);
}
};
} // namespace Spectra
#endif // SPARSE_GEN_REAL_SHIFT_SOLVE_H

100
gtsam/3rdparty/Spectra/MatOp/SparseRegularInverse.h vendored Normal file
View File

@ -0,0 +1,100 @@
// Copyright (C) 2017-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SPARSE_REGULAR_INVERSE_H
#define SPARSE_REGULAR_INVERSE_H
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/IterativeLinearSolvers>
#include <stdexcept>
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines matrix operations required by the generalized eigen solver
/// in the regular inverse mode. For a sparse and positive definite matrix \f$B\f$,
/// it implements the matrix-vector product \f$y=Bx\f$ and the linear equation
/// solving operation \f$y=B^{-1}x\f$.
///
/// This class is intended to be used with the SymGEigsSolver generalized eigen solver
/// in the regular inverse mode.
///
template <typename Scalar, int Uplo = Eigen::Lower, int Flags = 0, typename StorageIndex = int>
class SparseRegularInverse
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::SparseMatrix<Scalar, Flags, StorageIndex> SparseMatrix;
typedef const Eigen::Ref<const SparseMatrix> ConstGenericSparseMatrix;
ConstGenericSparseMatrix m_mat;
const int m_n;
Eigen::ConjugateGradient<SparseMatrix> m_cg;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** sparse matrix object, whose type can be
/// `Eigen::SparseMatrix<Scalar, ...>` or its mapped version
/// `Eigen::Map<Eigen::SparseMatrix<Scalar, ...> >`.
///
SparseRegularInverse(ConstGenericSparseMatrix& mat) :
m_mat(mat), m_n(mat.rows())
{
if (mat.rows() != mat.cols())
throw std::invalid_argument("SparseRegularInverse: matrix must be square");
m_cg.compute(mat);
}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_n; }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_n; }
///
/// Perform the solving operation \f$y=B^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(B) * x_in
void solve(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_cg.solve(x);
}
///
/// Perform the matrix-vector multiplication operation \f$y=Bx\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = B * x_in
void mat_prod(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_mat.template selfadjointView<Uplo>() * x;
}
};
} // namespace Spectra
#endif // SPARSE_REGULAR_INVERSE_H

73
gtsam/3rdparty/Spectra/MatOp/SparseSymMatProd.h vendored Normal file
View File

@ -0,0 +1,73 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SPARSE_SYM_MAT_PROD_H
#define SPARSE_SYM_MAT_PROD_H
#include <Eigen/Core>
#include <Eigen/SparseCore>
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the matrix-vector multiplication operation on a
/// sparse real symmetric matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector
/// \f$x\f$. It is mainly used in the SymEigsSolver eigen solver.
///
template <typename Scalar, int Uplo = Eigen::Lower, int Flags = 0, typename StorageIndex = int>
class SparseSymMatProd
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::SparseMatrix<Scalar, Flags, StorageIndex> SparseMatrix;
typedef const Eigen::Ref<const SparseMatrix> ConstGenericSparseMatrix;
ConstGenericSparseMatrix m_mat;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** sparse matrix object, whose type can be
/// `Eigen::SparseMatrix<Scalar, ...>` or its mapped version
/// `Eigen::Map<Eigen::SparseMatrix<Scalar, ...> >`.
///
SparseSymMatProd(ConstGenericSparseMatrix& mat) :
m_mat(mat)
{}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_mat.rows(); }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_mat.cols(); }
///
/// Perform the matrix-vector multiplication operation \f$y=Ax\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = A * x_in
void perform_op(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_mat.cols());
MapVec y(y_out, m_mat.rows());
y.noalias() = m_mat.template selfadjointView<Uplo>() * x;
}
};
} // namespace Spectra
#endif // SPARSE_SYM_MAT_PROD_H

95
gtsam/3rdparty/Spectra/MatOp/SparseSymShiftSolve.h vendored Normal file
View File

@ -0,0 +1,95 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SPARSE_SYM_SHIFT_SOLVE_H
#define SPARSE_SYM_SHIFT_SOLVE_H
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <stdexcept>
namespace Spectra {
///
/// \ingroup MatOp
///
/// This class defines the shift-solve operation on a sparse real symmetric matrix \f$A\f$,
/// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and
/// vector \f$x\f$. It is mainly used in the SymEigsShiftSolver eigen solver.
///
template <typename Scalar, int Uplo = Eigen::Lower, int Flags = 0, typename StorageIndex = int>
class SparseSymShiftSolve
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::SparseMatrix<Scalar, Flags, StorageIndex> SparseMatrix;
typedef const Eigen::Ref<const SparseMatrix> ConstGenericSparseMatrix;
ConstGenericSparseMatrix m_mat;
const int m_n;
Eigen::SparseLU<SparseMatrix> m_solver;
public:
///
/// Constructor to create the matrix operation object.
///
/// \param mat An **Eigen** sparse matrix object, whose type can be
/// `Eigen::SparseMatrix<Scalar, ...>` or its mapped version
/// `Eigen::Map<Eigen::SparseMatrix<Scalar, ...> >`.
///
SparseSymShiftSolve(ConstGenericSparseMatrix& mat) :
m_mat(mat), m_n(mat.rows())
{
if (mat.rows() != mat.cols())
throw std::invalid_argument("SparseSymShiftSolve: matrix must be square");
}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_n; }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_n; }
///
/// Set the real shift \f$\sigma\f$.
///
void set_shift(Scalar sigma)
{
SparseMatrix mat = m_mat.template selfadjointView<Uplo>();
SparseMatrix identity(m_n, m_n);
identity.setIdentity();
mat = mat - sigma * identity;
m_solver.isSymmetric(true);
m_solver.compute(mat);
if (m_solver.info() != Eigen::Success)
throw std::invalid_argument("SparseSymShiftSolve: factorization failed with the given shift");
}
///
/// Perform the shift-solve operation \f$y=(A-\sigma I)^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(A - sigma * I) * x_in
void perform_op(const Scalar* x_in, Scalar* y_out) const
{
MapConstVec x(x_in, m_n);
MapVec y(y_out, m_n);
y.noalias() = m_solver.solve(x);
}
};
} // namespace Spectra
#endif // SPARSE_SYM_SHIFT_SOLVE_H

150
gtsam/3rdparty/Spectra/MatOp/internal/ArnoldiOp.h vendored Normal file
View File

@ -0,0 +1,150 @@
// Copyright (C) 2018-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef ARNOLDI_OP_H
#define ARNOLDI_OP_H
#include <Eigen/Core>
#include <cmath> // std::sqrt
namespace Spectra {
///
/// \ingroup Internals
/// @{
///
///
/// \defgroup Operators Operators
///
/// Different types of operators.
///
///
/// \ingroup Operators
///
/// Operators used in the Arnoldi factorization.
///
template <typename Scalar, typename OpType, typename BOpType>
class ArnoldiOp
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
OpType& m_op;
BOpType& m_Bop;
Vector m_cache;
public:
ArnoldiOp(OpType* op, BOpType* Bop) :
m_op(*op), m_Bop(*Bop), m_cache(op->rows())
{}
inline Index rows() const { return m_op.rows(); }
// In generalized eigenvalue problem Ax=lambda*Bx, define the inner product to be <x, y> = x'By.
// For regular eigenvalue problems, it is the usual inner product <x, y> = x'y
// Compute <x, y> = x'By
// x and y are two vectors
template <typename Arg1, typename Arg2>
Scalar inner_product(const Arg1& x, const Arg2& y)
{
m_Bop.mat_prod(y.data(), m_cache.data());
return x.dot(m_cache);
}
// Compute res = <X, y> = X'By
// X is a matrix, y is a vector, res is a vector
template <typename Arg1, typename Arg2>
void trans_product(const Arg1& x, const Arg2& y, Eigen::Ref<Vector> res)
{
m_Bop.mat_prod(y.data(), m_cache.data());
res.noalias() = x.transpose() * m_cache;
}
// B-norm of a vector, ||x||_B = sqrt(x'Bx)
template <typename Arg>
Scalar norm(const Arg& x)
{
using std::sqrt;
return sqrt(inner_product<Arg, Arg>(x, x));
}
// The "A" operator to generate the Krylov subspace
inline void perform_op(const Scalar* x_in, Scalar* y_out)
{
m_op.perform_op(x_in, y_out);
}
};
///
/// \ingroup Operators
///
/// Placeholder for the B-operator when \f$B = I\f$.
///
class IdentityBOp
{};
///
/// \ingroup Operators
///
/// Partial specialization for the case \f$B = I\f$.
///
template <typename Scalar, typename OpType>
class ArnoldiOp<Scalar, OpType, IdentityBOp>
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
OpType& m_op;
public:
ArnoldiOp<Scalar, OpType, IdentityBOp>(OpType* op, IdentityBOp* /*Bop*/) :
m_op(*op)
{}
inline Index rows() const { return m_op.rows(); }
// Compute <x, y> = x'y
// x and y are two vectors
template <typename Arg1, typename Arg2>
Scalar inner_product(const Arg1& x, const Arg2& y) const
{
return x.dot(y);
}
// Compute res = <X, y> = X'y
// X is a matrix, y is a vector, res is a vector
template <typename Arg1, typename Arg2>
void trans_product(const Arg1& x, const Arg2& y, Eigen::Ref<Vector> res) const
{
res.noalias() = x.transpose() * y;
}
// B-norm of a vector. For regular eigenvalue problems it is simply the L2 norm
template <typename Arg>
Scalar norm(const Arg& x)
{
return x.norm();
}
// The "A" operator to generate the Krylov subspace
inline void perform_op(const Scalar* x_in, Scalar* y_out)
{
m_op.perform_op(x_in, y_out);
}
};
///
/// @}
///
} // namespace Spectra
#endif // ARNOLDI_OP_H

75
gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsCholeskyOp.h vendored Normal file
View File

@ -0,0 +1,75 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SYM_GEIGS_CHOLESKY_OP_H
#define SYM_GEIGS_CHOLESKY_OP_H
#include <Eigen/Core>
#include "../DenseSymMatProd.h"
#include "../DenseCholesky.h"
namespace Spectra {
///
/// \ingroup Operators
///
/// This class defines the matrix operation for generalized eigen solver in the
/// Cholesky decomposition mode. It calculates \f$y=L^{-1}A(L')^{-1}x\f$ for any
/// vector \f$x\f$, where \f$L\f$ is the Cholesky decomposition of \f$B\f$.
/// This class is intended for internal use.
///
template <typename Scalar = double,
typename OpType = DenseSymMatProd<double>,
typename BOpType = DenseCholesky<double> >
class SymGEigsCholeskyOp
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
OpType& m_op;
BOpType& m_Bop;
Vector m_cache; // temporary working space
public:
///
/// Constructor to create the matrix operation object.
///
/// \param op Pointer to the \f$A\f$ matrix operation object.
/// \param Bop Pointer to the \f$B\f$ matrix operation object.
///
SymGEigsCholeskyOp(OpType& op, BOpType& Bop) :
m_op(op), m_Bop(Bop), m_cache(op.rows())
{}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_Bop.rows(); }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_Bop.rows(); }
///
/// Perform the matrix operation \f$y=L^{-1}A(L')^{-1}x\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(L) * A * inv(L') * x_in
void perform_op(const Scalar* x_in, Scalar* y_out)
{
m_Bop.upper_triangular_solve(x_in, y_out);
m_op.perform_op(y_out, m_cache.data());
m_Bop.lower_triangular_solve(m_cache.data(), y_out);
}
};
} // namespace Spectra
#endif // SYM_GEIGS_CHOLESKY_OP_H

72
gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsRegInvOp.h vendored Normal file
View File

@ -0,0 +1,72 @@
// Copyright (C) 2017-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SYM_GEIGS_REG_INV_OP_H
#define SYM_GEIGS_REG_INV_OP_H
#include <Eigen/Core>
#include "../SparseSymMatProd.h"
#include "../SparseRegularInverse.h"
namespace Spectra {
///
/// \ingroup Operators
///
/// This class defines the matrix operation for generalized eigen solver in the
/// regular inverse mode. This class is intended for internal use.
///
template <typename Scalar = double,
typename OpType = SparseSymMatProd<double>,
typename BOpType = SparseRegularInverse<double> >
class SymGEigsRegInvOp
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
OpType& m_op;
BOpType& m_Bop;
Vector m_cache; // temporary working space
public:
///
/// Constructor to create the matrix operation object.
///
/// \param op Pointer to the \f$A\f$ matrix operation object.
/// \param Bop Pointer to the \f$B\f$ matrix operation object.
///
SymGEigsRegInvOp(OpType& op, BOpType& Bop) :
m_op(op), m_Bop(Bop), m_cache(op.rows())
{}
///
/// Return the number of rows of the underlying matrix.
///
Index rows() const { return m_Bop.rows(); }
///
/// Return the number of columns of the underlying matrix.
///
Index cols() const { return m_Bop.rows(); }
///
/// Perform the matrix operation \f$y=B^{-1}Ax\f$.
///
/// \param x_in Pointer to the \f$x\f$ vector.
/// \param y_out Pointer to the \f$y\f$ vector.
///
// y_out = inv(B) * A * x_in
void perform_op(const Scalar* x_in, Scalar* y_out)
{
m_op.perform_op(x_in, m_cache.data());
m_Bop.solve(m_cache.data(), y_out);
}
};
} // namespace Spectra
#endif // SYM_GEIGS_REG_INV_OP_H

448
gtsam/3rdparty/Spectra/SymEigsBase.h vendored Normal file
View File

@ -0,0 +1,448 @@
// Copyright (C) 2018-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SYM_EIGS_BASE_H
#define SYM_EIGS_BASE_H
#include <Eigen/Core>
#include <vector> // std::vector
#include <cmath> // std::abs, std::pow, std::sqrt
#include <algorithm> // std::min, std::copy
#include <stdexcept> // std::invalid_argument
#include "Util/TypeTraits.h"
#include "Util/SelectionRule.h"
#include "Util/CompInfo.h"
#include "Util/SimpleRandom.h"
#include "MatOp/internal/ArnoldiOp.h"
#include "LinAlg/UpperHessenbergQR.h"
#include "LinAlg/TridiagEigen.h"
#include "LinAlg/Lanczos.h"
namespace Spectra {
///
/// \defgroup EigenSolver Eigen Solvers
///
/// Eigen solvers for different types of problems.
///
///
/// \ingroup EigenSolver
///
/// This is the base class for symmetric eigen solvers, mainly for internal use.
/// It is kept here to provide the documentation for member functions of concrete eigen solvers
/// such as SymEigsSolver and SymEigsShiftSolver.
///
template <typename Scalar,
int SelectionRule,
typename OpType,
typename BOpType>
class SymEigsBase
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Array<Scalar, Eigen::Dynamic, 1> Array;
typedef Eigen::Array<bool, Eigen::Dynamic, 1> BoolArray;
typedef Eigen::Map<Matrix> MapMat;
typedef Eigen::Map<Vector> MapVec;
typedef Eigen::Map<const Vector> MapConstVec;
typedef ArnoldiOp<Scalar, OpType, BOpType> ArnoldiOpType;
typedef Lanczos<Scalar, ArnoldiOpType> LanczosFac;
protected:
// clang-format off
OpType* m_op; // object to conduct matrix operation,
// e.g. matrix-vector product
const Index m_n; // dimension of matrix A
const Index m_nev; // number of eigenvalues requested
const Index m_ncv; // dimension of Krylov subspace in the Lanczos method
Index m_nmatop; // number of matrix operations called
Index m_niter; // number of restarting iterations
LanczosFac m_fac; // Lanczos factorization
Vector m_ritz_val; // Ritz values
private:
Matrix m_ritz_vec; // Ritz vectors
Vector m_ritz_est; // last row of m_ritz_vec, also called the Ritz estimates
BoolArray m_ritz_conv; // indicator of the convergence of Ritz values
int m_info; // status of the computation
const Scalar m_near_0; // a very small value, but 1.0 / m_near_0 does not overflow
// ~= 1e-307 for the "double" type
const Scalar m_eps; // the machine precision, ~= 1e-16 for the "double" type
const Scalar m_eps23; // m_eps^(2/3), used to test the convergence
// clang-format on
// Implicitly restarted Lanczos factorization
void restart(Index k)
{
if (k >= m_ncv)
return;
TridiagQR<Scalar> decomp(m_ncv);
Matrix Q = Matrix::Identity(m_ncv, m_ncv);
for (Index i = k; i < m_ncv; i++)
{
// QR decomposition of H-mu*I, mu is the shift
decomp.compute(m_fac.matrix_H(), m_ritz_val[i]);
// Q -> Q * Qi
decomp.apply_YQ(Q);
// H -> Q'HQ
// Since QR = H - mu * I, we have H = QR + mu * I
// and therefore Q'HQ = RQ + mu * I
m_fac.compress_H(decomp);
}
m_fac.compress_V(Q);
m_fac.factorize_from(k, m_ncv, m_nmatop);
retrieve_ritzpair();
}
// Calculates the number of converged Ritz values
Index num_converged(Scalar tol)
{
// thresh = tol * max(m_eps23, abs(theta)), theta for Ritz value
Array thresh = tol * m_ritz_val.head(m_nev).array().abs().max(m_eps23);
Array resid = m_ritz_est.head(m_nev).array().abs() * m_fac.f_norm();
// Converged "wanted" Ritz values
m_ritz_conv = (resid < thresh);
return m_ritz_conv.cast<Index>().sum();
}
// Returns the adjusted nev for restarting
Index nev_adjusted(Index nconv)
{
using std::abs;
Index nev_new = m_nev;
for (Index i = m_nev; i < m_ncv; i++)
if (abs(m_ritz_est[i]) < m_near_0)
nev_new++;
// Adjust nev_new, according to dsaup2.f line 677~684 in ARPACK
nev_new += std::min(nconv, (m_ncv - nev_new) / 2);
if (nev_new == 1 && m_ncv >= 6)
nev_new = m_ncv / 2;
else if (nev_new == 1 && m_ncv > 2)
nev_new = 2;
if (nev_new > m_ncv - 1)
nev_new = m_ncv - 1;
return nev_new;
}
// Retrieves and sorts Ritz values and Ritz vectors
void retrieve_ritzpair()
{
TridiagEigen<Scalar> decomp(m_fac.matrix_H());
const Vector& evals = decomp.eigenvalues();
const Matrix& evecs = decomp.eigenvectors();
SortEigenvalue<Scalar, SelectionRule> sorting(evals.data(), evals.size());
std::vector<int> ind = sorting.index();
// For BOTH_ENDS, the eigenvalues are sorted according
// to the LARGEST_ALGE rule, so we need to move those smallest
// values to the left
// The order would be
// Largest => Smallest => 2nd largest => 2nd smallest => ...
// We keep this order since the first k values will always be
// the wanted collection, no matter k is nev_updated (used in restart())
// or is nev (used in sort_ritzpair())
if (SelectionRule == BOTH_ENDS)
{
std::vector<int> ind_copy(ind);
for (Index i = 0; i < m_ncv; i++)
{
// If i is even, pick values from the left (large values)
// If i is odd, pick values from the right (small values)
if (i % 2 == 0)
ind[i] = ind_copy[i / 2];
else
ind[i] = ind_copy[m_ncv - 1 - i / 2];
}
}
// Copy the Ritz values and vectors to m_ritz_val and m_ritz_vec, respectively
for (Index i = 0; i < m_ncv; i++)
{
m_ritz_val[i] = evals[ind[i]];
m_ritz_est[i] = evecs(m_ncv - 1, ind[i]);
}
for (Index i = 0; i < m_nev; i++)
{
m_ritz_vec.col(i).noalias() = evecs.col(ind[i]);
}
}
protected:
// Sorts the first nev Ritz pairs in the specified order
// This is used to return the final results
virtual void sort_ritzpair(int sort_rule)
{
// First make sure that we have a valid index vector
SortEigenvalue<Scalar, LARGEST_ALGE> sorting(m_ritz_val.data(), m_nev);
std::vector<int> ind = sorting.index();
switch (sort_rule)
{
case LARGEST_ALGE:
break;
case LARGEST_MAGN:
{
SortEigenvalue<Scalar, LARGEST_MAGN> sorting(m_ritz_val.data(), m_nev);
ind = sorting.index();
break;
}
case SMALLEST_ALGE:
{
SortEigenvalue<Scalar, SMALLEST_ALGE> sorting(m_ritz_val.data(), m_nev);
ind = sorting.index();
break;
}
case SMALLEST_MAGN:
{
SortEigenvalue<Scalar, SMALLEST_MAGN> sorting(m_ritz_val.data(), m_nev);
ind = sorting.index();
break;
}
default:
throw std::invalid_argument("unsupported sorting rule");
}
Vector new_ritz_val(m_ncv);
Matrix new_ritz_vec(m_ncv, m_nev);
BoolArray new_ritz_conv(m_nev);
for (Index i = 0; i < m_nev; i++)
{
new_ritz_val[i] = m_ritz_val[ind[i]];
new_ritz_vec.col(i).noalias() = m_ritz_vec.col(ind[i]);
new_ritz_conv[i] = m_ritz_conv[ind[i]];
}
m_ritz_val.swap(new_ritz_val);
m_ritz_vec.swap(new_ritz_vec);
m_ritz_conv.swap(new_ritz_conv);
}
public:
/// \cond
SymEigsBase(OpType* op, BOpType* Bop, Index nev, Index ncv) :
m_op(op),
m_n(m_op->rows()),
m_nev(nev),
m_ncv(ncv > m_n ? m_n : ncv),
m_nmatop(0),
m_niter(0),
m_fac(ArnoldiOpType(op, Bop), m_ncv),
m_info(NOT_COMPUTED),
m_near_0(TypeTraits<Scalar>::min() * Scalar(10)),
m_eps(Eigen::NumTraits<Scalar>::epsilon()),
m_eps23(Eigen::numext::pow(m_eps, Scalar(2.0) / 3))
{
if (nev < 1 || nev > m_n - 1)
throw std::invalid_argument("nev must satisfy 1 <= nev <= n - 1, n is the size of matrix");
if (ncv <= nev || ncv > m_n)
throw std::invalid_argument("ncv must satisfy nev < ncv <= n, n is the size of matrix");
}
///
/// Virtual destructor
///
virtual ~SymEigsBase() {}
/// \endcond
///
/// Initializes the solver by providing an initial residual vector.
///
/// \param init_resid Pointer to the initial residual vector.
///
/// **Spectra** (and also **ARPACK**) uses an iterative algorithm
/// to find eigenvalues. This function allows the user to provide the initial
/// residual vector.
///
void init(const Scalar* init_resid)
{
// Reset all matrices/vectors to zero
m_ritz_val.resize(m_ncv);
m_ritz_vec.resize(m_ncv, m_nev);
m_ritz_est.resize(m_ncv);
m_ritz_conv.resize(m_nev);
m_ritz_val.setZero();
m_ritz_vec.setZero();
m_ritz_est.setZero();
m_ritz_conv.setZero();
m_nmatop = 0;
m_niter = 0;
// Initialize the Lanczos factorization
MapConstVec v0(init_resid, m_n);
m_fac.init(v0, m_nmatop);
}
///
/// Initializes the solver by providing a random initial residual vector.
///
/// This overloaded function generates a random initial residual vector
/// (with a fixed random seed) for the algorithm. Elements in the vector
/// follow independent Uniform(-0.5, 0.5) distribution.
///
void init()
{
SimpleRandom<Scalar> rng(0);
Vector init_resid = rng.random_vec(m_n);
init(init_resid.data());
}
///
/// Conducts the major computation procedure.
///
/// \param maxit Maximum number of iterations allowed in the algorithm.
/// \param tol Precision parameter for the calculated eigenvalues.
/// \param sort_rule Rule to sort the eigenvalues and eigenvectors.
/// Supported values are
/// `Spectra::LARGEST_ALGE`, `Spectra::LARGEST_MAGN`,
/// `Spectra::SMALLEST_ALGE` and `Spectra::SMALLEST_MAGN`,
/// for example `LARGEST_ALGE` indicates that largest eigenvalues
/// come first. Note that this argument is only used to
/// **sort** the final result, and the **selection** rule
/// (e.g. selecting the largest or smallest eigenvalues in the
/// full spectrum) is specified by the template parameter
/// `SelectionRule` of SymEigsSolver.
///
/// \return Number of converged eigenvalues.
///
Index compute(Index maxit = 1000, Scalar tol = 1e-10, int sort_rule = LARGEST_ALGE)
{
// The m-step Lanczos factorization
m_fac.factorize_from(1, m_ncv, m_nmatop);
retrieve_ritzpair();
// Restarting
Index i, nconv = 0, nev_adj;
for (i = 0; i < maxit; i++)
{
nconv = num_converged(tol);
if (nconv >= m_nev)
break;
nev_adj = nev_adjusted(nconv);
restart(nev_adj);
}
// Sorting results
sort_ritzpair(sort_rule);
m_niter += i + 1;
m_info = (nconv >= m_nev) ? SUCCESSFUL : NOT_CONVERGING;
return std::min(m_nev, nconv);
}
///
/// Returns the status of the computation.
/// The full list of enumeration values can be found in \ref Enumerations.
///
int info() const { return m_info; }
///
/// Returns the number of iterations used in the computation.
///
Index num_iterations() const { return m_niter; }
///
/// Returns the number of matrix operations used in the computation.
///
Index num_operations() const { return m_nmatop; }
///
/// Returns the converged eigenvalues.
///
/// \return A vector containing the eigenvalues.
/// Returned vector type will be `Eigen::Vector<Scalar, ...>`, depending on
/// the template parameter `Scalar` defined.
///
Vector eigenvalues() const
{
const Index nconv = m_ritz_conv.cast<Index>().sum();
Vector res(nconv);
if (!nconv)
return res;
Index j = 0;
for (Index i = 0; i < m_nev; i++)
{
if (m_ritz_conv[i])
{
res[j] = m_ritz_val[i];
j++;
}
}
return res;
}
///
/// Returns the eigenvectors associated with the converged eigenvalues.
///
/// \param nvec The number of eigenvectors to return.
///
/// \return A matrix containing the eigenvectors.
/// Returned matrix type will be `Eigen::Matrix<Scalar, ...>`,
/// depending on the template parameter `Scalar` defined.
///
virtual Matrix eigenvectors(Index nvec) const
{
const Index nconv = m_ritz_conv.cast<Index>().sum();
nvec = std::min(nvec, nconv);
Matrix res(m_n, nvec);
if (!nvec)
return res;
Matrix ritz_vec_conv(m_ncv, nvec);
Index j = 0;
for (Index i = 0; i < m_nev && j < nvec; i++)
{
if (m_ritz_conv[i])
{
ritz_vec_conv.col(j).noalias() = m_ritz_vec.col(i);
j++;
}
}
res.noalias() = m_fac.matrix_V() * ritz_vec_conv;
return res;
}
///
/// Returns all converged eigenvectors.
///
virtual Matrix eigenvectors() const
{
return eigenvectors(m_nev);
}
};
} // namespace Spectra
#endif // SYM_EIGS_BASE_H

203
gtsam/3rdparty/Spectra/SymEigsShiftSolver.h vendored Normal file
View File

@ -0,0 +1,203 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SYM_EIGS_SHIFT_SOLVER_H
#define SYM_EIGS_SHIFT_SOLVER_H
#include <Eigen/Core>
#include "SymEigsBase.h"
#include "Util/SelectionRule.h"
#include "MatOp/DenseSymShiftSolve.h"
namespace Spectra {
///
/// \ingroup EigenSolver
///
/// This class implements the eigen solver for real symmetric matrices using
/// the **shift-and-invert mode**. The background information of the symmetric
/// eigen solver is documented in the SymEigsSolver class. Here we focus on
/// explaining the shift-and-invert mode.
///
/// The shift-and-invert mode is based on the following fact:
/// If \f$\lambda\f$ and \f$x\f$ are a pair of eigenvalue and eigenvector of
/// matrix \f$A\f$, such that \f$Ax=\lambda x\f$, then for any \f$\sigma\f$,
/// we have
/// \f[(A-\sigma I)^{-1}x=\nu x\f]
/// where
/// \f[\nu=\frac{1}{\lambda-\sigma}\f]
/// which indicates that \f$(\nu, x)\f$ is an eigenpair of the matrix
/// \f$(A-\sigma I)^{-1}\f$.
///
/// Therefore, if we pass the matrix operation \f$(A-\sigma I)^{-1}y\f$
/// (rather than \f$Ay\f$) to the eigen solver, then we would get the desired
/// values of \f$\nu\f$, and \f$\lambda\f$ can also be easily obtained by noting
/// that \f$\lambda=\sigma+\nu^{-1}\f$.
///
/// The reason why we need this type of manipulation is that
/// the algorithm of **Spectra** (and also **ARPACK**)
/// is good at finding eigenvalues with large magnitude, but may fail in looking
/// for eigenvalues that are close to zero. However, if we really need them, we
/// can set \f$\sigma=0\f$, find the largest eigenvalues of \f$A^{-1}\f$, and then
/// transform back to \f$\lambda\f$, since in this case largest values of \f$\nu\f$
/// implies smallest values of \f$\lambda\f$.
///
/// To summarize, in the shift-and-invert mode, the selection rule will apply to
/// \f$\nu=1/(\lambda-\sigma)\f$ rather than \f$\lambda\f$. So a selection rule
/// of `LARGEST_MAGN` combined with shift \f$\sigma\f$ will find eigenvalues of
/// \f$A\f$ that are closest to \f$\sigma\f$. But note that the eigenvalues()
/// method will always return the eigenvalues in the original problem (i.e.,
/// returning \f$\lambda\f$ rather than \f$\nu\f$), and eigenvectors are the
/// same for both the original problem and the shifted-and-inverted problem.
///
/// \tparam Scalar The element type of the matrix.
/// Currently supported types are `float`, `double` and `long double`.
/// \tparam SelectionRule An enumeration value indicating the selection rule of
/// the shifted-and-inverted eigenvalues.
/// The full list of enumeration values can be found in
/// \ref Enumerations.
/// \tparam OpType The name of the matrix operation class. Users could either
/// use the wrapper classes such as DenseSymShiftSolve and
/// SparseSymShiftSolve, or define their
/// own that implements all the public member functions as in
/// DenseSymShiftSolve.
///
/// Below is an example that illustrates the use of the shift-and-invert mode:
///
/// \code{.cpp}
/// #include <Eigen/Core>
/// #include <Spectra/SymEigsShiftSolver.h>
/// // <Spectra/MatOp/DenseSymShiftSolve.h> is implicitly included
/// #include <iostream>
///
/// using namespace Spectra;
///
/// int main()
/// {
/// // A size-10 diagonal matrix with elements 1, 2, ..., 10
/// Eigen::MatrixXd M = Eigen::MatrixXd::Zero(10, 10);
/// for(int i = 0; i < M.rows(); i++)
/// M(i, i) = i + 1;
///
/// // Construct matrix operation object using the wrapper class
/// DenseSymShiftSolve<double> op(M);
///
/// // Construct eigen solver object with shift 0
/// // This will find eigenvalues that are closest to 0
/// SymEigsShiftSolver< double, LARGEST_MAGN,
/// DenseSymShiftSolve<double> > eigs(&op, 3, 6, 0.0);
///
/// eigs.init();
/// eigs.compute();
/// if(eigs.info() == SUCCESSFUL)
/// {
/// Eigen::VectorXd evalues = eigs.eigenvalues();
/// // Will get (3.0, 2.0, 1.0)
/// std::cout << "Eigenvalues found:\n" << evalues << std::endl;
/// }
///
/// return 0;
/// }
/// \endcode
///
/// Also an example for user-supplied matrix shift-solve operation class:
///
/// \code{.cpp}
/// #include <Eigen/Core>
/// #include <Spectra/SymEigsShiftSolver.h>
/// #include <iostream>
///
/// using namespace Spectra;
///
/// // M = diag(1, 2, ..., 10)
/// class MyDiagonalTenShiftSolve
/// {
/// private:
/// double sigma_;
/// public:
/// int rows() { return 10; }
/// int cols() { return 10; }
/// void set_shift(double sigma) { sigma_ = sigma; }
/// // y_out = inv(A - sigma * I) * x_in
/// // inv(A - sigma * I) = diag(1/(1-sigma), 1/(2-sigma), ...)
/// void perform_op(double *x_in, double *y_out)
/// {
/// for(int i = 0; i < rows(); i++)
/// {
/// y_out[i] = x_in[i] / (i + 1 - sigma_);
/// }
/// }
/// };
///
/// int main()
/// {
/// MyDiagonalTenShiftSolve op;
/// // Find three eigenvalues that are closest to 3.14
/// SymEigsShiftSolver<double, LARGEST_MAGN,
/// MyDiagonalTenShiftSolve> eigs(&op, 3, 6, 3.14);
/// eigs.init();
/// eigs.compute();
/// if(eigs.info() == SUCCESSFUL)
/// {
/// Eigen::VectorXd evalues = eigs.eigenvalues();
/// // Will get (4.0, 3.0, 2.0)
/// std::cout << "Eigenvalues found:\n" << evalues << std::endl;
/// }
///
/// return 0;
/// }
/// \endcode
///
template <typename Scalar = double,
int SelectionRule = LARGEST_MAGN,
typename OpType = DenseSymShiftSolve<double> >
class SymEigsShiftSolver : public SymEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>
{
private:
typedef Eigen::Index Index;
typedef Eigen::Array<Scalar, Eigen::Dynamic, 1> Array;
const Scalar m_sigma;
// First transform back the Ritz values, and then sort
void sort_ritzpair(int sort_rule)
{
Array m_ritz_val_org = Scalar(1.0) / this->m_ritz_val.head(this->m_nev).array() + m_sigma;
this->m_ritz_val.head(this->m_nev) = m_ritz_val_org;
SymEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>::sort_ritzpair(sort_rule);
}
public:
///
/// Constructor to create a eigen solver object using the shift-and-invert mode.
///
/// \param op Pointer to the matrix operation object, which should implement
/// the shift-solve operation of \f$A\f$: calculating
/// \f$(A-\sigma I)^{-1}v\f$ for any vector \f$v\f$. Users could either
/// create the object from the wrapper class such as DenseSymShiftSolve, or
/// define their own that implements all the public member functions
/// as in DenseSymShiftSolve.
/// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$,
/// where \f$n\f$ is the size of matrix.
/// \param ncv Parameter that controls the convergence speed of the algorithm.
/// Typically a larger `ncv_` means faster convergence, but it may
/// also result in greater memory use and more matrix operations
/// in each iteration. This parameter must satisfy \f$nev < ncv \le n\f$,
/// and is advised to take \f$ncv \ge 2\cdot nev\f$.
/// \param sigma The value of the shift.
///
SymEigsShiftSolver(OpType* op, Index nev, Index ncv, Scalar sigma) :
SymEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>(op, NULL, nev, ncv),
m_sigma(sigma)
{
this->m_op->set_shift(m_sigma);
}
};
} // namespace Spectra
#endif // SYM_EIGS_SHIFT_SOLVER_H

171
gtsam/3rdparty/Spectra/SymEigsSolver.h vendored Normal file
View File

@ -0,0 +1,171 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SYM_EIGS_SOLVER_H
#define SYM_EIGS_SOLVER_H
#include <Eigen/Core>
#include "SymEigsBase.h"
#include "Util/SelectionRule.h"
#include "MatOp/DenseSymMatProd.h"
namespace Spectra {
///
/// \ingroup EigenSolver
///
/// This class implements the eigen solver for real symmetric matrices, i.e.,
/// to solve \f$Ax=\lambda x\f$ where \f$A\f$ is symmetric.
///
/// **Spectra** is designed to calculate a specified number (\f$k\f$)
/// of eigenvalues of a large square matrix (\f$A\f$). Usually \f$k\f$ is much
/// less than the size of the matrix (\f$n\f$), so that only a few eigenvalues
/// and eigenvectors are computed.
///
/// Rather than providing the whole \f$A\f$ matrix, the algorithm only requires
/// the matrix-vector multiplication operation of \f$A\f$. Therefore, users of
/// this solver need to supply a class that computes the result of \f$Av\f$
/// for any given vector \f$v\f$. The name of this class should be given to
/// the template parameter `OpType`, and instance of this class passed to
/// the constructor of SymEigsSolver.
///
/// If the matrix \f$A\f$ is already stored as a matrix object in **Eigen**,
/// for example `Eigen::MatrixXd`, then there is an easy way to construct such
/// matrix operation class, by using the built-in wrapper class DenseSymMatProd
/// which wraps an existing matrix object in **Eigen**. This is also the
/// default template parameter for SymEigsSolver. For sparse matrices, the
/// wrapper class SparseSymMatProd can be used similarly.
///
/// If the users need to define their own matrix-vector multiplication operation
/// class, it should implement all the public member functions as in DenseSymMatProd.
///
/// \tparam Scalar The element type of the matrix.
/// Currently supported types are `float`, `double` and `long double`.
/// \tparam SelectionRule An enumeration value indicating the selection rule of
/// the requested eigenvalues, for example `LARGEST_MAGN`
/// to retrieve eigenvalues with the largest magnitude.
/// The full list of enumeration values can be found in
/// \ref Enumerations.
/// \tparam OpType The name of the matrix operation class. Users could either
/// use the wrapper classes such as DenseSymMatProd and
/// SparseSymMatProd, or define their
/// own that implements all the public member functions as in
/// DenseSymMatProd.
///
/// Below is an example that demonstrates the usage of this class.
///
/// \code{.cpp}
/// #include <Eigen/Core>
/// #include <Spectra/SymEigsSolver.h>
/// // <Spectra/MatOp/DenseSymMatProd.h> is implicitly included
/// #include <iostream>
///
/// using namespace Spectra;
///
/// int main()
/// {
/// // We are going to calculate the eigenvalues of M
/// Eigen::MatrixXd A = Eigen::MatrixXd::Random(10, 10);
/// Eigen::MatrixXd M = A + A.transpose();
///
/// // Construct matrix operation object using the wrapper class DenseSymMatProd
/// DenseSymMatProd<double> op(M);
///
/// // Construct eigen solver object, requesting the largest three eigenvalues
/// SymEigsSolver< double, LARGEST_ALGE, DenseSymMatProd<double> > eigs(&op, 3, 6);
///
/// // Initialize and compute
/// eigs.init();
/// int nconv = eigs.compute();
///
/// // Retrieve results
/// Eigen::VectorXd evalues;
/// if(eigs.info() == SUCCESSFUL)
/// evalues = eigs.eigenvalues();
///
/// std::cout << "Eigenvalues found:\n" << evalues << std::endl;
///
/// return 0;
/// }
/// \endcode
///
/// And here is an example for user-supplied matrix operation class.
///
/// \code{.cpp}
/// #include <Eigen/Core>
/// #include <Spectra/SymEigsSolver.h>
/// #include <iostream>
///
/// using namespace Spectra;
///
/// // M = diag(1, 2, ..., 10)
/// class MyDiagonalTen
/// {
/// public:
/// int rows() { return 10; }
/// int cols() { return 10; }
/// // y_out = M * x_in
/// void perform_op(double *x_in, double *y_out)
/// {
/// for(int i = 0; i < rows(); i++)
/// {
/// y_out[i] = x_in[i] * (i + 1);
/// }
/// }
/// };
///
/// int main()
/// {
/// MyDiagonalTen op;
/// SymEigsSolver<double, LARGEST_ALGE, MyDiagonalTen> eigs(&op, 3, 6);
/// eigs.init();
/// eigs.compute();
/// if(eigs.info() == SUCCESSFUL)
/// {
/// Eigen::VectorXd evalues = eigs.eigenvalues();
/// // Will get (10, 9, 8)
/// std::cout << "Eigenvalues found:\n" << evalues << std::endl;
/// }
///
/// return 0;
/// }
/// \endcode
///
template <typename Scalar = double,
int SelectionRule = LARGEST_MAGN,
typename OpType = DenseSymMatProd<double> >
class SymEigsSolver : public SymEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>
{
private:
typedef Eigen::Index Index;
public:
///
/// Constructor to create a solver object.
///
/// \param op Pointer to the matrix operation object, which should implement
/// the matrix-vector multiplication operation of \f$A\f$:
/// calculating \f$Av\f$ for any vector \f$v\f$. Users could either
/// create the object from the wrapper class such as DenseSymMatProd, or
/// define their own that implements all the public member functions
/// as in DenseSymMatProd.
/// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$,
/// where \f$n\f$ is the size of matrix.
/// \param ncv Parameter that controls the convergence speed of the algorithm.
/// Typically a larger `ncv` means faster convergence, but it may
/// also result in greater memory use and more matrix operations
/// in each iteration. This parameter must satisfy \f$nev < ncv \le n\f$,
/// and is advised to take \f$ncv \ge 2\cdot nev\f$.
///
SymEigsSolver(OpType* op, Index nev, Index ncv) :
SymEigsBase<Scalar, SelectionRule, OpType, IdentityBOp>(op, NULL, nev, ncv)
{}
};
} // namespace Spectra
#endif // SYM_EIGS_SOLVER_H

326
gtsam/3rdparty/Spectra/SymGEigsSolver.h vendored Normal file
View File

@ -0,0 +1,326 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SYM_GEIGS_SOLVER_H
#define SYM_GEIGS_SOLVER_H
#include "SymEigsBase.h"
#include "Util/GEigsMode.h"
#include "MatOp/internal/SymGEigsCholeskyOp.h"
#include "MatOp/internal/SymGEigsRegInvOp.h"
namespace Spectra {
///
/// \defgroup GEigenSolver Generalized Eigen Solvers
///
/// Generalized eigen solvers for different types of problems.
///
///
/// \ingroup GEigenSolver
///
/// This class implements the generalized eigen solver for real symmetric
/// matrices, i.e., to solve \f$Ax=\lambda Bx\f$ where \f$A\f$ is symmetric and
/// \f$B\f$ is positive definite.
///
/// There are two modes of this solver, specified by the template parameter
/// GEigsMode. See the pages for the specialized classes for details.
/// - The Cholesky mode assumes that \f$B\f$ can be factorized using Cholesky
/// decomposition, which is the preferred mode when the decomposition is
/// available. (This can be easily done in Eigen using the dense or sparse
/// Cholesky solver.)
/// See \ref SymGEigsSolver<Scalar, SelectionRule, OpType, BOpType, GEIGS_CHOLESKY> "SymGEigsSolver (Cholesky mode)" for more details.
/// - The regular inverse mode requires the matrix-vector product \f$Bv\f$ and the
/// linear equation solving operation \f$B^{-1}v\f$. This mode should only be
/// used when the Cholesky decomposition of \f$B\f$ is hard to implement, or
/// when computing \f$B^{-1}v\f$ is much faster than the Cholesky decomposition.
/// See \ref SymGEigsSolver<Scalar, SelectionRule, OpType, BOpType, GEIGS_REGULAR_INVERSE> "SymGEigsSolver (Regular inverse mode)" for more details.
// Empty class template
template <typename Scalar,
int SelectionRule,
typename OpType,
typename BOpType,
int GEigsMode>
class SymGEigsSolver
{};
///
/// \ingroup GEigenSolver
///
/// This class implements the generalized eigen solver for real symmetric
/// matrices using Cholesky decomposition, i.e., to solve \f$Ax=\lambda Bx\f$
/// where \f$A\f$ is symmetric and \f$B\f$ is positive definite with the Cholesky
/// decomposition \f$B=LL'\f$.
///
/// This solver requires two matrix operation objects: one for \f$A\f$ that implements
/// the matrix multiplication \f$Av\f$, and one for \f$B\f$ that implements the lower
/// and upper triangular solving \f$L^{-1}v\f$ and \f$(L')^{-1}v\f$.
///
/// If \f$A\f$ and \f$B\f$ are stored as Eigen matrices, then the first operation
/// can be created using the DenseSymMatProd or SparseSymMatProd classes, and
/// the second operation can be created using the DenseCholesky or SparseCholesky
/// classes. If the users need to define their own operation classes, then they
/// should implement all the public member functions as in those built-in classes.
///
/// \tparam Scalar The element type of the matrix.
/// Currently supported types are `float`, `double` and `long double`.
/// \tparam SelectionRule An enumeration value indicating the selection rule of
/// the requested eigenvalues, for example `LARGEST_MAGN`
/// to retrieve eigenvalues with the largest magnitude.
/// The full list of enumeration values can be found in
/// \ref Enumerations.
/// \tparam OpType The name of the matrix operation class for \f$A\f$. Users could either
/// use the wrapper classes such as DenseSymMatProd and
/// SparseSymMatProd, or define their
/// own that implements all the public member functions as in
/// DenseSymMatProd.
/// \tparam BOpType The name of the matrix operation class for \f$B\f$. Users could either
/// use the wrapper classes such as DenseCholesky and
/// SparseCholesky, or define their
/// own that implements all the public member functions as in
/// DenseCholesky.
/// \tparam GEigsMode Mode of the generalized eigen solver. In this solver
/// it is Spectra::GEIGS_CHOLESKY.
///
/// Below is an example that demonstrates the usage of this class.
///
/// \code{.cpp}
/// #include <Eigen/Core>
/// #include <Eigen/SparseCore>
/// #include <Eigen/Eigenvalues>
/// #include <Spectra/SymGEigsSolver.h>
/// #include <Spectra/MatOp/DenseSymMatProd.h>
/// #include <Spectra/MatOp/SparseCholesky.h>
/// #include <iostream>
///
/// using namespace Spectra;
///
/// int main()
/// {
/// // We are going to solve the generalized eigenvalue problem A * x = lambda * B * x
/// const int n = 100;
///
/// // Define the A matrix
/// Eigen::MatrixXd M = Eigen::MatrixXd::Random(n, n);
/// Eigen::MatrixXd A = M + M.transpose();
///
/// // Define the B matrix, a band matrix with 2 on the diagonal and 1 on the subdiagonals
/// Eigen::SparseMatrix<double> B(n, n);
/// B.reserve(Eigen::VectorXi::Constant(n, 3));
/// for(int i = 0; i < n; i++)
/// {
/// B.insert(i, i) = 2.0;
/// if(i > 0)
/// B.insert(i - 1, i) = 1.0;
/// if(i < n - 1)
/// B.insert(i + 1, i) = 1.0;
/// }
///
/// // Construct matrix operation object using the wrapper classes
/// DenseSymMatProd<double> op(A);
/// SparseCholesky<double> Bop(B);
///
/// // Construct generalized eigen solver object, requesting the largest three generalized eigenvalues
/// SymGEigsSolver<double, LARGEST_ALGE, DenseSymMatProd<double>, SparseCholesky<double>, GEIGS_CHOLESKY>
/// geigs(&op, &Bop, 3, 6);
///
/// // Initialize and compute
/// geigs.init();
/// int nconv = geigs.compute();
///
/// // Retrieve results
/// Eigen::VectorXd evalues;
/// Eigen::MatrixXd evecs;
/// if(geigs.info() == SUCCESSFUL)
/// {
/// evalues = geigs.eigenvalues();
/// evecs = geigs.eigenvectors();
/// }
///
/// std::cout << "Generalized eigenvalues found:\n" << evalues << std::endl;
/// std::cout << "Generalized eigenvectors found:\n" << evecs.topRows(10) << std::endl;
///
/// // Verify results using the generalized eigen solver in Eigen
/// Eigen::MatrixXd Bdense = B;
/// Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es(A, Bdense);
///
/// std::cout << "Generalized eigenvalues:\n" << es.eigenvalues().tail(3) << std::endl;
/// std::cout << "Generalized eigenvectors:\n" << es.eigenvectors().rightCols(3).topRows(10) << std::endl;
///
/// return 0;
/// }
/// \endcode
// Partial specialization for GEigsMode = GEIGS_CHOLESKY
template <typename Scalar,
int SelectionRule,
typename OpType,
typename BOpType>
class SymGEigsSolver<Scalar, SelectionRule, OpType, BOpType, GEIGS_CHOLESKY> :
public SymEigsBase<Scalar, SelectionRule, SymGEigsCholeskyOp<Scalar, OpType, BOpType>, IdentityBOp>
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
BOpType* m_Bop;
public:
///
/// Constructor to create a solver object.
///
/// \param op Pointer to the \f$A\f$ matrix operation object. It
/// should implement the matrix-vector multiplication operation of \f$A\f$:
/// calculating \f$Av\f$ for any vector \f$v\f$. Users could either
/// create the object from the wrapper classes such as DenseSymMatProd, or
/// define their own that implements all the public member functions
/// as in DenseSymMatProd.
/// \param Bop Pointer to the \f$B\f$ matrix operation object. It
/// represents a Cholesky decomposition of \f$B\f$, and should
/// implement the lower and upper triangular solving operations:
/// calculating \f$L^{-1}v\f$ and \f$(L')^{-1}v\f$ for any vector
/// \f$v\f$, where \f$LL'=B\f$. Users could either
/// create the object from the wrapper classes such as DenseCholesky, or
/// define their own that implements all the public member functions
/// as in DenseCholesky.
/// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$,
/// where \f$n\f$ is the size of matrix.
/// \param ncv Parameter that controls the convergence speed of the algorithm.
/// Typically a larger `ncv` means faster convergence, but it may
/// also result in greater memory use and more matrix operations
/// in each iteration. This parameter must satisfy \f$nev < ncv \le n\f$,
/// and is advised to take \f$ncv \ge 2\cdot nev\f$.
///
SymGEigsSolver(OpType* op, BOpType* Bop, Index nev, Index ncv) :
SymEigsBase<Scalar, SelectionRule, SymGEigsCholeskyOp<Scalar, OpType, BOpType>, IdentityBOp>(
new SymGEigsCholeskyOp<Scalar, OpType, BOpType>(*op, *Bop), NULL, nev, ncv),
m_Bop(Bop)
{}
/// \cond
~SymGEigsSolver()
{
// m_op contains the constructed SymGEigsCholeskyOp object
delete this->m_op;
}
Matrix eigenvectors(Index nvec) const
{
Matrix res = SymEigsBase<Scalar, SelectionRule, SymGEigsCholeskyOp<Scalar, OpType, BOpType>, IdentityBOp>::eigenvectors(nvec);
Vector tmp(res.rows());
const Index nconv = res.cols();
for (Index i = 0; i < nconv; i++)
{
m_Bop->upper_triangular_solve(&res(0, i), tmp.data());
res.col(i).noalias() = tmp;
}
return res;
}
Matrix eigenvectors() const
{
return SymGEigsSolver<Scalar, SelectionRule, OpType, BOpType, GEIGS_CHOLESKY>::eigenvectors(this->m_nev);
}
/// \endcond
};
///
/// \ingroup GEigenSolver
///
/// This class implements the generalized eigen solver for real symmetric
/// matrices in the regular inverse mode, i.e., to solve \f$Ax=\lambda Bx\f$
/// where \f$A\f$ is symmetric, and \f$B\f$ is positive definite with the operations
/// defined below.
///
/// This solver requires two matrix operation objects: one for \f$A\f$ that implements
/// the matrix multiplication \f$Av\f$, and one for \f$B\f$ that implements the
/// matrix-vector product \f$Bv\f$ and the linear equation solving operation \f$B^{-1}v\f$.
///
/// If \f$A\f$ and \f$B\f$ are stored as Eigen matrices, then the first operation
/// can be created using the DenseSymMatProd or SparseSymMatProd classes, and
/// the second operation can be created using the SparseRegularInverse class. There is no
/// wrapper class for a dense \f$B\f$ matrix since in this case the Cholesky mode
/// is always preferred. If the users need to define their own operation classes, then they
/// should implement all the public member functions as in those built-in classes.
///
/// \tparam Scalar The element type of the matrix.
/// Currently supported types are `float`, `double` and `long double`.
/// \tparam SelectionRule An enumeration value indicating the selection rule of
/// the requested eigenvalues, for example `LARGEST_MAGN`
/// to retrieve eigenvalues with the largest magnitude.
/// The full list of enumeration values can be found in
/// \ref Enumerations.
/// \tparam OpType The name of the matrix operation class for \f$A\f$. Users could either
/// use the wrapper classes such as DenseSymMatProd and
/// SparseSymMatProd, or define their
/// own that implements all the public member functions as in
/// DenseSymMatProd.
/// \tparam BOpType The name of the matrix operation class for \f$B\f$. Users could either
/// use the wrapper class SparseRegularInverse, or define their
/// own that implements all the public member functions as in
/// SparseRegularInverse.
/// \tparam GEigsMode Mode of the generalized eigen solver. In this solver
/// it is Spectra::GEIGS_REGULAR_INVERSE.
///
// Partial specialization for GEigsMode = GEIGS_REGULAR_INVERSE
template <typename Scalar,
int SelectionRule,
typename OpType,
typename BOpType>
class SymGEigsSolver<Scalar, SelectionRule, OpType, BOpType, GEIGS_REGULAR_INVERSE> :
public SymEigsBase<Scalar, SelectionRule, SymGEigsRegInvOp<Scalar, OpType, BOpType>, BOpType>
{
private:
typedef Eigen::Index Index;
public:
///
/// Constructor to create a solver object.
///
/// \param op Pointer to the \f$A\f$ matrix operation object. It
/// should implement the matrix-vector multiplication operation of \f$A\f$:
/// calculating \f$Av\f$ for any vector \f$v\f$. Users could either
/// create the object from the wrapper classes such as DenseSymMatProd, or
/// define their own that implements all the public member functions
/// as in DenseSymMatProd.
/// \param Bop Pointer to the \f$B\f$ matrix operation object. It should
/// implement the multiplication operation \f$Bv\f$ and the linear equation
/// solving operation \f$B^{-1}v\f$ for any vector \f$v\f$. Users could either
/// create the object from the wrapper class SparseRegularInverse, or
/// define their own that implements all the public member functions
/// as in SparseRegularInverse.
/// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$,
/// where \f$n\f$ is the size of matrix.
/// \param ncv Parameter that controls the convergence speed of the algorithm.
/// Typically a larger `ncv` means faster convergence, but it may
/// also result in greater memory use and more matrix operations
/// in each iteration. This parameter must satisfy \f$nev < ncv \le n\f$,
/// and is advised to take \f$ncv \ge 2\cdot nev\f$.
///
SymGEigsSolver(OpType* op, BOpType* Bop, Index nev, Index ncv) :
SymEigsBase<Scalar, SelectionRule, SymGEigsRegInvOp<Scalar, OpType, BOpType>, BOpType>(
new SymGEigsRegInvOp<Scalar, OpType, BOpType>(*op, *Bop), Bop, nev, ncv)
{}
/// \cond
~SymGEigsSolver()
{
// m_op contains the constructed SymGEigsRegInvOp object
delete this->m_op;
}
/// \endcond
};
} // namespace Spectra
#endif // SYM_GEIGS_SOLVER_H

35
gtsam/3rdparty/Spectra/Util/CompInfo.h vendored Normal file
View File

@ -0,0 +1,35 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef COMP_INFO_H
#define COMP_INFO_H
namespace Spectra {
///
/// \ingroup Enumerations
///
/// The enumeration to report the status of computation.
///
enum COMPUTATION_INFO
{
SUCCESSFUL = 0, ///< Computation was successful.
NOT_COMPUTED, ///< Used in eigen solvers, indicating that computation
///< has not been conducted. Users should call
///< the `compute()` member function of solvers.
NOT_CONVERGING, ///< Used in eigen solvers, indicating that some eigenvalues
///< did not converge. The `compute()`
///< function returns the number of converged eigenvalues.
NUMERICAL_ISSUE ///< Used in Cholesky decomposition, indicating that the
///< matrix is not positive definite.
};
} // namespace Spectra
#endif // COMP_INFO_H

32
gtsam/3rdparty/Spectra/Util/GEigsMode.h vendored Normal file
View File

@ -0,0 +1,32 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef GEIGS_MODE_H
#define GEIGS_MODE_H
namespace Spectra {
///
/// \ingroup Enumerations
///
/// The enumeration to specify the mode of generalized eigenvalue solver.
///
enum GEIGS_MODE
{
GEIGS_CHOLESKY = 0, ///< Using Cholesky decomposition to solve generalized eigenvalues.
GEIGS_REGULAR_INVERSE, ///< Regular inverse mode for generalized eigenvalue solver.
GEIGS_SHIFT_INVERT, ///< Shift-and-invert mode for generalized eigenvalue solver.
GEIGS_BUCKLING, ///< Buckling mode for generalized eigenvalue solver.
GEIGS_CAYLEY ///< Cayley transformation mode for generalized eigenvalue solver.
};
} // namespace Spectra
#endif // GEIGS_MODE_H

275
gtsam/3rdparty/Spectra/Util/SelectionRule.h vendored Normal file
View File

@ -0,0 +1,275 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SELECTION_RULE_H
#define SELECTION_RULE_H
#include <vector> // std::vector
#include <cmath> // std::abs
#include <algorithm> // std::sort
#include <complex> // std::complex
#include <utility> // std::pair
#include <stdexcept> // std::invalid_argument
namespace Spectra {
///
/// \defgroup Enumerations
///
/// Enumeration types for the selection rule of eigenvalues.
///
///
/// \ingroup Enumerations
///
/// The enumeration of selection rules of desired eigenvalues.
///
enum SELECT_EIGENVALUE
{
LARGEST_MAGN = 0, ///< Select eigenvalues with largest magnitude. Magnitude
///< means the absolute value for real numbers and norm for
///< complex numbers. Applies to both symmetric and general
///< eigen solvers.
LARGEST_REAL, ///< Select eigenvalues with largest real part. Only for general eigen solvers.
LARGEST_IMAG, ///< Select eigenvalues with largest imaginary part (in magnitude). Only for general eigen solvers.
LARGEST_ALGE, ///< Select eigenvalues with largest algebraic value, considering
///< any negative sign. Only for symmetric eigen solvers.
SMALLEST_MAGN, ///< Select eigenvalues with smallest magnitude. Applies to both symmetric and general
///< eigen solvers.
SMALLEST_REAL, ///< Select eigenvalues with smallest real part. Only for general eigen solvers.
SMALLEST_IMAG, ///< Select eigenvalues with smallest imaginary part (in magnitude). Only for general eigen solvers.
SMALLEST_ALGE, ///< Select eigenvalues with smallest algebraic value. Only for symmetric eigen solvers.
BOTH_ENDS ///< Select eigenvalues half from each end of the spectrum. When
///< `nev` is odd, compute more from the high end. Only for symmetric eigen solvers.
};
///
/// \ingroup Enumerations
///
/// The enumeration of selection rules of desired eigenvalues. Alias for `SELECT_EIGENVALUE`.
///
enum SELECT_EIGENVALUE_ALIAS
{
WHICH_LM = 0, ///< Alias for `LARGEST_MAGN`
WHICH_LR, ///< Alias for `LARGEST_REAL`
WHICH_LI, ///< Alias for `LARGEST_IMAG`
WHICH_LA, ///< Alias for `LARGEST_ALGE`
WHICH_SM, ///< Alias for `SMALLEST_MAGN`
WHICH_SR, ///< Alias for `SMALLEST_REAL`
WHICH_SI, ///< Alias for `SMALLEST_IMAG`
WHICH_SA, ///< Alias for `SMALLEST_ALGE`
WHICH_BE ///< Alias for `BOTH_ENDS`
};
/// \cond
// Get the element type of a "scalar"
// ElemType<double> => double
// ElemType< std::complex<double> > => double
template <typename T>
class ElemType
{
public:
typedef T type;
};
template <typename T>
class ElemType<std::complex<T> >
{
public:
typedef T type;
};
// When comparing eigenvalues, we first calculate the "target"
// to sort. For example, if we want to choose the eigenvalues with
// largest magnitude, the target will be -abs(x).
// The minus sign is due to the fact that std::sort() sorts in ascending order.
// Default target: throw an exception
template <typename Scalar, int SelectionRule>
class SortingTarget
{
public:
static typename ElemType<Scalar>::type get(const Scalar& val)
{
using std::abs;
throw std::invalid_argument("incompatible selection rule");
return -abs(val);
}
};
// Specialization for LARGEST_MAGN
// This covers [float, double, complex] x [LARGEST_MAGN]
template <typename Scalar>
class SortingTarget<Scalar, LARGEST_MAGN>
{
public:
static typename ElemType<Scalar>::type get(const Scalar& val)
{
using std::abs;
return -abs(val);
}
};
// Specialization for LARGEST_REAL
// This covers [complex] x [LARGEST_REAL]
template <typename RealType>
class SortingTarget<std::complex<RealType>, LARGEST_REAL>
{
public:
static RealType get(const std::complex<RealType>& val)
{
return -val.real();
}
};
// Specialization for LARGEST_IMAG
// This covers [complex] x [LARGEST_IMAG]
template <typename RealType>
class SortingTarget<std::complex<RealType>, LARGEST_IMAG>
{
public:
static RealType get(const std::complex<RealType>& val)
{
using std::abs;
return -abs(val.imag());
}
};
// Specialization for LARGEST_ALGE
// This covers [float, double] x [LARGEST_ALGE]
template <typename Scalar>
class SortingTarget<Scalar, LARGEST_ALGE>
{
public:
static Scalar get(const Scalar& val)
{
return -val;
}
};
// Here BOTH_ENDS is the same as LARGEST_ALGE, but
// we need some additional steps, which are done in
// SymEigsSolver.h => retrieve_ritzpair().
// There we move the smallest values to the proper locations.
template <typename Scalar>
class SortingTarget<Scalar, BOTH_ENDS>
{
public:
static Scalar get(const Scalar& val)
{
return -val;
}
};
// Specialization for SMALLEST_MAGN
// This covers [float, double, complex] x [SMALLEST_MAGN]
template <typename Scalar>
class SortingTarget<Scalar, SMALLEST_MAGN>
{
public:
static typename ElemType<Scalar>::type get(const Scalar& val)
{
using std::abs;
return abs(val);
}
};
// Specialization for SMALLEST_REAL
// This covers [complex] x [SMALLEST_REAL]
template <typename RealType>
class SortingTarget<std::complex<RealType>, SMALLEST_REAL>
{
public:
static RealType get(const std::complex<RealType>& val)
{
return val.real();
}
};
// Specialization for SMALLEST_IMAG
// This covers [complex] x [SMALLEST_IMAG]
template <typename RealType>
class SortingTarget<std::complex<RealType>, SMALLEST_IMAG>
{
public:
static RealType get(const std::complex<RealType>& val)
{
using std::abs;
return abs(val.imag());
}
};
// Specialization for SMALLEST_ALGE
// This covers [float, double] x [SMALLEST_ALGE]
template <typename Scalar>
class SortingTarget<Scalar, SMALLEST_ALGE>
{
public:
static Scalar get(const Scalar& val)
{
return val;
}
};
// Sort eigenvalues and return the order index
template <typename PairType>
class PairComparator
{
public:
bool operator()(const PairType& v1, const PairType& v2)
{
return v1.first < v2.first;
}
};
template <typename T, int SelectionRule>
class SortEigenvalue
{
private:
typedef typename ElemType<T>::type TargetType; // Type of the sorting target, will be
// a floating number type, e.g. "double"
typedef std::pair<TargetType, int> PairType; // Type of the sorting pair, including
// the sorting target and the index
std::vector<PairType> pair_sort;
public:
SortEigenvalue(const T* start, int size) :
pair_sort(size)
{
for (int i = 0; i < size; i++)
{
pair_sort[i].first = SortingTarget<T, SelectionRule>::get(start[i]);
pair_sort[i].second = i;
}
PairComparator<PairType> comp;
std::sort(pair_sort.begin(), pair_sort.end(), comp);
}
std::vector<int> index()
{
std::vector<int> ind(pair_sort.size());
for (unsigned int i = 0; i < ind.size(); i++)
ind[i] = pair_sort[i].second;
return ind;
}
};
/// \endcond
} // namespace Spectra
#endif // SELECTION_RULE_H

91
gtsam/3rdparty/Spectra/Util/SimpleRandom.h vendored Normal file
View File

@ -0,0 +1,91 @@
// Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef SIMPLE_RANDOM_H
#define SIMPLE_RANDOM_H
#include <Eigen/Core>
/// \cond
namespace Spectra {
// We need a simple pseudo random number generator here:
// 1. It is used to generate initial and restarted residual vector.
// 2. It is not necessary to be so "random" and advanced. All we hope
// is that the residual vector is not in the space spanned by the
// current Krylov space. This should be met almost surely.
// 3. We don't want to call RNG in C++, since we actually want the
// algorithm to be deterministic. Also, calling RNG in C/C++ is not
// allowed in R packages submitted to CRAN.
// 4. The method should be as simple as possible, so an LCG is enough.
// 5. Based on public domain code by Ray Gardner
// http://stjarnhimlen.se/snippets/rg_rand.c
template <typename Scalar = double>
class SimpleRandom
{
private:
typedef Eigen::Index Index;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
const unsigned int m_a; // multiplier
const unsigned long m_max; // 2^31 - 1
long m_rand;
inline long next_long_rand(long seed)
{
unsigned long lo, hi;
lo = m_a * (long) (seed & 0xFFFF);
hi = m_a * (long) ((unsigned long) seed >> 16);
lo += (hi & 0x7FFF) << 16;
if (lo > m_max)
{
lo &= m_max;
++lo;
}
lo += hi >> 15;
if (lo > m_max)
{
lo &= m_max;
++lo;
}
return (long) lo;
}
public:
SimpleRandom(unsigned long init_seed) :
m_a(16807),
m_max(2147483647L),
m_rand(init_seed ? (init_seed & m_max) : 1)
{}
Scalar random()
{
m_rand = next_long_rand(m_rand);
return Scalar(m_rand) / Scalar(m_max) - Scalar(0.5);
}
// Vector of random numbers of type Scalar
// Ranging from -0.5 to 0.5
Vector random_vec(const Index len)
{
Vector res(len);
for (Index i = 0; i < len; i++)
{
m_rand = next_long_rand(m_rand);
res[i] = Scalar(m_rand) / Scalar(m_max) - Scalar(0.5);
}
return res;
}
};
} // namespace Spectra
/// \endcond
#endif // SIMPLE_RANDOM_H

71
gtsam/3rdparty/Spectra/Util/TypeTraits.h vendored Normal file
View File

@ -0,0 +1,71 @@
// Copyright (C) 2018-2019 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef TYPE_TRAITS_H
#define TYPE_TRAITS_H
#include <Eigen/Core>
#include <limits>
/// \cond
namespace Spectra {
// For a real value type "Scalar", we want to know its smallest
// positive value, i.e., std::numeric_limits<Scalar>::min().
// However, we must take non-standard value types into account,
// so we rely on Eigen::NumTraits.
//
// Eigen::NumTraits has defined epsilon() and lowest(), but
// lowest() means negative highest(), which is a very small
// negative value.
//
// Therefore, we manually define this limit, and use eplison()^3
// to mimic it for non-standard types.
// Generic definition
template <typename Scalar>
struct TypeTraits
{
static inline Scalar min()
{
return Eigen::numext::pow(Eigen::NumTraits<Scalar>::epsilon(), Scalar(3));
}
};
// Full specialization
template <>
struct TypeTraits<float>
{
static inline float min()
{
return std::numeric_limits<float>::min();
}
};
template <>
struct TypeTraits<double>
{
static inline double min()
{
return std::numeric_limits<double>::min();
}
};
template <>
struct TypeTraits<long double>
{
static inline long double min()
{
return std::numeric_limits<long double>::min();
}
};
} // namespace Spectra
/// \endcond
#endif // TYPE_TRAITS_H

552
gtsam/3rdparty/Spectra/contrib/LOBPCGSolver.h vendored Normal file
View File

@ -0,0 +1,552 @@
// Written by Anna Araslanova
// Modified by Yixuan Qiu
// License: MIT
#ifndef LOBPCG_SOLVER
#define LOBPCG_SOLVER
#include <functional>
#include <map>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/SparseCholesky>
#include "../SymGEigsSolver.h"
namespace Spectra {
///
/// \ingroup EigenSolver
///
/// *** METHOD
/// The class represent the LOBPCG algorithm, which was invented by Andrew Knyazev
/// Theoretical background of the procedure can be found in the articles below
/// - Knyazev, A.V., 2001. Toward the optimal preconditioned eigensolver : Locally optimal block preconditioned conjugate gradient method.SIAM journal on scientific computing, 23(2), pp.517 - 541.
/// - Knyazev, A.V., Argentati, M.E., Lashuk, I. and Ovtchinnikov, E.E., 2007. Block locally optimal preconditioned eigenvalue xolvers(BLOPEX) in HYPRE and PETSc.SIAM Journal on Scientific Computing, 29(5), pp.2224 - 2239.
///
/// *** CONDITIONS OF USE
/// Locally Optimal Block Preconditioned Conjugate Gradient(LOBPCG) is a method for finding the M smallest eigenvalues
/// and eigenvectors of a large symmetric positive definite generalized eigenvalue problem
/// \f$Ax=\lambda Bx,\f$
/// where \f$A_{NxN}\f$ is a symmetric matrix, \f$B\f$ is symmetric and positive - definite. \f$A and B\f$ are also assumed large and sparse
/// \f$\textit{M}\f$ should be \f$\<< textit{N}\f$ (at least \f$\textit{5M} < \textit{N} \f$)
///
/// *** ARGUMENTS
/// Eigen::SparseMatrix<long double> A; // N*N - Ax = lambda*Bx, lrage and sparse
/// Eigen::SparseMatrix<long double> X; // N*M - initial approximations to eigenvectors (random in general case)
/// Spectra::LOBPCGSolver<long double> solver(A, X);
/// *Eigen::SparseMatrix<long double> B; // N*N - Ax = lambda*Bx, sparse, positive definite
/// solver.setConstraints(B);
/// *Eigen::SparseMatrix<long double> Y; // N*K - constraints, already found eigenvectors
/// solver.setB(B);
/// *Eigen::SparseMatrix<long double> T; // N*N - preconditioner ~ A^-1
/// solver.setPreconditioner(T);
///
/// *** OUTCOMES
/// solver.solve(); // compute eigenpairs // void
/// solver.info(); // state of converjance // int
/// solver.residuals(); // get residuals to evaluate biases // Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
/// solver.eigenvalues(); // get eigenvalues // Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
/// solver.eigenvectors(); // get eigenvectors // Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
///
/// *** EXAMPLE
/// \code{.cpp}
/// #include <Spectra/contrib/SymSparseEigsSolverLOBPCG.h>
///
/// // random A
/// Matrix a;
/// a = (Matrix::Random(10, 10).array() > 0.6).cast<long double>() * Matrix::Random(10, 10).array() * 5;
/// a = Matrix((a).triangularView<Eigen::Lower>()) + Matrix((a).triangularView<Eigen::Lower>()).transpose();
/// for (int i = 0; i < 10; i++)
/// a(i, i) = i + 0.5;
/// std::cout << a << "\n";
/// Eigen::SparseMatrix<long double> A(a.sparseView());
/// // random X
/// Eigen::Matrix<long double, 10, 2> x;
/// x = Matrix::Random(10, 2).array();
/// Eigen::SparseMatrix<long double> X(x.sparseView());
/// // solve Ax = lambda*x
/// Spectra::LOBPCGSolver<long double> solver(A, X);
/// solver.compute(10, 1e-4); // 10 iterations, L2_tolerance = 1e-4*N
/// std::cout << "info\n" << solver.info() << std::endl;
/// std::cout << "eigenvalues\n" << solver.eigenvalues() << std::endl;
/// std::cout << "eigenvectors\n" << solver.eigenvectors() << std::endl;
/// std::cout << "residuals\n" << solver.residuals() << std::endl;
/// \endcode
///
template <typename Scalar = long double>
class LOBPCGSolver
{
private:
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef std::complex<Scalar> Complex;
typedef Eigen::Matrix<Complex, Eigen::Dynamic, Eigen::Dynamic> ComplexMatrix;
typedef Eigen::Matrix<Complex, Eigen::Dynamic, 1> ComplexVector;
typedef Eigen::SparseMatrix<Scalar> SparseMatrix;
typedef Eigen::SparseMatrix<Complex> SparseComplexMatrix;
const int m_n; // dimension of matrix A
const int m_nev; // number of eigenvalues requested
SparseMatrix A, X;
SparseMatrix m_Y, m_B, m_preconditioner;
bool flag_with_constraints, flag_with_B, flag_with_preconditioner;
public:
SparseMatrix m_residuals;
Matrix m_evectors;
Vector m_evalues;
int m_info;
private:
// B-orthonormalize matrix M
int orthogonalizeInPlace(SparseMatrix& M, SparseMatrix& B,
SparseMatrix& true_BM, bool has_true_BM = false)
{
SparseMatrix BM;
if (has_true_BM == false)
{
if (flag_with_B)
{
BM = B * M;
}
else
{
BM = M;
}
}
else
{
BM = true_BM;
}
Eigen::SimplicialLDLT<SparseMatrix> chol_MBM(M.transpose() * BM);
if (chol_MBM.info() != SUCCESSFUL)
{
// LDLT decomposition fail
m_info = chol_MBM.info();
return chol_MBM.info();
}
SparseComplexMatrix Upper_MBM = chol_MBM.matrixU().template cast<Complex>();
ComplexVector D_MBM_vec = chol_MBM.vectorD().template cast<Complex>();
D_MBM_vec = D_MBM_vec.cwiseSqrt();
for (int i = 0; i < D_MBM_vec.rows(); i++)
{
D_MBM_vec(i) = Complex(1.0, 0.0) / D_MBM_vec(i);
}
SparseComplexMatrix D_MBM_mat(D_MBM_vec.asDiagonal());
SparseComplexMatrix U_inv(Upper_MBM.rows(), Upper_MBM.cols());
U_inv.setIdentity();
Upper_MBM.template triangularView<Eigen::Upper>().solveInPlace(U_inv);
SparseComplexMatrix right_product = U_inv * D_MBM_mat;
M = M * right_product.real();
if (flag_with_B)
{
true_BM = B * M;
}
else
{
true_BM = M;
}
return SUCCESSFUL;
}
void applyConstraintsInPlace(SparseMatrix& X, SparseMatrix& Y,
SparseMatrix& B)
{
SparseMatrix BY;
if (flag_with_B)
{
BY = B * Y;
}
else
{
BY = Y;
}
SparseMatrix YBY = Y.transpose() * BY;
SparseMatrix BYX = BY.transpose() * X;
SparseMatrix YBY_XYX = (Matrix(YBY).bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Matrix(BYX))).sparseView();
X = X - Y * YBY_XYX;
}
/*
return
'AB
CD'
*/
Matrix stack_4_matricies(Matrix A, Matrix B,
Matrix C, Matrix D)
{
Matrix result(A.rows() + C.rows(), A.cols() + B.cols());
result.topLeftCorner(A.rows(), A.cols()) = A;
result.topRightCorner(B.rows(), B.cols()) = B;
result.bottomLeftCorner(C.rows(), C.cols()) = C;
result.bottomRightCorner(D.rows(), D.cols()) = D;
return result;
}
Matrix stack_9_matricies(Matrix A, Matrix B, Matrix C,
Matrix D, Matrix E, Matrix F,
Matrix G, Matrix H, Matrix I)
{
Matrix result(A.rows() + D.rows() + G.rows(), A.cols() + B.cols() + C.cols());
result.block(0, 0, A.rows(), A.cols()) = A;
result.block(0, A.cols(), B.rows(), B.cols()) = B;
result.block(0, A.cols() + B.cols(), C.rows(), C.cols()) = C;
result.block(A.rows(), 0, D.rows(), D.cols()) = D;
result.block(A.rows(), A.cols(), E.rows(), E.cols()) = E;
result.block(A.rows(), A.cols() + B.cols(), F.rows(), F.cols()) = F;
result.block(A.rows() + D.rows(), 0, G.rows(), G.cols()) = G;
result.block(A.rows() + D.rows(), A.cols(), H.rows(), H.cols()) = H;
result.block(A.rows() + D.rows(), A.cols() + B.cols(), I.rows(), I.cols()) = I;
return result;
}
void sort_epairs(Vector& evalues, Matrix& evectors, int SelectionRule)
{
std::function<bool(Scalar, Scalar)> cmp;
if (SelectionRule == SMALLEST_ALGE)
cmp = std::less<Scalar>{};
else
cmp = std::greater<Scalar>{};
std::map<Scalar, Vector, decltype(cmp)> epairs(cmp);
for (int i = 0; i < m_evectors.cols(); ++i)
epairs.insert(std::make_pair(evalues(i), evectors.col(i)));
int i = 0;
for (auto& epair : epairs)
{
evectors.col(i) = epair.second;
evalues(i) = epair.first;
i++;
}
}
void removeColumns(SparseMatrix& matrix, std::vector<int>& colToRemove)
{
// remove columns through matrix multiplication
SparseMatrix new_matrix(matrix.cols(), matrix.cols() - int(colToRemove.size()));
int iCol = 0;
std::vector<Eigen::Triplet<Scalar>> tripletList;
tripletList.reserve(matrix.cols() - int(colToRemove.size()));
for (int iRow = 0; iRow < matrix.cols(); iRow++)
{
if (std::find(colToRemove.begin(), colToRemove.end(), iRow) == colToRemove.end())
{
tripletList.push_back(Eigen::Triplet<Scalar>(iRow, iCol, 1));
iCol++;
}
}
new_matrix.setFromTriplets(tripletList.begin(), tripletList.end());
matrix = matrix * new_matrix;
}
int checkConvergence_getBlocksize(SparseMatrix& m_residuals, Scalar tolerance_L2, std::vector<int>& columnsToDelete)
{
// square roots from sum of squares by column
int BlockSize = m_nev;
Scalar sum, buffer;
for (int iCol = 0; iCol < m_nev; iCol++)
{
sum = 0;
for (int iRow = 0; iRow < m_n; iRow++)
{
buffer = m_residuals.coeff(iRow, iCol);
sum += buffer * buffer;
}
if (sqrt(sum) < tolerance_L2)
{
BlockSize--;
columnsToDelete.push_back(iCol);
}
}
return BlockSize;
}
public:
LOBPCGSolver(const SparseMatrix& A, const SparseMatrix X) :
m_n(A.rows()),
m_nev(X.cols()),
m_info(NOT_COMPUTED),
flag_with_constraints(false),
flag_with_B(false),
flag_with_preconditioner(false),
A(A),
X(X)
{
if (A.rows() != X.rows() || A.rows() != A.cols())
throw std::invalid_argument("Wrong size");
//if (m_n < 5* m_nev)
// throw std::invalid_argument("The problem size is small compared to the block size. Use standard eigensolver");
}
void setConstraints(const SparseMatrix& Y)
{
m_Y = Y;
flag_with_constraints = true;
}
void setB(const SparseMatrix& B)
{
if (B.rows() != A.rows() || B.cols() != A.cols())
throw std::invalid_argument("Wrong size");
m_B = B;
flag_with_B = true;
}
void setPreconditioner(const SparseMatrix& preconditioner)
{
m_preconditioner = preconditioner;
flag_with_preconditioner = true;
}
void compute(int maxit = 10, Scalar tol_div_n = 1e-7)
{
Scalar tolerance_L2 = tol_div_n * m_n;
int BlockSize;
int max_iter = std::min(m_n, maxit);
SparseMatrix directions, AX, AR, BX, AD, ADD, DD, BDD, BD, XAD, RAD, DAD, XBD, RBD, BR, sparse_eVecX, sparse_eVecR, sparse_eVecD, inverse_matrix;
Matrix XAR, RAR, XBR, gramA, gramB, eVecX, eVecR, eVecD;
std::vector<int> columnsToDelete;
if (flag_with_constraints)
{
// Apply the constraints Y to X
applyConstraintsInPlace(X, m_Y, m_B);
}
// Make initial vectors orthonormal
// implicit BX declaration
if (orthogonalizeInPlace(X, m_B, BX) != SUCCESSFUL)
{
max_iter = 0;
}
AX = A * X;
// Solve the following NxN eigenvalue problem for all N eigenvalues and -vectors:
// first approximation via a dense problem
Eigen::EigenSolver<Matrix> eigs(Matrix(X.transpose() * AX));
if (eigs.info() != SUCCESSFUL)
{
m_info = eigs.info();
max_iter = 0;
}
else
{
m_evalues = eigs.eigenvalues().real();
m_evectors = eigs.eigenvectors().real();
sort_epairs(m_evalues, m_evectors, SMALLEST_ALGE);
sparse_eVecX = m_evectors.sparseView();
X = X * sparse_eVecX;
AX = AX * sparse_eVecX;
BX = BX * sparse_eVecX;
}
for (int iter_num = 0; iter_num < max_iter; iter_num++)
{
m_residuals.resize(m_n, m_nev);
for (int i = 0; i < m_nev; i++)
{
m_residuals.col(i) = AX.col(i) - m_evalues(i) * BX.col(i);
}
BlockSize = checkConvergence_getBlocksize(m_residuals, tolerance_L2, columnsToDelete);
if (BlockSize == 0)
{
m_info = SUCCESSFUL;
break;
}
// substitution of the original active mask
if (columnsToDelete.size() > 0)
{
removeColumns(m_residuals, columnsToDelete);
if (iter_num > 0)
{
removeColumns(directions, columnsToDelete);
removeColumns(AD, columnsToDelete);
removeColumns(BD, columnsToDelete);
}
columnsToDelete.clear(); // for next iteration
}
if (flag_with_preconditioner)
{
// Apply the preconditioner to the residuals
m_residuals = m_preconditioner * m_residuals;
}
if (flag_with_constraints)
{
// Apply the constraints Y to residuals
applyConstraintsInPlace(m_residuals, m_Y, m_B);
}
if (orthogonalizeInPlace(m_residuals, m_B, BR) != SUCCESSFUL)
{
break;
}
AR = A * m_residuals;
// Orthonormalize conjugate directions
if (iter_num > 0)
{
if (orthogonalizeInPlace(directions, m_B, BD, true) != SUCCESSFUL)
{
break;
}
AD = A * directions;
}
// Perform the Rayleigh Ritz Procedure
XAR = Matrix(X.transpose() * AR);
RAR = Matrix(m_residuals.transpose() * AR);
XBR = Matrix(X.transpose() * BR);
if (iter_num > 0)
{
XAD = X.transpose() * AD;
RAD = m_residuals.transpose() * AD;
DAD = directions.transpose() * AD;
XBD = X.transpose() * BD;
RBD = m_residuals.transpose() * BD;
gramA = stack_9_matricies(m_evalues.asDiagonal(), XAR, XAD, XAR.transpose(), RAR, RAD, XAD.transpose(), RAD.transpose(), DAD.transpose());
gramB = stack_9_matricies(Matrix::Identity(m_nev, m_nev), XBR, XBD, XBR.transpose(), Matrix::Identity(BlockSize, BlockSize), RBD, XBD.transpose(), RBD.transpose(), Matrix::Identity(BlockSize, BlockSize));
}
else
{
gramA = stack_4_matricies(m_evalues.asDiagonal(), XAR, XAR.transpose(), RAR);
gramB = stack_4_matricies(Matrix::Identity(m_nev, m_nev), XBR, XBR.transpose(), Matrix::Identity(BlockSize, BlockSize));
}
//calculate the lowest/largest m eigenpairs; Solve the generalized eigenvalue problem.
DenseSymMatProd<Scalar> Aop(gramA);
DenseCholesky<Scalar> Bop(gramB);
SymGEigsSolver<Scalar, SMALLEST_ALGE, DenseSymMatProd<Scalar>,
DenseCholesky<Scalar>, GEIGS_CHOLESKY>
geigs(&Aop, &Bop, m_nev, std::min(10, int(gramA.rows()) - 1));
geigs.init();
int nconv = geigs.compute();
//Mat evecs;
if (geigs.info() == SUCCESSFUL)
{
m_evalues = geigs.eigenvalues();
m_evectors = geigs.eigenvectors();
sort_epairs(m_evalues, m_evectors, SMALLEST_ALGE);
}
else
{
// Problem With General EgenVec
m_info = geigs.info();
break;
}
// Compute Ritz vectors
if (iter_num > 0)
{
eVecX = m_evectors.block(0, 0, m_nev, m_nev);
eVecR = m_evectors.block(m_nev, 0, BlockSize, m_nev);
eVecD = m_evectors.block(m_nev + BlockSize, 0, BlockSize, m_nev);
sparse_eVecX = eVecX.sparseView();
sparse_eVecR = eVecR.sparseView();
sparse_eVecD = eVecD.sparseView();
DD = m_residuals * sparse_eVecR; // new conjugate directions
ADD = AR * sparse_eVecR;
BDD = BR * sparse_eVecR;
DD = DD + directions * sparse_eVecD;
ADD = ADD + AD * sparse_eVecD;
BDD = BDD + BD * sparse_eVecD;
}
else
{
eVecX = m_evectors.block(0, 0, m_nev, m_nev);
eVecR = m_evectors.block(m_nev, 0, BlockSize, m_nev);
sparse_eVecX = eVecX.sparseView();
sparse_eVecR = eVecR.sparseView();
DD = m_residuals * sparse_eVecR;
ADD = AR * sparse_eVecR;
BDD = BR * sparse_eVecR;
}
X = X * sparse_eVecX + DD;
AX = AX * sparse_eVecX + ADD;
BX = BX * sparse_eVecX + BDD;
directions = DD;
AD = ADD;
BD = BDD;
} // iteration loop
// calculate last residuals
m_residuals.resize(m_n, m_nev);
for (int i = 0; i < m_nev; i++)
{
m_residuals.col(i) = AX.col(i) - m_evalues(i) * BX.col(i);
}
BlockSize = checkConvergence_getBlocksize(m_residuals, tolerance_L2, columnsToDelete);
if (BlockSize == 0)
{
m_info = SUCCESSFUL;
}
} // compute
Vector eigenvalues()
{
return m_evalues;
}
Matrix eigenvectors()
{
return m_evectors;
}
Matrix residuals()
{
return Matrix(m_residuals);
}
int info() { return m_info; }
};
} // namespace Spectra
#endif // LOBPCG_SOLVER

202
gtsam/3rdparty/Spectra/contrib/PartialSVDSolver.h vendored Normal file
View File

@ -0,0 +1,202 @@
// Copyright (C) 2018 Yixuan Qiu <yixuan.qiu@cos.name>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
#ifndef PARTIAL_SVD_SOLVER_H
#define PARTIAL_SVD_SOLVER_H
#include <Eigen/Core>
#include "../SymEigsSolver.h"
namespace Spectra {
// Abstract class for matrix operation
template <typename Scalar>
class SVDMatOp
{
public:
virtual int rows() const = 0;
virtual int cols() const = 0;
// y_out = A' * A * x_in or y_out = A * A' * x_in
virtual void perform_op(const Scalar* x_in, Scalar* y_out) = 0;
virtual ~SVDMatOp() {}
};
// Operation of a tall matrix in SVD
// We compute the eigenvalues of A' * A
// MatrixType is either Eigen::Matrix<Scalar, ...> or Eigen::SparseMatrix<Scalar, ...>
template <typename Scalar, typename MatrixType>
class SVDTallMatOp : public SVDMatOp<Scalar>
{
private:
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef const Eigen::Ref<const MatrixType> ConstGenericMatrix;
ConstGenericMatrix m_mat;
const int m_dim;
Vector m_cache;
public:
// Constructor
SVDTallMatOp(ConstGenericMatrix& mat) :
m_mat(mat),
m_dim(std::min(mat.rows(), mat.cols())),
m_cache(mat.rows())
{}
// These are the rows and columns of A' * A
int rows() const { return m_dim; }
int cols() const { return m_dim; }
// y_out = A' * A * x_in
void perform_op(const Scalar* x_in, Scalar* y_out)
{
MapConstVec x(x_in, m_mat.cols());
MapVec y(y_out, m_mat.cols());
m_cache.noalias() = m_mat * x;
y.noalias() = m_mat.transpose() * m_cache;
}
};
// Operation of a wide matrix in SVD
// We compute the eigenvalues of A * A'
// MatrixType is either Eigen::Matrix<Scalar, ...> or Eigen::SparseMatrix<Scalar, ...>
template <typename Scalar, typename MatrixType>
class SVDWideMatOp : public SVDMatOp<Scalar>
{
private:
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Vector> MapConstVec;
typedef Eigen::Map<Vector> MapVec;
typedef const Eigen::Ref<const MatrixType> ConstGenericMatrix;
ConstGenericMatrix m_mat;
const int m_dim;
Vector m_cache;
public:
// Constructor
SVDWideMatOp(ConstGenericMatrix& mat) :
m_mat(mat),
m_dim(std::min(mat.rows(), mat.cols())),
m_cache(mat.cols())
{}
// These are the rows and columns of A * A'
int rows() const { return m_dim; }
int cols() const { return m_dim; }
// y_out = A * A' * x_in
void perform_op(const Scalar* x_in, Scalar* y_out)
{
MapConstVec x(x_in, m_mat.rows());
MapVec y(y_out, m_mat.rows());
m_cache.noalias() = m_mat.transpose() * x;
y.noalias() = m_mat * m_cache;
}
};
// Partial SVD solver
// MatrixType is either Eigen::Matrix<Scalar, ...> or Eigen::SparseMatrix<Scalar, ...>
template <typename Scalar = double,
typename MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> >
class PartialSVDSolver
{
private:
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef const Eigen::Ref<const MatrixType> ConstGenericMatrix;
ConstGenericMatrix m_mat;
const int m_m;
const int m_n;
SVDMatOp<Scalar>* m_op;
SymEigsSolver<Scalar, LARGEST_ALGE, SVDMatOp<Scalar> >* m_eigs;
int m_nconv;
Matrix m_evecs;
public:
// Constructor
PartialSVDSolver(ConstGenericMatrix& mat, int ncomp, int ncv) :
m_mat(mat), m_m(mat.rows()), m_n(mat.cols()), m_evecs(0, 0)
{
// Determine the matrix type, tall or wide
if (m_m > m_n)
{
m_op = new SVDTallMatOp<Scalar, MatrixType>(mat);
}
else
{
m_op = new SVDWideMatOp<Scalar, MatrixType>(mat);
}
// Solver object
m_eigs = new SymEigsSolver<Scalar, LARGEST_ALGE, SVDMatOp<Scalar> >(m_op, ncomp, ncv);
}
// Destructor
virtual ~PartialSVDSolver()
{
delete m_eigs;
delete m_op;
}
// Computation
int compute(int maxit = 1000, Scalar tol = 1e-10)
{
m_eigs->init();
m_nconv = m_eigs->compute(maxit, tol);
return m_nconv;
}
// The converged singular values
Vector singular_values() const
{
Vector svals = m_eigs->eigenvalues().cwiseSqrt();
return svals;
}
// The converged left singular vectors
Matrix matrix_U(int nu)
{
if (m_evecs.cols() < 1)
{
m_evecs = m_eigs->eigenvectors();
}
nu = std::min(nu, m_nconv);
if (m_m <= m_n)
{
return m_evecs.leftCols(nu);
}
return m_mat * (m_evecs.leftCols(nu).array().rowwise() / m_eigs->eigenvalues().head(nu).transpose().array().sqrt()).matrix();
}
// The converged right singular vectors
Matrix matrix_V(int nv)
{
if (m_evecs.cols() < 1)
{
m_evecs = m_eigs->eigenvectors();
}
nv = std::min(nv, m_nconv);
if (m_m > m_n)
{
return m_evecs.leftCols(nv);
}
return m_mat.transpose() * (m_evecs.leftCols(nv).array().rowwise() / m_eigs->eigenvalues().head(nv).transpose().array().sqrt()).matrix();
}
};
} // namespace Spectra
#endif // PARTIAL_SVD_SOLVER_H

22
gtsam/CMakeLists.txt
View File

@ -14,7 +14,6 @@ set (gtsam_subdirs
sam
sfm
slam
smart
navigation
)
@ -134,12 +133,6 @@ endif()
# paths so that the compiler uses GTSAM headers in our source directory instead
# of any previously installed GTSAM headers.
target_include_directories(gtsam BEFORE PUBLIC
# SuiteSparse_config
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config>
$<INSTALL_INTERFACE:include/gtsam/3rdparty/SuiteSparse_config>
# CCOLAMD
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Include>
$<INSTALL_INTERFACE:include/gtsam/3rdparty/CCOLAMD>
# main gtsam includes:
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
$<INSTALL_INTERFACE:include/>
@ -148,6 +141,19 @@ target_include_directories(gtsam BEFORE PUBLIC
# unit tests:
$<BUILD_INTERFACE:${CMAKE_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
# reported as warnings/errors in our targets:
target_include_directories(gtsam SYSTEM BEFORE PUBLIC
# SuiteSparse_config
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config>
$<INSTALL_INTERFACE:include/gtsam/3rdparty/SuiteSparse_config>
# Spectra
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/Spectra>
# CCOLAMD
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Include>
$<INSTALL_INTERFACE:include/gtsam/3rdparty/CCOLAMD>
)
if(GTSAM_SUPPORT_NESTED_DISSECTION)
target_include_directories(gtsam BEFORE PUBLIC
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/include>
@ -211,5 +217,5 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
endif()
# Wrap
wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
wrap_and_install_library(gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
endif ()

5
gtsam/base/DSFVector.h
View File

@ -55,11 +55,6 @@ public:
/// Merge the sets containing i1 and i2. Does nothing if i1 and i2 are already in the same set.
void merge(const size_t& i1, const size_t& i2);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline size_t findSet(size_t key) const {return find(key);}
inline void makeUnionInPlace(const size_t& i1, const size_t& i2) {return merge(i1,i2);}
#endif
};
/**

36
gtsam/base/GenericValue.h
View File

@ -30,6 +30,14 @@
#include <iostream>
#include <typeinfo> // operator typeid
#ifdef _WIN32
#define GENERICVALUE_VISIBILITY
#else
// This will trigger a LNKxxxx on MSVC, so disable for MSVC build
// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM-EXPORT.md
#define GENERICVALUE_VISIBILITY GTSAM_EXPORT
#endif
namespace gtsam {
/**
@ -70,7 +78,7 @@ public:
}
/// equals implementing generic Value interface
virtual bool equals_(const Value& p, double tol = 1e-9) const {
bool equals_(const Value& p, double tol = 1e-9) const override {
// Cast the base class Value pointer to a templated generic class pointer
const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
// Return the result of using the equals traits for the derived class
@ -83,15 +91,15 @@ public:
}
/// Virtual print function, uses traits
virtual void print(const std::string& str) const {
std::cout << "(" << demangle(typeid(T).name()) << ") ";
void print(const std::string& str) const override {
std::cout << "(" << demangle(typeid(T).name()) << ")\n";
traits<T>::Print(value_, str);
}
/**
* Create a duplicate object returned as a pointer to the generic Value interface.
*/
virtual Value* clone_() const {
Value* clone_() const override {
GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in
return ptr;
}
@ -99,19 +107,19 @@ public:
/**
* Destroy and deallocate this object, only if it was originally allocated using clone_().
*/
virtual void deallocate_() const {
void deallocate_() const override {
delete this;
}
/**
* Clone this value (normal clone on the heap, delete with 'delete' operator)
*/
virtual boost::shared_ptr<Value> clone() const {
boost::shared_ptr<Value> clone() const override {
return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
}
/// Generic Value interface version of retract
virtual Value* retract_(const Vector& delta) const {
Value* retract_(const Vector& delta) const override {
// Call retract on the derived class using the retract trait function
const T retractResult = traits<T>::Retract(GenericValue<T>::value(), delta);
@ -122,7 +130,7 @@ public:
}
/// Generic Value interface version of localCoordinates
virtual Vector localCoordinates_(const Value& value2) const {
Vector localCoordinates_(const Value& value2) const override {
// Cast the base class Value pointer to a templated generic class pointer
const GenericValue<T>& genericValue2 =
static_cast<const GenericValue<T>&>(value2);
@ -142,12 +150,12 @@ public:
}
/// Return run-time dimensionality
virtual size_t dim() const {
size_t dim() const override {
return traits<T>::GetDimension(value_);
}
/// Assignment operator
virtual Value& operator=(const Value& rhs) {
Value& operator=(const Value& rhs) override {
// Cast the base class Value pointer to a derived class pointer
const GenericValue& derivedRhs = static_cast<const GenericValue&>(rhs);
@ -198,4 +206,12 @@ const ValueType& Value::cast() const {
return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
}
/** Functional constructor of GenericValue<T> so T can be automatically deduced
*/
template<class T>
GenericValue<T> genericValue(const T& v) {
return GenericValue<T>(v);
}
} /* namespace gtsam */

56
gtsam/base/Manifold.h
View File

@ -167,62 +167,6 @@ struct FixedDimension {
BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
"FixedDimension instantiated for dymanically-sized type.");
};
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Helper class to construct the product manifold of two other manifolds, M1 and M2
/// Deprecated because of limited usefulness, maximum obfuscation
template<typename M1, typename M2>
class ProductManifold: public std::pair<M1, M2> {
BOOST_CONCEPT_ASSERT((IsManifold<M1>));
BOOST_CONCEPT_ASSERT((IsManifold<M2>));
protected:
enum { dimension1 = traits<M1>::dimension };
enum { dimension2 = traits<M2>::dimension };
public:
enum { dimension = dimension1 + dimension2 };
inline static size_t Dim() { return dimension;}
inline size_t dim() const { return dimension;}
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
/// Default constructor needs default constructors to be defined
ProductManifold():std::pair<M1,M2>(M1(),M2()) {}
// Construct from two original manifold values
ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}
/// Retract delta to manifold
ProductManifold retract(const TangentVector& xi) const {
M1 m1 = traits<M1>::Retract(this->first, xi.template head<dimension1>());
M2 m2 = traits<M2>::Retract(this->second, xi.template tail<dimension2>());
return ProductManifold(m1,m2);
}
/// Compute the coordinates in the tangent space
TangentVector localCoordinates(const ProductManifold& other) const {
typename traits<M1>::TangentVector v1 = traits<M1>::Local(this->first, other.first);
typename traits<M2>::TangentVector v2 = traits<M2>::Local(this->second, other.second);
TangentVector v;
v << v1, v2;
return v;
}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
};
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename M1, typename M2>
struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
};
#endif
} // \ namespace gtsam
///**

26
gtsam/base/Matrix.cpp
View File

@ -136,20 +136,24 @@ Vector operator^(const Matrix& A, const Vector & v) {
return A.transpose() * v;
}
const Eigen::IOFormat& matlabFormat() {
static const Eigen::IOFormat matlab(
Eigen::StreamPrecision, // precision
Eigen::DontAlignCols, // flags set such that rowSpacers are not added
", ", // coeffSeparator
";\n", // rowSeparator
"\t", // rowPrefix
"", // rowSuffix
"[\n", // matPrefix
"\n]" // matSuffix
);
return matlab;
}
/* ************************************************************************* */
//3 argument call
void print(const Matrix& A, const string &s, ostream& stream) {
static const Eigen::IOFormat matlab(
Eigen::StreamPrecision, // precision
0, // flags
", ", // coeffSeparator
";\n", // rowSeparator
"\t", // rowPrefix
"", // rowSuffix
"[\n", // matPrefix
"\n]" // matSuffix
);
cout << s << A.format(matlab) << endl;
cout << s << A.format(matlabFormat()) << endl;
}
/* ************************************************************************* */

28
gtsam/base/Matrix.h
View File

@ -23,14 +23,11 @@
// \callgraph
#pragma once
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam/config.h>
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#endif
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/tuple/tuple.hpp>
@ -76,6 +73,10 @@ GTSAM_MAKE_MATRIX_DEFS(9);
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
// Matrix formatting arguments when printing.
// Akin to Matlab style.
const Eigen::IOFormat& matlabFormat();
/**
* equals with a tolerance
*/
@ -517,23 +518,6 @@ struct MultiplyWithInverseFunction {
const Operator phi_;
};
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); }
inline Matrix ones( size_t m, size_t n ) { return Matrix::Ones(m,n); }
inline Matrix eye( size_t m, size_t n) { return Matrix::Identity(m, n); }
inline Matrix eye( size_t m ) { return eye(m,m); }
inline Matrix diag(const Vector& v) { return v.asDiagonal(); }
inline void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { e += alpha * A * x; }
inline void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { e += A * x; }
inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { x += alpha * A.transpose() * e; }
inline void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { x += A.transpose() * e; }
inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { x += alpha * A.transpose() * e; }
inline void insertColumn(Matrix& A, const Vector& col, size_t j) { A.col(j) = col; }
inline void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { A.col(j).segment(i, col.size()) = col; }
inline void solve(Matrix& A, Matrix& B) { B = A.fullPivLu().solve(B); }
inline Matrix inverse(const Matrix& A) { return A.inverse(); }
#endif
GTSAM_EXPORT Matrix LLt(const Matrix& A);
GTSAM_EXPORT Matrix RtR(const Matrix& A);

10
gtsam/base/ThreadsafeException.h
View File

@ -71,12 +71,12 @@ protected:
String(description.begin(), description.end())) {
}
/// Default destructor doesn't have the throw()
virtual ~ThreadsafeException() throw () {
/// Default destructor doesn't have the noexcept
virtual ~ThreadsafeException() noexcept {
}
public:
virtual const char* what() const throw () {
const char* what() const noexcept override {
return description_ ? description_->c_str() : "";
}
};
@ -113,8 +113,8 @@ public:
class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
{
public:
CholeskyFailed() throw() {}
virtual ~CholeskyFailed() throw() {}
CholeskyFailed() noexcept {}
virtual ~CholeskyFailed() noexcept {}
};
} // namespace gtsam

20
gtsam/base/Vector.h
View File

@ -256,26 +256,6 @@ GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
* concatenate Vectors
*/
GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline Vector abs(const Vector& v){return v.cwiseAbs();}
inline Vector basis(size_t n, size_t i) { return Vector::Unit(n,i); }
inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;}
inline size_t dim(const Vector& v) { return v.size(); }
inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
inline double max(const Vector &a){return a.maxCoeff();}
inline double norm_2(const Vector& v) {return v.norm();}
inline Vector ones(size_t n) { return Vector::Ones(n); }
inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);}
inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);}
inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
inline double sum(const Vector &a){return a.sum();}
inline bool zero(const Vector& v){ return v.isZero(); }
inline Vector zero(size_t n) { return Vector::Zero(n); }
#endif
} // namespace gtsam
#include <boost/serialization/nvp.hpp>

2
gtsam/base/timing.cpp
View File

@ -89,7 +89,7 @@ void TimingOutline::print(const std::string& outline) const {
childOrder[child.second->myOrder_] = child.second;
}
// Print children
for(const ChildOrder::value_type order_child: childOrder) {
for(const ChildOrder::value_type& order_child: childOrder) {
std::string childOutline(outline);
childOutline += "| ";
order_child.second->print(childOutline);

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

@ -57,7 +57,7 @@ namespace gtsam {
makeNewTasks(makeNewTasks),
isPostOrderPhase(false) {}
tbb::task* execute()
tbb::task* execute() override
{
if(isPostOrderPhase)
{
@ -144,7 +144,7 @@ namespace gtsam {
roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
problemSizeThreshold(problemSizeThreshold) {}
tbb::task* execute()
tbb::task* execute() override
{
typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
// Create data and tasks for our children

5
gtsam/config.h.in
View File

@ -70,10 +70,7 @@
#cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION
// Make sure dependent projects that want it can see deprecated functions
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Publish flag about Eigen typedef
#cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V41
// Support Metis-based nested dissection
#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION

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

@ -66,42 +66,42 @@ namespace gtsam {
}
/// Leaf-Leaf equality
bool sameLeaf(const Leaf& q) const {
bool sameLeaf(const Leaf& q) const override {
return constant_ == q.constant_;
}
/// polymorphic equality: is q is a leaf, could be
bool sameLeaf(const Node& q) const {
bool sameLeaf(const Node& q) const override {
return (q.isLeaf() && q.sameLeaf(*this));
}
/** equality up to tolerance */
bool equals(const Node& q, double tol) const {
bool equals(const Node& q, double tol) const override {
const Leaf* other = dynamic_cast<const Leaf*> (&q);
if (!other) return false;
return std::abs(double(this->constant_ - other->constant_)) < tol;
}
/** print */
void print(const std::string& s) const {
void print(const std::string& s) const override {
bool showZero = true;
if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl;
}
/** to graphviz file */
void dot(std::ostream& os, bool showZero) const {
void dot(std::ostream& os, bool showZero) const override {
if (showZero || constant_) os << "\"" << this->id() << "\" [label=\""
<< boost::format("%4.2g") % constant_
<< "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55,
}
/** evaluate */
const Y& operator()(const Assignment<L>& x) const {
const Y& operator()(const Assignment<L>& x) const override {
return constant_;
}
/** apply unary operator */
NodePtr apply(const Unary& op) const {
NodePtr apply(const Unary& op) const override {
NodePtr f(new Leaf(op(constant_)));
return f;
}
@ -111,27 +111,27 @@ namespace gtsam {
// Simply calls apply on argument to call correct virtual method:
// fL.apply_f_op_g(gL) -> gL.apply_g_op_fL(fL) (below)
// fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice)
NodePtr apply_f_op_g(const Node& g, const Binary& op) const {
NodePtr apply_f_op_g(const Node& g, const Binary& op) const override {
return g.apply_g_op_fL(*this, op);
}
// Applying binary operator to two leaves results in a leaf
NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const {
NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override {
NodePtr h(new Leaf(op(fL.constant_, constant_))); // fL op gL
return h;
}
// If second argument is a Choice node, call it's apply with leaf as second
NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const {
NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override {
return fC.apply_fC_op_gL(*this, op); // operand order back to normal
}
/** choose a branch, create new memory ! */
NodePtr choose(const L& label, size_t index) const {
NodePtr choose(const L& label, size_t index) const override {
return NodePtr(new Leaf(constant()));
}
bool isLeaf() const { return true; }
bool isLeaf() const override { return true; }
}; // Leaf
@ -175,7 +175,7 @@ namespace gtsam {
return f;
}
bool isLeaf() const { return false; }
bool isLeaf() const override { return false; }
/** Constructor, given choice label and mandatory expected branch count */
Choice(const L& label, size_t count) :
@ -236,7 +236,7 @@ namespace gtsam {
}
/** print (as a tree) */
void print(const std::string& s) const {
void print(const std::string& s) const override {
std::cout << s << " Choice(";
// std::cout << this << ",";
std::cout << label_ << ") " << std::endl;
@ -245,7 +245,7 @@ namespace gtsam {
}
/** output to graphviz (as a a graph) */
void dot(std::ostream& os, bool showZero) const {
void dot(std::ostream& os, bool showZero) const override {
os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_
<< "\"]\n";
for (size_t i = 0; i < branches_.size(); i++) {
@ -266,17 +266,17 @@ namespace gtsam {
}
/// Choice-Leaf equality: always false
bool sameLeaf(const Leaf& q) const {
bool sameLeaf(const Leaf& q) const override {
return false;
}
/// polymorphic equality: if q is a leaf, could be...
bool sameLeaf(const Node& q) const {
bool sameLeaf(const Node& q) const override {
return (q.isLeaf() && q.sameLeaf(*this));
}
/** equality up to tolerance */
bool equals(const Node& q, double tol) const {
bool equals(const Node& q, double tol) const override {
const Choice* other = dynamic_cast<const Choice*> (&q);
if (!other) return false;
if (this->label_ != other->label_) return false;
@ -288,7 +288,7 @@ namespace gtsam {
}
/** evaluate */
const Y& operator()(const Assignment<L>& x) const {
const Y& operator()(const Assignment<L>& x) const override {
#ifndef NDEBUG
typename Assignment<L>::const_iterator it = x.find(label_);
if (it == x.end()) {
@ -314,7 +314,7 @@ namespace gtsam {
}
/** apply unary operator */
NodePtr apply(const Unary& op) const {
NodePtr apply(const Unary& op) const override {
boost::shared_ptr<Choice> r(new Choice(label_, *this, op));
return Unique(r);
}
@ -324,12 +324,12 @@ namespace gtsam {
// Simply calls apply on argument to call correct virtual method:
// fC.apply_f_op_g(gL) -> gL.apply_g_op_fC(fC) -> (Leaf)
// fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below)
NodePtr apply_f_op_g(const Node& g, const Binary& op) const {
NodePtr apply_f_op_g(const Node& g, const Binary& op) const override {
return g.apply_g_op_fC(*this, op);
}
// If second argument of binary op is Leaf node, recurse on branches
NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const {
NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override {
boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
for(NodePtr branch: branches_)
h->push_back(fL.apply_f_op_g(*branch, op));
@ -337,7 +337,7 @@ namespace gtsam {
}
// If second argument of binary op is Choice, call constructor
NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const {
NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override {
boost::shared_ptr<Choice> h(new Choice(fC, *this, op));
return Unique(h);
}
@ -352,7 +352,7 @@ namespace gtsam {
}
/** choose a branch, recursively */
NodePtr choose(const L& label, size_t index) const {
NodePtr choose(const L& label, size_t index) const override {
if (label_ == label)
return branches_[index]; // choose branch

2
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -69,7 +69,7 @@ namespace gtsam {
for(Key j: f.keys()) cs[j] = f.cardinality(j);
// Convert map into keys
DiscreteKeys keys;
for(const DiscreteKey& key: cs)
for(const std::pair<const Key,size_t>& key: cs)
keys.push_back(key);
// apply operand
ADT result = ADT::apply(f, op);

12
gtsam/discrete/DecisionTreeFactor.h
View File

@ -69,23 +69,23 @@ namespace gtsam {
/// @{
/// equality
bool equals(const DiscreteFactor& other, double tol = 1e-9) const;
bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
// print
virtual void print(const std::string& s = "DecisionTreeFactor:\n",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
void print(const std::string& s = "DecisionTreeFactor:\n",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// @}
/// @name Standard Interface
/// @{
/// Value is just look up in AlgebraicDecisonTree
virtual double operator()(const Values& values) const {
double operator()(const Values& values) const override {
return Potentials::operator()(values);
}
/// multiply two factors
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const {
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
return apply(f, ADT::Ring::mul);
}
@ -95,7 +95,7 @@ namespace gtsam {
}
/// Convert into a decisiontree
virtual DecisionTreeFactor toDecisionTreeFactor() const {
DecisionTreeFactor toDecisionTreeFactor() const override {
return *this;
}

1
gtsam/discrete/DiscreteBayesTree.h
View File

@ -45,6 +45,7 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
DiscreteBayesTreeClique() {}
virtual ~DiscreteBayesTreeClique() {}
DiscreteBayesTreeClique(
const boost::shared_ptr<DiscreteConditional>& conditional)
: Base(conditional) {}

6
gtsam/discrete/DiscreteConditional.h
View File

@ -85,10 +85,10 @@ public:
/// GTSAM-style print
void print(const std::string& s = "Discrete Conditional: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// GTSAM-style equals
bool equals(const DiscreteFactor& other, double tol = 1e-9) const;
bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
/// @}
/// @name Standard Interface
@ -102,7 +102,7 @@ public:
}
/// Evaluate, just look up in AlgebraicDecisonTree
virtual double operator()(const Values& values) const {
double operator()(const Values& values) const override {
return Potentials::operator()(values);
}

2
gtsam/discrete/Potentials.cpp
View File

@ -56,7 +56,7 @@ bool Potentials::equals(const Potentials& other, double tol) const {
/* ************************************************************************* */
void Potentials::print(const string& s, const KeyFormatter& formatter) const {
cout << s << "\n Cardinalities: {";
for (const DiscreteKey& key : cardinalities_)
for (const std::pair<const Key,size_t>& key : cardinalities_)
cout << formatter(key.first) << ":" << key.second << ", ";
cout << "}" << endl;
ADT::print(" ");

4
gtsam/geometry/Cal3DS2.h
View File

@ -60,7 +60,7 @@ public:
/// @{
/// 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& K, double tol = 10e-9) const;
@ -86,7 +86,7 @@ public:
/// @{
/// @return a deep copy of this object
virtual boost::shared_ptr<Base> clone() const {
boost::shared_ptr<Base> clone() const override {
return boost::shared_ptr<Base>(new Cal3DS2(*this));
}

2
gtsam/geometry/Cal3DS2_Base.h
View File

@ -69,7 +69,7 @@ public:
/// @{
/// print with optional string
virtual void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;

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