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

Merge branch 'develop' into imu-examples

release/4.3a0
Varun Agrawal 2020-08-21 11:25:39 -04:00
parent 5a85494f33 838c5df471
commit 9e1c4fc2fb
542 changed files with 22583 additions and 16866 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -47,8 +47,6 @@ case $WRAPPER in
;;
esac
git submodule update --init --recursive
mkdir $GITHUB_WORKSPACE/build
cd $GITHUB_WORKSPACE/build
@ -82,9 +80,9 @@ case $WRAPPER in
$PYTHON -m unittest discover
;;
"pybind")
cd $GITHUB_WORKSPACE/python
cd $GITHUB_WORKSPACE/build/python
$PYTHON setup.py install --user --prefix=
cd $GITHUB_WORKSPACE/wrap/python/gtsam_py/tests
cd $GITHUB_WORKSPACE/python/gtsam/tests
$PYTHON -m unittest discover
;;
*)

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

@ -47,7 +47,6 @@ function configure()
BUILD_DIR=$GITHUB_WORKSPACE/build
#env
git submodule update --init --recursive
rm -fr $BUILD_DIR || true
mkdir $BUILD_DIR && cd $BUILD_DIR
@ -64,11 +63,11 @@ 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_V41:-OFF} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_TYPEDEF_POINTS_TO_VECTOR=${GTSAM_TYPEDEF_POINTS_TO_VECTOR:-OFF} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DCMAKE_VERBOSE_MAKEFILE=ON \
-DBOOST_ROOT=$BOOST_ROOT \
-DBoost_NO_SYSTEM_PATHS=ON \
-DBoost_ARCHITECTURE=-x64

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

@ -72,7 +72,7 @@ jobs:
if: runner.os == 'Linux'
run: |
echo "BOOST_ROOT = $BOOST_ROOT"
- name: Build (Linux)
- name: Build and Test (Linux)
if: runner.os == 'Linux'
run: |
bash .github/scripts/unix.sh -t

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

@ -45,7 +45,7 @@ jobs:
echo "::set-env name=CC::clang"
echo "::set-env name=CXX::clang++"
fi
- name: Build (macOS)
- name: Build and Test (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/unix.sh -t

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

@ -20,24 +20,25 @@ jobs:
# 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-gcc-9, # TODO Disabled for now because of timeouts
ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1,
]
build_type: [Debug, Release]
python_version: [3]
wrapper: [cython]
wrapper: [pybind]
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"
# 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

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-points-vector,
]
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-points-vector
os: ubuntu-18.04
compiler: gcc
version: "9"
flag: points-vector
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'
env:
GTSAM_ALLOW_DEPRECATED_SINCE_V41: ON
run: echo "Allow deprecated since version 4.1"
- name: Set Use Quaternions Flag
if: matrix.flag == 'quaternions'
env:
GTSAM_USE_QUATERNIONS: ON
run: echo "Use Quaternions for rotations"
- name: Set Typedef Points to Vector Flag
if: matrix.flag == 'points-vector'
env:
GTSAM_TYPEDEF_POINTS_TO_VECTOR: ON
run: echo "Typedef Points to Vector"
- name: Build & Test
run: |
bash .github/scripts/unix.sh -t

1
.gitignore vendored
View File

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

266
CMakeLists.txt
View File

@ -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_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_TYPEDEF_POINTS_TO_VECTORS "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF)
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)
@ -524,7 +512,9 @@ endif()
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_enabled_config(${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)
@ -572,13 +562,13 @@ else()
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
print_config("Build with ccache" "Yes")
elseif(CCACHE_FOUND)
elseif(CCACHE_FOUND)
print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else()
else()
print_config("Build with ccache" "No")
endif()
endif()
endif()
message(STATUS "Packaging flags")
@ -591,10 +581,8 @@ print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency c
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_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
print_enabled_config(${GTSAM_BUILD_WRAP} "Build Wrap ")
message(STATUS "MATLAB toolbox flags")
print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ")
@ -602,23 +590,23 @@ if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
print_config("MATLAB root" "${MATLAB_ROOT}")
print_config("MEX binary" "${MEX_COMMAND}")
endif()
message(STATUS "Cython toolbox flags ")
print_enabled_config(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ")
if(GTSAM_INSTALL_CYTHON_TOOLBOX)
print_config("Python version" "${GTSAM_PYTHON_VERSION}")
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 "===============================================================")
# 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

17
README.md
View File

@ -1,6 +1,10 @@
# README - Georgia Tech Smoothing and Mapping Library
**As of August 1, develop is officially in "Pre 4.1" mode, and features deprecated in 4.0 were removed. Use the last 4.0.3 release 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**
**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?
@ -9,13 +13,14 @@ 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) |
| 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

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

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

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

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

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

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

21
gtsam/CMakeLists.txt
View File

@ -133,6 +133,18 @@ 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
# main gtsam includes:
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
$<INSTALL_INTERFACE:include/>
# config.h
$<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
# 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>
@ -141,13 +153,6 @@ target_include_directories(gtsam BEFORE PUBLIC
# 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/>
# config.h
$<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
# unit tests:
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
)
if(GTSAM_SUPPORT_NESTED_DISSECTION)
target_include_directories(gtsam BEFORE PUBLIC
@ -212,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 ()

3
gtsam/config.h.in
View File

@ -72,9 +72,6 @@
// Make sure dependent projects that want it can see deprecated functions
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V41
// Publish flag about Eigen typedef
#cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS
// Support Metis-based nested dissection
#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION

31
gtsam/geometry/Point2.cpp
View File

@ -50,37 +50,6 @@ double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1,
}
}
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/* ************************************************************************* */
void Point2::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol);
}
/* ************************************************************************* */
double Point2::norm(OptionalJacobian<1,2> H) const {
return gtsam::norm2(*this, H);
}
/* ************************************************************************* */
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const {
return gtsam::distance2(*this, point, H1, H2);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
os << '(' << p.x() << ", " << p.y() << ')';
return os;
}
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/* ************************************************************************* */
// Math inspired by http://paulbourke.net/geometry/circlesphere/
boost::optional<Point2> circleCircleIntersection(double R_d, double r_d,

108
gtsam/geometry/Point2.h
View File

@ -22,111 +22,9 @@
namespace gtsam {
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point2 to Vector2
typedef Vector2 Point2;
#else
/**
* A 2D point
* Complies with the Testable Concept
* Functional, so no set functions: once created, a point is constant.
* @addtogroup geometry
* \nosubgrouping
*/
class Point2 : public Vector2 {
private:
public:
enum { dimension = 2 };
/// @name Standard Constructors
/// @{
/// default constructor
Point2() {}
using Vector2::Vector2;
/// @}
/// @name Advanced Constructors
/// @{
/// construct from 2D vector
explicit Point2(const Vector2& v):Vector2(v) {}
/// @}
/// @name Testable
/// @{
/// print with optional string
GTSAM_EXPORT void print(const std::string& s = "") const;
/// equals with an tolerance, prints out message if unequal
GTSAM_EXPORT bool equals(const Point2& q, double tol = 1e-9) const;
/// @}
/// @name Group
/// @{
/// identity
inline static Point2 identity() {return Point2(0,0);}
/// @}
/// @name Vector Space
/// @{
/** creates a unit vector */
Point2 unit() const { return *this/norm(); }
/** norm of point, with derivative */
GTSAM_EXPORT double norm(OptionalJacobian<1,2> H = boost::none) const;
/** distance between two points */
GTSAM_EXPORT double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
OptionalJacobian<1,2> H2 = boost::none) const;
/// @}
/// @name Standard Interface
/// @{
/// equality
inline bool operator ==(const Point2& q) const {return x()==q.x() && y()==q.y();}
/// get x
inline double x() const {return (*this)[0];}
/// get y
inline double y() const {return (*this)[1];}
/// return vectorized form (column-wise).
const Vector2& vector() const { return *this; }
/// @}
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
private:
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
{
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector2);}
/// @}
};
template<>
struct traits<Point2> : public internal::VectorSpace<Point2> {
};
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point2 to Vector2
typedef Vector2 Point2;
/// Distance of the point from the origin, with Jacobian
GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);

41
gtsam/geometry/Point3.cpp
View File

@ -22,47 +22,6 @@ using namespace std;
namespace gtsam {
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
bool Point3::equals(const Point3 &q, double tol) const {
return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol &&
std::abs(z() - q.z()) < tol);
}
void Point3::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
return gtsam::distance3(*this,q,H1,H2);
}
double Point3::norm(OptionalJacobian<1,3> H) const {
return gtsam::norm3(*this, H);
}
Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
return gtsam::normalize(*this, H);
}
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
return gtsam::cross(*this, q, H1, H2);
}
double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
return gtsam::dot(*this, q, H1, H2);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]'";
return os;
}
#endif
/* ************************************************************************* */
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {

103
gtsam/geometry/Point3.h
View File

@ -29,106 +29,9 @@
namespace gtsam {
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point3 to Vector3
typedef Vector3 Point3;
#else
/**
* A 3D point is just a Vector3 with some additional methods
* @addtogroup geometry
* \nosubgrouping
*/
class Point3 : public Vector3 {
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
using Vector3::Vector3;
/// @}
/// @name Testable
/// @{
/** print with optional string */
GTSAM_EXPORT void print(const std::string& s = "") const;
/** equals with an tolerance */
GTSAM_EXPORT bool equals(const Point3& p, double tol = 1e-9) const;
/// @}
/// @name Group
/// @{
/// identity for group operation
inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); }
/// @}
/// @name Vector Space
/// @{
/** distance between two points */
GTSAM_EXPORT double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const;
/** Distance of the point from the origin, with Jacobian */
GTSAM_EXPORT double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
GTSAM_EXPORT Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */
GTSAM_EXPORT Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
OptionalJacobian<3, 3> H_q = boost::none) const;
/** dot product @return this * q*/
GTSAM_EXPORT double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
OptionalJacobian<1, 3> H_q = boost::none) const;
/// @}
/// @name Standard Interface
/// @{
/// return as Vector3
const Vector3& vector() const { return *this; }
/// get x
inline double x() const {return (*this)[0];}
/// get y
inline double y() const {return (*this)[1];}
/// get z
inline double z() const {return (*this)[2];}
/// @}
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3);
}
};
template<>
struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point3 to Vector3
typedef Vector3 Point3;
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;

7
gtsam/geometry/Rot2.cpp
View File

@ -39,6 +39,13 @@ Rot2 Rot2::atan2(double y, double x) {
return R.normalize();
}
/* ************************************************************************* */
Rot2 Rot2::Random(std::mt19937& rng) {
uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
double angle = randomAngle(rng);
return fromAngle(angle);
}
/* ************************************************************************* */
void Rot2::print(const string& s) const {
cout << s << ": " << theta() << endl;

10
gtsam/geometry/Rot2.h
View File

@ -22,6 +22,8 @@
#include <gtsam/base/Lie.h>
#include <boost/optional.hpp>
#include <random>
namespace gtsam {
/**
@ -79,6 +81,14 @@ namespace gtsam {
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
static Rot2 atan2(double y, double x);
/**
* Random, generates random angle \in [-p,pi]
* Example:
* std::mt19937 engine(42);
* Unit3 unit = Unit3::Random(engine);
*/
static Rot2 Random(std::mt19937 & rng);
/// @}
/// @name Testable
/// @{

19
gtsam/geometry/SOn-inl.h
View File

@ -60,8 +60,9 @@ typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R,
template <int N>
typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
if (N==2) return I_1x1; // SO(2) case
throw std::runtime_error(
"SO<N>::AdjointMap only implemented for SO3 and SO4.");
"SO<N>::AdjointMap only implemented for SO2, SO3 and SO4.");
}
template <int N>
@ -84,26 +85,22 @@ typename SO<N>::MatrixDD SO<N>::LogmapDerivative(const TangentVector& omega) {
throw std::runtime_error("O<N>::LogmapDerivative only implemented for SO3.");
}
// Default fixed size version (but specialized elsewehere for N=2,3,4)
template <int N>
typename SO<N>::VectorN2 SO<N>::vec(
OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
const size_t n = rows();
const size_t n2 = n * n;
// Vectorize
VectorN2 X(n2);
X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
VectorN2 X = Eigen::Map<const VectorN2>(matrix_.data());
// If requested, calculate H as (I \oplus Q) * P,
// where Q is the N*N rotation matrix, and P is calculated below.
if (H) {
// Calculate P matrix of vectorized generators
// TODO(duy): Should we refactor this as the jacobian of Hat?
Matrix P = VectorizedGenerators(n);
const size_t d = dim();
H->resize(n2, d);
for (size_t i = 0; i < n; i++) {
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
Matrix P = SO<N>::VectorizedGenerators();
for (size_t i = 0; i < N; i++) {
H->block(i * N, 0, N, dimension) =
matrix_ * P.block(i * N, 0, N, dimension);
}
}
return X;

43
gtsam/geometry/SOn.cpp
View File

@ -22,21 +22,18 @@
namespace gtsam {
template <>
GTSAM_EXPORT
Matrix SOn::Hat(const Vector& xi) {
GTSAM_EXPORT void SOn::Hat(const Vector &xi, Eigen::Ref<Matrix> X) {
size_t n = AmbientDim(xi.size());
if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
Matrix X(n, n); // allocate space for n*n skew-symmetric matrix
X.setZero();
if (n == 2) {
if (n < 2)
throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
else if (n == 2) {
// Handle SO(2) case as recursion bottom
assert(xi.size() == 1);
X << 0, -xi(0), xi(0), 0;
} else {
// Recursively call SO(n-1) call for top-left block
const size_t dmin = (n - 1) * (n - 2) / 2;
X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
Hat(xi.tail(dmin), X.topLeftCorner(n - 1, n - 1));
// determine sign of last element (signs alternate)
double sign = pow(-1.0, xi.size());
@ -47,7 +44,14 @@ Matrix SOn::Hat(const Vector& xi) {
X(j, n - 1) = -X(n - 1, j);
sign = -sign;
}
X(n - 1, n - 1) = 0; // bottom-right
}
}
template <> GTSAM_EXPORT Matrix SOn::Hat(const Vector &xi) {
size_t n = AmbientDim(xi.size());
Matrix X(n, n); // allocate space for n*n skew-symmetric matrix
SOn::Hat(xi, X);
return X;
}
@ -99,4 +103,27 @@ SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
return result;
}
// Dynamic version of vec
template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const {
const size_t n = rows(), n2 = n * n;
// Vectorize
VectorN2 X(n2);
X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
// If requested, calculate H as (I \oplus Q) * P,
// where Q is the N*N rotation matrix, and P is calculated below.
if (H) {
// Calculate P matrix of vectorized generators
// TODO(duy): Should we refactor this as the jacobian of Hat?
Matrix P = SOn::VectorizedGenerators(n);
const size_t d = dim();
H->resize(n2, d);
for (size_t i = 0; i < n; i++) {
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
}
}
return X;
}
} // namespace gtsam

22
gtsam/geometry/SOn.h
View File

@ -98,8 +98,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
Matrix Q = Matrix::Identity(n, n);
size_t p = R.rows();
assert(p < n && R.cols() == p);
const int p = R.rows();
assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
Q.topLeftCorner(p, p) = R;
return SO(Q);
}
@ -208,7 +208,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
// Calculate run-time dimensionality of manifold.
// Available as dimension or Dim() for fixed N.
size_t dim() const { return Dimension(matrix_.rows()); }
size_t dim() const { return Dimension(static_cast<size_t>(matrix_.rows())); }
/**
* Hat operator creates Lie algebra element corresponding to d-vector, where d
@ -227,9 +227,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
*/
static MatrixNN Hat(const TangentVector& xi);
/**
* Inverse of Hat. See note about xi element order in Hat.
*/
/// In-place version of Hat (see details there), implements recursion.
static void Hat(const Vector &xi, Eigen::Ref<MatrixNN> X);
/// Inverse of Hat. See note about xi element order in Hat.
static TangentVector Vee(const MatrixNN& X);
// Chart at origin
@ -295,10 +296,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
template <int N_ = N, typename = IsFixed<N_>>
static Matrix VectorizedGenerators() {
constexpr size_t N2 = static_cast<size_t>(N * N);
Matrix G(N2, dimension);
Eigen::Matrix<double, N2, dimension> G;
for (size_t j = 0; j < dimension; j++) {
const auto X = Hat(Vector::Unit(dimension, j));
G.col(j) = Eigen::Map<const Matrix>(X.data(), N2, 1);
G.col(j) = Eigen::Map<const VectorN2>(X.data());
}
return G;
}
@ -362,6 +363,11 @@ template <>
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const;
/*
* Specialize dynamic vec.
*/
template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
/** Serialization function */
template<class Archive>
void serialize(

10
gtsam/geometry/tests/testPoint2.cpp
View File

@ -237,16 +237,6 @@ TEST( Point2, circleCircleIntersection) {
}
/* ************************************************************************* */
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
TEST( Point2, stream) {
Point2 p(1, 2);
std::ostringstream os;
os << p;
EXPECT(os.str() == "(1, 2)");
}
#endif
/* ************************************************************************* */
int main () {
TestResult tr;

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

@ -153,16 +153,6 @@ TEST( Point3, cross2) {
}
}
/* ************************************************************************* */
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
TEST( Point3, stream) {
Point3 p(1, 2, -3);
std::ostringstream os;
os << p;
EXPECT(os.str() == "[1, 2, -3]'");
}
#endif
//*************************************************************************
TEST (Point3, normalize) {
Matrix actualH;

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

@ -864,11 +864,7 @@ TEST( Pose3, stream)
os << T;
string expected;
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 0\n0\n0";;
#else
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: [0, 0, 0]'";
#endif
EXPECT(os.str() == expected);
}
@ -1043,13 +1039,9 @@ TEST(Pose3, print) {
// Add expected rotation
expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
expected << "t: 1\n"
"2\n"
"3\n";
#else
expected << "t: [" << translation.x() << ", " << translation.y() << ", " << translation.z() << "]'\n";
#endif
// reset cout to the original stream
std::cout.rdbuf(oldbuf);

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

@ -39,8 +39,8 @@ using namespace std;
using namespace gtsam;
//******************************************************************************
// Test dhynamic with n=0
TEST(SOn, SO0) {
// Test dynamic with n=0
TEST(SOn, SOn0) {
const auto R = SOn(0);
EXPECT_LONGS_EQUAL(0, R.rows());
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
@ -50,7 +50,8 @@ TEST(SOn, SO0) {
}
//******************************************************************************
TEST(SOn, SO5) {
// Test dynamic with n=5
TEST(SOn, SOn5) {
const auto R = SOn(5);
EXPECT_LONGS_EQUAL(5, R.rows());
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
@ -59,6 +60,28 @@ TEST(SOn, SO5) {
EXPECT_LONGS_EQUAL(10, traits<SOn>::GetDimension(R));
}
//******************************************************************************
// Test fixed with n=2
TEST(SOn, SO0) {
const auto R = SO<2>();
EXPECT_LONGS_EQUAL(2, R.rows());
EXPECT_LONGS_EQUAL(1, SO<2>::dimension);
EXPECT_LONGS_EQUAL(1, SO<2>::Dim());
EXPECT_LONGS_EQUAL(1, R.dim());
EXPECT_LONGS_EQUAL(1, traits<SO<2>>::GetDimension(R));
}
//******************************************************************************
// Test fixed with n=5
TEST(SOn, SO5) {
const auto R = SO<5>();
EXPECT_LONGS_EQUAL(5, R.rows());
EXPECT_LONGS_EQUAL(10, SO<5>::dimension);
EXPECT_LONGS_EQUAL(10, SO<5>::Dim());
EXPECT_LONGS_EQUAL(10, R.dim());
EXPECT_LONGS_EQUAL(10, traits<SO<5>>::GetDimension(R));
}
//******************************************************************************
TEST(SOn, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SOn>));
@ -105,29 +128,29 @@ TEST(SOn, HatVee) {
EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
Matrix expected3(3, 3);
expected3 << 0, -3, 2, //
3, 0, -1, //
-2, 1, 0;
expected3 << 0, -3, 2, //
3, 0, -1, //
-2, 1, 0;
const auto actual3 = SOn::Hat(v.head<3>());
EXPECT(assert_equal(expected3, actual3));
EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
Matrix expected4(4, 4);
expected4 << 0, -6, 5, 3, //
6, 0, -4, -2, //
-5, 4, 0, 1, //
-3, 2, -1, 0;
expected4 << 0, -6, 5, 3, //
6, 0, -4, -2, //
-5, 4, 0, 1, //
-3, 2, -1, 0;
const auto actual4 = SOn::Hat(v.head<6>());
EXPECT(assert_equal(expected4, actual4));
EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
Matrix expected5(5, 5);
expected5 << 0,-10, 9, 7, -4, //
10, 0, -8, -6, 3, //
-9, 8, 0, 5, -2, //
-7, 6, -5, 0, 1, //
4, -3, 2, -1, 0;
expected5 << 0, -10, 9, 7, -4, //
10, 0, -8, -6, 3, //
-9, 8, 0, 5, -2, //
-7, 6, -5, 0, 1, //
4, -3, 2, -1, 0;
const auto actual5 = SOn::Hat(v);
EXPECT(assert_equal(expected5, actual5));
EXPECT(assert_equal((Vector)v, SOn::Vee(actual5)));
@ -159,6 +182,22 @@ TEST(SOn, RetractLocal) {
CHECK(assert_equal(v1, SOn::ChartAtOrigin::Local(Q1), 1e-7));
}
//******************************************************************************
Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); }
/// Test Jacobian of Retract at origin
TEST(SOn, RetractJacobian) {
Matrix actualH = RetractJacobian(3);
boost::function<Matrix(const Vector &)> h = [](const Vector &v) {
return SOn::ChartAtOrigin::Retract(v).matrix();
};
Vector3 v;
v.setZero();
const Matrix expectedH = numericalDerivative11<Matrix, Vector, 3>(h, v, 1e-5);
CHECK(assert_equal(expectedH, actualH));
}
//******************************************************************************
TEST(SOn, vec) {
Vector10 v;
@ -166,11 +205,28 @@ TEST(SOn, vec) {
SOn Q = SOn::ChartAtOrigin::Retract(v);
Matrix actualH;
const Vector actual = Q.vec(actualH);
boost::function<Vector(const SOn&)> h = [](const SOn& Q) { return Q.vec(); };
boost::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
CHECK(assert_equal(H, actualH));
}
//******************************************************************************
TEST(SOn, VectorizedGenerators) {
// Default fixed
auto actual2 = SO<2>::VectorizedGenerators();
CHECK(actual2.rows()==4 && actual2.cols()==1)
// Specialized
auto actual3 = SO<3>::VectorizedGenerators();
CHECK(actual3.rows()==9 && actual3.cols()==3)
auto actual4 = SO<4>::VectorizedGenerators();
CHECK(actual4.rows()==16 && actual4.cols()==6)
// Dynamic
auto actual5 = SOn::VectorizedGenerators(5);
CHECK(actual5.rows()==25 && actual5.cols()==10)
}
//******************************************************************************
int main() {
TestResult tr;

156
gtsam.h → gtsam/gtsam.i
View File

@ -618,6 +618,9 @@ class SOn {
// Other methods
Vector vec() const;
Matrix matrix() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Rot3.h>
@ -2171,7 +2174,11 @@ class Values {
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
// The order is important: Vector has to precede Point2/Point3 so `atVector`
// can work for those fixed-size vectors.
void insert(size_t j, Vector vector);
void insert(size_t j, Matrix matrix);
void insert(size_t j, const gtsam::Point2& point2);
void insert(size_t j, const gtsam::Point3& point3);
void insert(size_t j, const gtsam::Rot2& rot2);
@ -2188,8 +2195,6 @@ class Values {
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, const gtsam::NavState& nav_state);
void insert(size_t j, Vector vector);
void insert(size_t j, Matrix matrix);
void update(size_t j, const gtsam::Point2& point2);
void update(size_t j, const gtsam::Point3& point3);
@ -2797,16 +2802,16 @@ class SfmData {
string findExampleDataFile(string name);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID);
gtsam::noiseModel::Diagonal* model, int maxIndex);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
gtsam::noiseModel::Base* model);
gtsam::noiseModel::Base* model, int maxIndex);
void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
@ -2816,8 +2821,8 @@ class BetweenFactorPose3s
{
BetweenFactorPose3s();
size_t size() const;
gtsam::BetweenFactorPose3* at(size_t i) const;
void push_back(const gtsam::BetweenFactorPose3* factor);
gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
};
#include <gtsam/slam/InitializePose3.h>
@ -2855,7 +2860,7 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
};
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
gtsam::noiseModel::Isotropic* ConvertNoiseModel(
gtsam::noiseModel::Base* model, size_t d);
template<T = {gtsam::SO3, gtsam::SO4}>
@ -2874,12 +2879,14 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
Vector evaluateError(const T& R1, const T& R2);
};
virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
#include <gtsam/sfm/ShonanFactor.h>
virtual class ShonanFactor3 : gtsam::NoiseModelFactor {
ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12,
size_t p);
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
size_t p, gtsam::noiseModel::Base* model);
Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12,
size_t p, gtsam::noiseModel::Base *model);
Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2);
};
#include <gtsam/sfm/BinaryMeasurement.h>
@ -2895,6 +2902,125 @@ class BinaryMeasurement {
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
#include <gtsam/sfm/ShonanAveraging.h>
// TODO(frank): copy/pasta below until we have integer template paremeters in wrap!
class ShonanAveragingParameters2 {
ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm);
ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, string method);
gtsam::LevenbergMarquardtParams getLMParams() const;
void setOptimalityThreshold(double value);
double getOptimalityThreshold() const;
void setAnchor(size_t index, const gtsam::Rot2& value);
void setAnchorWeight(double value);
double getAnchorWeight() const;
void setKarcherWeight(double value);
double getKarcherWeight();
void setGaugesWeight(double value);
double getGaugesWeight();
};
class ShonanAveragingParameters3 {
ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm);
ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, string method);
gtsam::LevenbergMarquardtParams getLMParams() const;
void setOptimalityThreshold(double value);
double getOptimalityThreshold() const;
void setAnchor(size_t index, const gtsam::Rot3& value);
void setAnchorWeight(double value);
double getAnchorWeight() const;
void setKarcherWeight(double value);
double getKarcherWeight();
void setGaugesWeight(double value);
double getGaugesWeight();
};
class ShonanAveraging2 {
ShonanAveraging2(string g2oFile);
ShonanAveraging2(string g2oFile,
const gtsam::ShonanAveragingParameters2 &parameters);
// Query properties
size_t nrUnknowns() const;
size_t nrMeasurements() const;
gtsam::Rot2 measured(size_t i);
gtsam::KeyVector keys(size_t i);
// Matrix API (advanced use, debugging)
Matrix denseD() const;
Matrix denseQ() const;
Matrix denseL() const;
// Matrix computeLambda_(Matrix S) const;
Matrix computeLambda_(const gtsam::Values& values) const;
Matrix computeA_(const gtsam::Values& values) const;
double computeMinEigenValue(const gtsam::Values& values) const;
gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
const Vector& minEigenVector, double minEigenValue) const;
// Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
gtsam::Values initializeRandomlyAt(size_t p) const;
double costAt(size_t p, const gtsam::Values& values) const;
pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
bool checkOptimality(const gtsam::Values& values) const;
gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial);
// gtsam::Values tryOptimizingAt(size_t p) const;
gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
gtsam::Values roundSolution(const gtsam::Values& values) const;
// Basic API
double cost(const gtsam::Values& values) const;
gtsam::Values initializeRandomly() const;
pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
};
class ShonanAveraging3 {
ShonanAveraging3(string g2oFile);
ShonanAveraging3(string g2oFile,
const gtsam::ShonanAveragingParameters3 &parameters);
// TODO(frank): deprecate once we land pybind wrapper
ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors);
ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors,
const gtsam::ShonanAveragingParameters3 &parameters);
// Query properties
size_t nrUnknowns() const;
size_t nrMeasurements() const;
gtsam::Rot3 measured(size_t i);
gtsam::KeyVector keys(size_t i);
// Matrix API (advanced use, debugging)
Matrix denseD() const;
Matrix denseQ() const;
Matrix denseL() const;
// Matrix computeLambda_(Matrix S) const;
Matrix computeLambda_(const gtsam::Values& values) const;
Matrix computeA_(const gtsam::Values& values) const;
double computeMinEigenValue(const gtsam::Values& values) const;
gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
const Vector& minEigenVector, double minEigenValue) const;
// Advanced API
gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
gtsam::Values initializeRandomlyAt(size_t p) const;
double costAt(size_t p, const gtsam::Values& values) const;
pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
bool checkOptimality(const gtsam::Values& values) const;
gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial);
// gtsam::Values tryOptimizingAt(size_t p) const;
gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
gtsam::Values roundSolution(const gtsam::Values& values) const;
// Basic API
double cost(const gtsam::Values& values) const;
gtsam::Values initializeRandomly() const;
pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
};
//*************************************************************************
// Navigation
//*************************************************************************

4
gtsam/navigation/tests/testNavState.cpp
View File

@ -215,11 +215,7 @@ TEST(NavState, Stream)
os << state;
string expected;
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0\n0\n0\nv: 0\n0\n0";
#else
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: [0, 0, 0]'\nv: [0, 0, 0]'";
#endif
EXPECT(os.str() == expected);
}

2
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -90,7 +90,7 @@ void NonlinearOptimizer::defaultOptimize() {
// Iterative loop
do {
// Do next iteration
currentError = error();
currentError = error(); // TODO(frank): don't do this twice at first !? Computed above!
iterate();
tictoc_finishedIteration();

854
gtsam/sfm/ShonanAveraging.cpp Normal file
View File

@ -0,0 +1,854 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 ShonanAveraging.cpp
* @date March 2019 - August 2020
* @author Frank Dellaert, David Rosen, and Jing Wu
* @brief Shonan Averaging algorithm
*/
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/sfm/ShonanGaugeFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h>
#include <gtsam/sfm/ShonanFactor.h>
#include <Eigen/Eigenvalues>
#include <SymEigsSolver.h>
#include <algorithm>
#include <complex>
#include <iostream>
#include <map>
#include <random>
#include <vector>
namespace gtsam {
// In Wrappers we have no access to this so have a default ready
static std::mt19937 kRandomNumberGenerator(42);
using Sparse = Eigen::SparseMatrix<double>;
/* ************************************************************************* */
template <size_t d>
ShonanAveragingParameters<d>::ShonanAveragingParameters(
const LevenbergMarquardtParams &_lm, const std::string &method,
double optimalityThreshold, double alpha, double beta, double gamma)
: lm(_lm), optimalityThreshold(optimalityThreshold), alpha(alpha),
beta(beta), gamma(gamma) {
// By default, we will do conjugate gradient
lm.linearSolverType = LevenbergMarquardtParams::Iterative;
// Create subgraph builder parameters
SubgraphBuilderParameters builderParameters;
builderParameters.skeletonType = SubgraphBuilderParameters::KRUSKAL;
builderParameters.skeletonWeight = SubgraphBuilderParameters::EQUAL;
builderParameters.augmentationWeight = SubgraphBuilderParameters::SKELETON;
builderParameters.augmentationFactor = 0.0;
auto pcg = boost::make_shared<PCGSolverParameters>();
// Choose optimization method
if (method == "SUBGRAPH") {
lm.iterativeParams =
boost::make_shared<SubgraphSolverParameters>(builderParameters);
} else if (method == "SGPC") {
pcg->preconditioner_ =
boost::make_shared<SubgraphPreconditionerParameters>(builderParameters);
lm.iterativeParams = pcg;
} else if (method == "JACOBI") {
pcg->preconditioner_ =
boost::make_shared<BlockJacobiPreconditionerParameters>();
lm.iterativeParams = pcg;
} else if (method == "QR") {
lm.setLinearSolverType("MULTIFRONTAL_QR");
} else if (method == "CHOLESKY") {
lm.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
} else {
throw std::invalid_argument("ShonanAveragingParameters: unknown method");
}
}
/* ************************************************************************* */
// Explicit instantiation for d=2 and d=3
template struct ShonanAveragingParameters<2>;
template struct ShonanAveragingParameters<3>;
/* ************************************************************************* */
// Calculate number of unknown rotations referenced by measurements
template <size_t d>
static size_t
NrUnknowns(const typename ShonanAveraging<d>::Measurements &measurements) {
std::set<Key> keys;
for (const auto &measurement : measurements) {
keys.insert(measurement.key1());
keys.insert(measurement.key2());
}
return keys.size();
}
/* ************************************************************************* */
template <size_t d>
ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
const Parameters &parameters)
: parameters_(parameters), measurements_(measurements),
nrUnknowns_(NrUnknowns<d>(measurements)) {
for (const auto &measurement : measurements_) {
const auto &model = measurement.noiseModel();
if (model && model->dim() != SO<d>::dimension) {
measurement.print("Factor with incorrect noise model:\n");
throw std::invalid_argument("ShonanAveraging: measurements passed to "
"constructor have incorrect dimension.");
}
}
Q_ = buildQ();
D_ = buildD();
L_ = D_ - Q_;
}
/* ************************************************************************* */
template <size_t d>
NonlinearFactorGraph ShonanAveraging<d>::buildGraphAt(size_t p) const {
NonlinearFactorGraph graph;
auto G = boost::make_shared<Matrix>(SO<-1>::VectorizedGenerators(p));
for (const auto &measurement : measurements_) {
const auto &keys = measurement.keys();
const auto &Rij = measurement.measured();
const auto &model = measurement.noiseModel();
graph.emplace_shared<ShonanFactor<d>>(keys[0], keys[1], Rij, p, model, G);
}
// Possibly add Karcher prior
if (parameters_.beta > 0) {
const size_t dim = SOn::Dimension(p);
graph.emplace_shared<KarcherMeanFactor<SOn>>(graph.keys(), dim);
}
// Possibly add gauge factors - they are probably useless as gradient is zero
if (parameters_.gamma > 0 && p > d + 1) {
for (auto key : graph.keys())
graph.emplace_shared<ShonanGaugeFactor>(key, p, d, parameters_.gamma);
}
return graph;
}
/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::costAt(size_t p, const Values &values) const {
const NonlinearFactorGraph graph = buildGraphAt(p);
return graph.error(values);
}
/* ************************************************************************* */
template <size_t d>
boost::shared_ptr<LevenbergMarquardtOptimizer>
ShonanAveraging<d>::createOptimizerAt(size_t p, const Values &initial) const {
// Build graph
NonlinearFactorGraph graph = buildGraphAt(p);
// Anchor prior is added here as depends on initial value (and cost is zero)
if (parameters_.alpha > 0) {
size_t i;
Rot value;
const size_t dim = SOn::Dimension(p);
std::tie(i, value) = parameters_.anchor;
auto model = noiseModel::Isotropic::Precision(dim, parameters_.alpha);
graph.emplace_shared<PriorFactor<SOn>>(i, SOn::Lift(p, value.matrix()),
model);
}
// Optimize
return boost::make_shared<LevenbergMarquardtOptimizer>(graph, initial,
parameters_.lm);
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::tryOptimizingAt(size_t p,
const Values &initial) const {
auto lm = createOptimizerAt(p, initial);
return lm->optimize();
}
/* ************************************************************************* */
// Project to pxdN Stiefel manifold
template <size_t d>
Matrix ShonanAveraging<d>::StiefelElementMatrix(const Values &values) {
const size_t N = values.size();
const size_t p = values.at<SOn>(0).rows();
Matrix S(p, N * d);
for (const auto it : values.filter<SOn>()) {
S.middleCols<d>(it.key * d) =
it.value.matrix().leftCols<d>(); // project Qj to Stiefel
}
return S;
}
/* ************************************************************************* */
template <>
Values ShonanAveraging<2>::projectFrom(size_t p, const Values &values) const {
Values result;
for (const auto it : values.filter<SOn>()) {
assert(it.value.rows() == p);
const auto &M = it.value.matrix();
const Rot2 R = Rot2::atan2(M(1, 0), M(0, 0));
result.insert(it.key, R);
}
return result;
}
template <>
Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const {
Values result;
for (const auto it : values.filter<SOn>()) {
assert(it.value.rows() == p);
const auto &M = it.value.matrix();
const Rot3 R = Rot3::ClosestTo(M.topLeftCorner<3, 3>());
result.insert(it.key, R);
}
return result;
}
/* ************************************************************************* */
template <size_t d> static Matrix RoundSolutionS(const Matrix &S) {
const size_t N = S.cols() / d;
// First, compute a thin SVD of S
Eigen::JacobiSVD<Matrix> svd(S, Eigen::ComputeThinV);
const Vector sigmas = svd.singularValues();
// Construct a diagonal matrix comprised of the first d singular values
using DiagonalMatrix = Eigen::DiagonalMatrix<double, d>;
DiagonalMatrix Sigma_d;
Sigma_d.diagonal() = sigmas.head<d>();
// Now, construct a rank-d truncated singular value decomposition for S
Matrix R = Sigma_d * svd.matrixV().leftCols<d>().transpose();
// Count the number of blocks whose determinants have positive sign
size_t numPositiveBlocks = 0;
for (size_t i = 0; i < N; ++i) {
// Compute the determinant of the ith dxd block of R
double determinant = R.middleCols<d>(d * i).determinant();
if (determinant > 0)
++numPositiveBlocks;
}
if (numPositiveBlocks < N / 2) {
// Less than half of the total number of blocks have the correct sign.
// To reverse their orientations, multiply with a reflection matrix.
DiagonalMatrix reflector;
reflector.setIdentity();
reflector.diagonal()(d - 1) = -1;
R = reflector * R;
}
return R;
}
/* ************************************************************************* */
template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
// Round to a 2*2N matrix
Matrix R = RoundSolutionS<2>(S);
// Finally, project each dxd rotation block to SO(2)
Values values;
for (size_t j = 0; j < nrUnknowns(); ++j) {
const Rot2 Ri = Rot2::atan2(R(1, 2 * j), R(0, 2 * j));
values.insert(j, Ri);
}
return values;
}
template <> Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const {
// Round to a 3*3N matrix
Matrix R = RoundSolutionS<3>(S);
// Finally, project each dxd rotation block to SO(3)
Values values;
for (size_t j = 0; j < nrUnknowns(); ++j) {
const Rot3 Ri = Rot3::ClosestTo(R.middleCols<3>(3 * j));
values.insert(j, Ri);
}
return values;
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::roundSolution(const Values &values) const {
// Project to pxdN Stiefel manifold...
Matrix S = StiefelElementMatrix(values);
// ...and call version above.
return roundSolutionS(S);
}
/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::cost(const Values &values) const {
NonlinearFactorGraph graph;
for (const auto &measurement : measurements_) {
const auto &keys = measurement.keys();
const auto &Rij = measurement.measured();
const auto &model = measurement.noiseModel();
graph.emplace_shared<FrobeniusBetweenFactor<SO<d>>>(
keys[0], keys[1], SO<d>(Rij.matrix()), model);
}
// Finally, project each dxd rotation block to SO(d)
Values result;
for (const auto it : values.filter<Rot>()) {
result.insert(it.key, SO<d>(it.value.matrix()));
}
return graph.error(result);
}
/* ************************************************************************* */
// Get kappa from noise model
template <typename T>
static double Kappa(const BinaryMeasurement<T> &measurement) {
const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
measurement.noiseModel());
if (!isotropic) {
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic.");
}
const double sigma = isotropic->sigma();
return 1.0 / (sigma * sigma);
}
/* ************************************************************************* */
template <size_t d> Sparse ShonanAveraging<d>::buildD() const {
// Each measurement contributes 2*d elements along the diagonal of the
// degree matrix.
static constexpr size_t stride = 2 * d;
// Reserve space for triplets
std::vector<Eigen::Triplet<double>> triplets;
triplets.reserve(stride * measurements_.size());
for (const auto &measurement : measurements_) {
// Get pose keys
const auto &keys = measurement.keys();
// Get kappa from noise model
double kappa = Kappa<Rot>(measurement);
const size_t di = d * keys[0], dj = d * keys[1];
for (size_t k = 0; k < d; k++) {
// Elements of ith block-diagonal
triplets.emplace_back(di + k, di + k, kappa);
// Elements of jth block-diagonal
triplets.emplace_back(dj + k, dj + k, kappa);
}
}
// Construct and return a sparse matrix from these triplets
const size_t dN = d * nrUnknowns();
Sparse D(dN, dN);
D.setFromTriplets(triplets.begin(), triplets.end());
return D;
}
/* ************************************************************************* */
template <size_t d> Sparse ShonanAveraging<d>::buildQ() const {
// Each measurement contributes 2*d^2 elements on a pair of symmetric
// off-diagonal blocks
static constexpr size_t stride = 2 * d * d;
// Reserve space for triplets
std::vector<Eigen::Triplet<double>> triplets;
triplets.reserve(stride * measurements_.size());
for (const auto &measurement : measurements_) {
// Get pose keys
const auto &keys = measurement.keys();
// Extract rotation measurement
const auto Rij = measurement.measured().matrix();
// Get kappa from noise model
double kappa = Kappa<Rot>(measurement);
const size_t di = d * keys[0], dj = d * keys[1];
for (size_t r = 0; r < d; r++) {
for (size_t c = 0; c < d; c++) {
// Elements of ij block
triplets.emplace_back(di + r, dj + c, kappa * Rij(r, c));
// Elements of ji block
triplets.emplace_back(dj + r, di + c, kappa * Rij(c, r));
}
}
}
// Construct and return a sparse matrix from these triplets
const size_t dN = d * nrUnknowns();
Sparse Q(dN, dN);
Q.setFromTriplets(triplets.begin(), triplets.end());
return Q;
}
/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeLambda(const Matrix &S) const {
// Each pose contributes 2*d elements along the diagonal of Lambda
static constexpr size_t stride = d * d;
// Reserve space for triplets
const size_t N = nrUnknowns();
std::vector<Eigen::Triplet<double>> triplets;
triplets.reserve(stride * N);
// Do sparse-dense multiply to get Q*S'
auto QSt = Q_ * S.transpose();
for (size_t j = 0; j < N; j++) {
// Compute B, the building block for the j^th diagonal block of Lambda
const size_t dj = d * j;
Matrix B = QSt.middleRows(dj, d) * S.middleCols<d>(dj);
// Elements of jth block-diagonal
for (size_t r = 0; r < d; r++)
for (size_t c = 0; c < d; c++)
triplets.emplace_back(dj + r, dj + c, 0.5 * (B(r, c) + B(c, r)));
}
// Construct and return a sparse matrix from these triplets
Sparse Lambda(d * N, d * N);
Lambda.setFromTriplets(triplets.begin(), triplets.end());
return Lambda;
}
/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeLambda(const Values &values) const {
// Project to pxdN Stiefel manifold...
Matrix S = StiefelElementMatrix(values);
// ...and call version above.
return computeLambda(S);
}
/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeA(const Values &values) const {
assert(values.size() == nrUnknowns());
const Matrix S = StiefelElementMatrix(values);
auto Lambda = computeLambda(S);
return Lambda - Q_;
}
/* ************************************************************************* */
/// MINIMUM EIGENVALUE COMPUTATIONS
/** This is a lightweight struct used in conjunction with Spectra to compute
* the minimum eigenvalue and eigenvector of a sparse matrix A; it has a single
* nontrivial function, perform_op(x,y), that computes and returns the product
* y = (A + sigma*I) x */
struct MatrixProdFunctor {
// Const reference to an externally-held matrix whose minimum-eigenvalue we
// want to compute
const Sparse &A_;
// Spectral shift
double sigma_;
// Constructor
explicit MatrixProdFunctor(const Sparse &A, double sigma = 0)
: A_(A), sigma_(sigma) {}
int rows() const { return A_.rows(); }
int cols() const { return A_.cols(); }
// Matrix-vector multiplication operation
void perform_op(const double *x, double *y) const {
// Wrap the raw arrays as Eigen Vector types
Eigen::Map<const Vector> X(x, rows());
Eigen::Map<Vector> Y(y, rows());
// Do the multiplication using wrapped Eigen vectors
Y = A_ * X + sigma_ * X;
}
};
/// Function to compute the minimum eigenvalue of A using Lanczos in Spectra.
/// This does 2 things:
///
/// (1) Quick (coarse) eigenvalue computation to estimate the largest-magnitude
/// eigenvalue (2) A second eigenvalue computation applied to A-sigma*I, where
/// sigma is chosen to make the minimum eigenvalue of A the extremal eigenvalue
/// of A-sigma*I
///
/// Upon completion, this returns a boolean value indicating whether the minimum
/// eigenvalue was computed to the required precision -- if so, its sets the
/// values of minEigenValue and minEigenVector appropriately
/// Note that in the following function signature, S is supposed to be the
/// block-row-matrix that is a critical point for the optimization algorithm;
/// either S (Stiefel manifold) or R (block rotations). We use this to
/// construct a starting vector v for the Lanczos process that will be close to
/// the minimum eigenvector we're looking for whenever the relaxation is exact
/// -- this is a key feature that helps to make this method fast. Note that
/// instead of passing in all of S, it would be enough to pass in one of S's
/// *rows*, if that's more convenient.
// For the defaults, David Rosen says:
// - maxIterations refers to the max number of Lanczos iterations to run;
// ~1000 should be sufficiently large
// - We've been using 10^-4 for the nonnegativity tolerance
// - for numLanczosVectors, 20 is a good default value
static bool
SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
Vector *minEigenVector = 0, size_t *numIterations = 0,
size_t maxIterations = 1000,
double minEigenvalueNonnegativityTolerance = 10e-4,
Eigen::Index numLanczosVectors = 20) {
// a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos
MatrixProdFunctor lmOperator(A);
Spectra::SymEigsSolver<double, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN,
MatrixProdFunctor>
lmEigenValueSolver(&lmOperator, 1, std::min(numLanczosVectors, A.rows()));
lmEigenValueSolver.init();
const int lmConverged = lmEigenValueSolver.compute(
maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
// Check convergence and bail out if necessary
if (lmConverged != 1)
return false;
const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
if (lmEigenValue < 0) {
// The largest-magnitude eigenvalue is negative, and therefore also the
// minimum eigenvalue, so just return this solution
*minEigenValue = lmEigenValue;
if (minEigenVector) {
*minEigenVector = lmEigenValueSolver.eigenvectors(1).col(0);
minEigenVector->normalize(); // Ensure that this is a unit vector
}
return true;
}
// The largest-magnitude eigenvalue is positive, and is therefore the
// maximum eigenvalue. Therefore, after shifting the spectrum of A
// by -2*lmEigenValue (by forming A - 2*lambda_max*I), the shifted
// spectrum will lie in the interval [minEigenValue(A) - 2* lambda_max(A),
// -lambda_max*A]; in particular, the largest-magnitude eigenvalue of
// A - 2*lambda_max*I is minEigenValue - 2*lambda_max, with corresponding
// eigenvector v_min
MatrixProdFunctor minShiftedOperator(A, -2 * lmEigenValue);
Spectra::SymEigsSolver<double, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN,
MatrixProdFunctor>
minEigenValueSolver(&minShiftedOperator, 1,
std::min(numLanczosVectors, A.rows()));
// If S is a critical point of F, then S^T is also in the null space of S -
// Lambda(S) (cf. Lemma 6 of the tech report), and therefore its rows are
// eigenvectors corresponding to the eigenvalue 0. In the case that the
// relaxation is exact, this is the *minimum* eigenvalue, and therefore the
// rows of S are exactly the eigenvectors that we're looking for. On the
// other hand, if the relaxation is *not* exact, then S - Lambda(S) has at
// least one strictly negative eigenvalue, and the rows of S are *unstable
// fixed points* for the Lanczos iterations. Thus, we will take a slightly
// "fuzzed" version of the first row of S as an initialization for the
// Lanczos iterations; this allows for rapid convergence in the case that
// the relaxation is exact (since are starting close to a solution), while
// simultaneously allowing the iterations to escape from this fixed point in
// the case that the relaxation is not exact.
Vector v0 = S.row(0).transpose();
Vector perturbation(v0.size());
perturbation.setRandom();
perturbation.normalize();
Vector xinit = v0 + (.03 * v0.norm()) * perturbation; // Perturb v0 by ~3%
// Use this to initialize the eigensolver
minEigenValueSolver.init(xinit.data());
// Now determine the relative precision required in the Lanczos method in
// order to be able to estimate the smallest eigenvalue within an *absolute*
// tolerance of 'minEigenvalueNonnegativityTolerance'
const int minConverged = minEigenValueSolver.compute(
maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
if (minConverged != 1)
return false;
*minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue;
if (minEigenVector) {
*minEigenVector = minEigenValueSolver.eigenvectors(1).col(0);
minEigenVector->normalize(); // Ensure that this is a unit vector
}
if (numIterations)
*numIterations = minEigenValueSolver.num_iterations();
return true;
}
/* ************************************************************************* */
template <size_t d> Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
auto Lambda = computeLambda(S);
return Lambda - Q_;
}
/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
Vector *minEigenVector) const {
assert(values.size() == nrUnknowns());
const Matrix S = StiefelElementMatrix(values);
auto A = computeA(S);
double minEigenValue;
bool success = SparseMinimumEigenValue(A, S, &minEigenValue, minEigenVector);
if (!success) {
throw std::runtime_error(
"SparseMinimumEigenValue failed to compute minimum eigenvalue.");
}
return minEigenValue;
}
/* ************************************************************************* */
template <size_t d>
std::pair<double, Vector>
ShonanAveraging<d>::computeMinEigenVector(const Values &values) const {
Vector minEigenVector;
double minEigenValue = computeMinEigenValue(values, &minEigenVector);
return std::make_pair(minEigenValue, minEigenVector);
}
/* ************************************************************************* */
template <size_t d>
bool ShonanAveraging<d>::checkOptimality(const Values &values) const {
double minEigenValue = computeMinEigenValue(values);
return minEigenValue > parameters_.optimalityThreshold;
}
/* ************************************************************************* */
/// Create a tangent direction xi with eigenvector segment v_i
template <size_t d>
Vector ShonanAveraging<d>::MakeATangentVector(size_t p, const Vector &v,
size_t i) {
// Create a tangent direction xi with eigenvector segment v_i
const size_t dimension = SOn::Dimension(p);
const auto v_i = v.segment<d>(d * i);
Vector xi = Vector::Zero(dimension);
double sign = pow(-1.0, round((p + 1) / 2) + 1);
for (size_t j = 0; j < d; j++) {
xi(j + p - d - 1) = sign * v_i(d - j - 1);
sign = -sign;
}
return xi;
}
/* ************************************************************************* */
template <size_t d>
Matrix ShonanAveraging<d>::riemannianGradient(size_t p,
const Values &values) const {
Matrix S_dot = StiefelElementMatrix(values);
// calculate the gradient of F(Q_dot) at Q_dot
Matrix euclideanGradient = 2 * (L_ * (S_dot.transpose())).transpose();
// cout << "euclidean gradient rows and cols" << euclideanGradient.rows() <<
// "\t" << euclideanGradient.cols() << endl;
// project the gradient onto the entire euclidean space
Matrix symBlockDiagProduct(p, d * nrUnknowns());
for (size_t i = 0; i < nrUnknowns(); i++) {
// Compute block product
const size_t di = d * i;
const Matrix P = S_dot.middleCols<d>(di).transpose() *
euclideanGradient.middleCols<d>(di);
// Symmetrize this block
Matrix S = .5 * (P + P.transpose());
// Compute S_dot * S and set corresponding block
symBlockDiagProduct.middleCols<d>(di) = S_dot.middleCols<d>(di) * S;
}
Matrix riemannianGradient = euclideanGradient - symBlockDiagProduct;
return riemannianGradient;
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::LiftwithDescent(size_t p, const Values &values,
const Vector &minEigenVector) {
Values lifted = LiftTo<SOn>(p, values);
for (auto it : lifted.filter<SOn>()) {
// Create a tangent direction xi with eigenvector segment v_i
// Assumes key is 0-based integer
const Vector xi = MakeATangentVector(p, minEigenVector, it.key);
// Move the old value in the descent direction
it.value = it.value.retract(xi);
}
return lifted;
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeWithDescent(
size_t p, const Values &values, const Vector &minEigenVector,
double minEigenValue, double gradienTolerance,
double preconditionedGradNormTolerance) const {
double funcVal = costAt(p - 1, values);
double alphaMin = 1e-2;
double alpha =
std::max(1024 * alphaMin, 10 * gradienTolerance / fabs(minEigenValue));
vector<double> alphas;
vector<double> fvals;
// line search
while ((alpha >= alphaMin)) {
Values Qplus = LiftwithDescent(p, values, alpha * minEigenVector);
double funcValTest = costAt(p, Qplus);
Matrix gradTest = riemannianGradient(p, Qplus);
double gradTestNorm = gradTest.norm();
// Record alpha and funcVal
alphas.push_back(alpha);
fvals.push_back(funcValTest);
if ((funcVal > funcValTest) && (gradTestNorm > gradienTolerance)) {
return Qplus;
}
alpha /= 2;
}
auto fminIter = min_element(fvals.begin(), fvals.end());
auto minIdx = distance(fvals.begin(), fminIter);
double fMin = fvals[minIdx];
double aMin = alphas[minIdx];
if (fMin < funcVal) {
Values Qplus = LiftwithDescent(p, values, aMin * minEigenVector);
return Qplus;
}
return LiftwithDescent(p, values, alpha * minEigenVector);
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomly(std::mt19937 &rng) const {
Values initial;
for (size_t j = 0; j < nrUnknowns(); j++) {
initial.insert(j, Rot::Random(rng));
}
return initial;
}
/* ************************************************************************* */
template <size_t d> Values ShonanAveraging<d>::initializeRandomly() const {
return initializeRandomly(kRandomNumberGenerator);
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomlyAt(size_t p,
std::mt19937 &rng) const {
const Values randomRotations = initializeRandomly(rng);
return LiftTo<Rot3>(p, randomRotations); // lift to p!
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomlyAt(size_t p) const {
return initializeRandomlyAt(p, kRandomNumberGenerator);
}
/* ************************************************************************* */
template <size_t d>
std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
size_t pMin,
size_t pMax) const {
Values Qstar;
Values initialSOp = LiftTo<Rot>(pMin, initialEstimate); // lift to pMin!
for (size_t p = pMin; p <= pMax; p++) {
// Optimize until convergence at this level
Qstar = tryOptimizingAt(p, initialSOp);
// Check certificate of global optimzality
Vector minEigenVector;
double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
if (minEigenValue > parameters_.optimalityThreshold) {
// If at global optimum, round and return solution
const Values SO3Values = roundSolution(Qstar);
return std::make_pair(SO3Values, minEigenValue);
}
// Not at global optimimum yet, so check whether we will go to next level
if (p != pMax) {
// Calculate initial estimate for next level by following minEigenVector
initialSOp =
initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue);
}
}
throw std::runtime_error("Shonan::run did not converge for given pMax");
}
/* ************************************************************************* */
// Explicit instantiation for d=2
template class ShonanAveraging<2>;
ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<2>(measurements, parameters) {}
ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
: ShonanAveraging<2>(parseMeasurements<Rot2>(g2oFile), parameters) {}
/* ************************************************************************* */
// Explicit instantiation for d=3
template class ShonanAveraging<3>;
ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<3>(measurements, parameters) {}
ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
: ShonanAveraging<3>(parseMeasurements<Rot3>(g2oFile), parameters) {}
// TODO(frank): Deprecate after we land pybind wrapper
// Extract Rot3 measurement from Pose3 betweenfactors
// Modeled after similar function in dataset.cpp
static BinaryMeasurement<Rot3>
convert(const BetweenFactor<Pose3>::shared_ptr &f) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian)
throw std::invalid_argument(
"parseMeasurements<Rot3> can only convert Pose3 measurements "
"with Gaussian noise models.");
const Matrix6 M = gaussian->covariance();
return BinaryMeasurement<Rot3>(
f->key1(), f->key2(), f->measured().rotation(),
noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
}
static ShonanAveraging3::Measurements
extractRot3Measurements(const BetweenFactorPose3s &factors) {
ShonanAveraging3::Measurements result;
result.reserve(factors.size());
for (auto f : factors)
result.push_back(convert(f));
return result;
}
ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
const Parameters &parameters)
: ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {}
/* ************************************************************************* */
} // namespace gtsam

365
gtsam/sfm/ShonanAveraging.h Normal file
View File

@ -0,0 +1,365 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 ShonanAveraging.h
* @date March 2019 - August 2020
* @author Frank Dellaert, David Rosen, and Jing Wu
* @brief Shonan Averaging algorithm
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/dllexport.h>
#include <Eigen/Sparse>
#include <map>
#include <string>
#include <type_traits>
#include <utility>
namespace gtsam {
class NonlinearFactorGraph;
class LevenbergMarquardtOptimizer;
/// Parameters governing optimization etc.
template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters {
// Select Rot2 or Rot3 interface based template parameter d
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
using Anchor = std::pair<size_t, Rot>;
// Paremeters themselves:
LevenbergMarquardtParams lm; // LM parameters
double optimalityThreshold; // threshold used in checkOptimality
Anchor anchor; // pose to use as anchor if not Karcher
double alpha; // weight of anchor-based prior (default 0)
double beta; // weight of Karcher-based prior (default 1)
double gamma; // weight of gauge-fixing factors (default 0)
ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
LevenbergMarquardtParams::CeresDefaults(),
const std::string &method = "JACOBI",
double optimalityThreshold = -1e-4,
double alpha = 0.0, double beta = 1.0,
double gamma = 0.0);
LevenbergMarquardtParams getLMParams() const { return lm; }
void setOptimalityThreshold(double value) { optimalityThreshold = value; }
double getOptimalityThreshold() const { return optimalityThreshold; }
void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
void setAnchorWeight(double value) { alpha = value; }
double getAnchorWeight() { return alpha; }
void setKarcherWeight(double value) { beta = value; }
double getKarcherWeight() { return beta; }
void setGaugesWeight(double value) { gamma = value; }
double getGaugesWeight() { return gamma; }
};
using ShonanAveragingParameters2 = ShonanAveragingParameters<2>;
using ShonanAveragingParameters3 = ShonanAveragingParameters<3>;
/**
* Class that implements Shonan Averaging from our ECCV'20 paper.
* Note: The "basic" API uses all Rot values (Rot2 or Rot3, depending on value
* of d), whereas the different levels and "advanced" API at SO(p) needs Values
* of type SOn<Dynamic>.
*
* The template parameter d can be 2 or 3.
* Both are specialized in the .cpp file.
*
* If you use this code in your work, please consider citing our paper:
* Shonan Rotation Averaging, Global Optimality by Surfing SO(p)^n
* Frank Dellaert, David M. Rosen, Jing Wu, Robert Mahony, and Luca Carlone,
* European Computer Vision Conference, 2020.
* You can view our ECCV spotlight video at https://youtu.be/5ppaqMyHtE0
*/
template <size_t d> class GTSAM_EXPORT ShonanAveraging {
public:
using Sparse = Eigen::SparseMatrix<double>;
// Define the Parameters type and use its typedef of the rotation type:
using Parameters = ShonanAveragingParameters<d>;
using Rot = typename Parameters::Rot;
// We store SO(d) BetweenFactors to get noise model
// TODO(frank): use BinaryMeasurement?
using Measurements = std::vector<BinaryMeasurement<Rot>>;
private:
Parameters parameters_;
Measurements measurements_;
size_t nrUnknowns_;
Sparse D_; // Sparse (diagonal) degree matrix
Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
Sparse L_; // connection Laplacian L = D - Q, needed for optimality check
/**
* Build 3Nx3N sparse matrix consisting of rotation measurements, arranged as
* (i,j) and (j,i) blocks within a sparse matrix.
*/
Sparse buildQ() const;
/// Build 3Nx3N sparse degree matrix D
Sparse buildD() const;
public:
/// @name Standard Constructors
/// @{
/// Construct from set of relative measurements (given as BetweenFactor<Rot3>
/// for now) NoiseModel *must* be isotropic.
ShonanAveraging(const Measurements &measurements,
const Parameters &parameters = Parameters());
/// @}
/// @name Query properties
/// @{
/// Return number of unknowns
size_t nrUnknowns() const { return nrUnknowns_; }
/// Return number of measurements
size_t nrMeasurements() const { return measurements_.size(); }
/// k^th binary measurement
const BinaryMeasurement<Rot> &measurement(size_t k) const {
return measurements_[k];
}
/// k^th measurement, as a Rot.
const Rot &measured(size_t k) const { return measurements_[k].measured(); }
/// Keys for k^th measurement, as a vector of Key values.
const KeyVector &keys(size_t k) const { return measurements_[k].keys(); }
/// @}
/// @name Matrix API (advanced use, debugging)
/// @{
Sparse D() const { return D_; } ///< Sparse version of D
Matrix denseD() const { return Matrix(D_); } ///< Dense version of D
Sparse Q() const { return Q_; } ///< Sparse version of Q
Matrix denseQ() const { return Matrix(Q_); } ///< Dense version of Q
Sparse L() const { return L_; } ///< Sparse version of L
Matrix denseL() const { return Matrix(L_); } ///< Dense version of L
/// Version that takes pxdN Stiefel manifold elements
Sparse computeLambda(const Matrix &S) const;
/// Dense versions of computeLambda for wrapper/testing
Matrix computeLambda_(const Values &values) const {
return Matrix(computeLambda(values));
}
/// Dense versions of computeLambda for wrapper/testing
Matrix computeLambda_(const Matrix &S) const {
return Matrix(computeLambda(S));
}
/// Compute A matrix whose Eigenvalues we will examine
Sparse computeA(const Values &values) const;
/// Version that takes pxdN Stiefel manifold elements
Sparse computeA(const Matrix &S) const;
/// Dense version of computeA for wrapper/testing
Matrix computeA_(const Values &values) const {
return Matrix(computeA(values));
}
/// Project to pxdN Stiefel manifold
static Matrix StiefelElementMatrix(const Values &values);
/**
* Compute minimum eigenvalue for optimality check.
* @param values: should be of type SOn
*/
double computeMinEigenValue(const Values &values,
Vector *minEigenVector = nullptr) const;
/// Project pxdN Stiefel manifold matrix S to Rot3^N
Values roundSolutionS(const Matrix &S) const;
/// Create a tangent direction xi with eigenvector segment v_i
static Vector MakeATangentVector(size_t p, const Vector &v, size_t i);
/// Calculate the riemannian gradient of F(values) at values
Matrix riemannianGradient(size_t p, const Values &values) const;
/**
* Lift up the dimension of values in type SO(p-1) with descent direction
* provided by minEigenVector and return new values in type SO(p)
*/
static Values LiftwithDescent(size_t p, const Values &values,
const Vector &minEigenVector);
/**
* Given some values at p-1, return new values at p, by doing a line search
* along the descent direction, computed from the minimum eigenvector at p-1.
* @param values should be of type SO(p-1)
* @param minEigenVector corresponding to minEigenValue at level p-1
* @return values of type SO(p)
*/
Values
initializeWithDescent(size_t p, const Values &values,
const Vector &minEigenVector, double minEigenValue,
double gradienTolerance = 1e-2,
double preconditionedGradNormTolerance = 1e-4) const;
/// @}
/// @name Advanced API
/// @{
/**
* Build graph for SO(p)
* @param p the dimensionality of the rotation manifold to optimize over
*/
NonlinearFactorGraph buildGraphAt(size_t p) const;
/**
* Create initial Values of type SO(p)
* @param p the dimensionality of the rotation manifold
*/
Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
/// Version of initializeRandomlyAt with fixed random seed.
Values initializeRandomlyAt(size_t p) const;
/**
* Calculate cost for SO(p)
* Values should be of type SO(p)
*/
double costAt(size_t p, const Values &values) const;
/**
* Given an estimated local minimum Yopt for the (possibly lifted)
* relaxation, this function computes and returns the block-diagonal elements
* of the corresponding Lagrange multiplier.
*/
Sparse computeLambda(const Values &values) const;
/**
* Compute minimum eigenvalue for optimality check.
* @param values: should be of type SOn
* @return minEigenVector and minEigenValue
*/
std::pair<double, Vector> computeMinEigenVector(const Values &values) const;
/**
* Check optimality
* @param values: should be of type SOn
*/
bool checkOptimality(const Values &values) const;
/**
* Try to create optimizer at SO(p)
* @param p the dimensionality of the rotation manifold to optimize over
* @param initial initial SO(p) values
* @return lm optimizer
*/
boost::shared_ptr<LevenbergMarquardtOptimizer> createOptimizerAt(
size_t p, const Values &initial) const;
/**
* Try to optimize at SO(p)
* @param p the dimensionality of the rotation manifold to optimize over
* @param initial initial SO(p) values
* @return SO(p) values
*/
Values tryOptimizingAt(size_t p, const Values &initial) const;
/**
* Project from SO(p) to Rot2 or Rot3
* Values should be of type SO(p)
*/
Values projectFrom(size_t p, const Values &values) const;
/**
* Project from SO(p)^N to Rot2^N or Rot3^N
* Values should be of type SO(p)
*/
Values roundSolution(const Values &values) const;
/// Lift Values of type T to SO(p)
template <class T> static Values LiftTo(size_t p, const Values &values) {
Values result;
for (const auto it : values.filter<T>()) {
result.insert(it.key, SOn::Lift(p, it.value.matrix()));
}
return result;
}
/// @}
/// @name Basic API
/// @{
/**
* Calculate cost for SO(3)
* Values should be of type Rot3
*/
double cost(const Values &values) const;
/**
* Initialize randomly at SO(d)
* @param rng random number generator
* Example:
* std::mt19937 rng(42);
* Values initial = initializeRandomly(rng, p);
*/
Values initializeRandomly(std::mt19937 &rng) const;
/// Random initialization for wrapper, fixed random seed.
Values initializeRandomly() const;
/**
* Optimize at different values of p until convergence.
* @param initial initial Rot3 values
* @param pMin value of p to start Riemanian staircase at (default: d).
* @param pMax maximum value of p to try (default: 10)
* @return (Rot3 values, minimum eigenvalue)
*/
std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
size_t pMax = 10) const;
/// @}
};
// Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a
// convenience interface with file access.
class ShonanAveraging2 : public ShonanAveraging<2> {
public:
ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters = Parameters());
ShonanAveraging2(string g2oFile, const Parameters &parameters = Parameters());
};
class ShonanAveraging3 : public ShonanAveraging<3> {
public:
ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters = Parameters());
ShonanAveraging3(string g2oFile, const Parameters &parameters = Parameters());
// TODO(frank): Deprecate after we land pybind wrapper
ShonanAveraging3(const BetweenFactorPose3s &factors,
const Parameters &parameters = Parameters());
};
} // namespace gtsam

141
gtsam/sfm/ShonanFactor.cpp Normal file
View File

@ -0,0 +1,141 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 FrobeniusFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Various factors that minimize some Frobenius norm
*/
#include <gtsam/sfm/ShonanFactor.h>
#include <gtsam/base/timing.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <cmath>
#include <iostream>
#include <vector>
using namespace std;
namespace gtsam {
//******************************************************************************
template <size_t d>
ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
const SharedNoiseModel &model,
const boost::shared_ptr<Matrix> &G)
: NoiseModelFactor2<SOn, SOn>(ConvertNoiseModel(model, p * d), j1, j2),
M_(R12.matrix()), // d*d in all cases
p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4)
G_(G) {
if (noiseModel()->dim() != d * p_)
throw std::invalid_argument(
"ShonanFactor: model with incorrect dimension.");
if (!G) {
G_ = boost::make_shared<Matrix>();
*G_ = SOn::VectorizedGenerators(p); // expensive!
}
if (static_cast<size_t>(G_->rows()) != pp_ ||
static_cast<size_t>(G_->cols()) != SOn::Dimension(p))
throw std::invalid_argument("ShonanFactor: passed in generators "
"of incorrect dimension.");
}
//******************************************************************************
template <size_t d>
void ShonanFactor<d>::print(const std::string &s,
const KeyFormatter &keyFormatter) const {
std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
<< keyFormatter(key2()) << ")\n";
traits<Matrix>::Print(M_, " M: ");
noiseModel_->print(" noise model: ");
}
//******************************************************************************
template <size_t d>
bool ShonanFactor<d>::equals(const NonlinearFactor &expected,
double tol) const {
auto e = dynamic_cast<const ShonanFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
p_ == e->p_ && M_ == e->M_;
}
//******************************************************************************
template <size_t d>
void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const {
gttic(ShonanFactor_Jacobians);
const size_t dim = p_ * d; // Stiefel manifold dimension
if (H1) {
// If asked, calculate Jacobian H1 as as (M' \otimes M1) * G
// M' = dxd, M1 = pxp, G = (p*p)xDim(p), result should be dim x Dim(p)
// (M' \otimes M1) is dim*dim, but last pp-dim columns are zero
*H1 = Matrix::Zero(dim, G_->cols());
for (size_t j = 0; j < d; j++) {
auto MG_j = M1 * G_->middleRows(j * p_, p_); // p_ * Dim(p)
for (size_t i = 0; i < d; i++) {
H1->middleRows(i * p_, p_) -= M_(j, i) * MG_j;
}
}
}
if (H2) {
// If asked, calculate Jacobian H2 as as (I_d \otimes M2) * G
// I_d = dxd, M2 = pxp, G = (p*p)xDim(p), result should be dim x Dim(p)
// (I_d \otimes M2) is dim*dim, but again last pp-dim columns are zero
H2->resize(dim, G_->cols());
for (size_t i = 0; i < d; i++) {
H2->middleRows(i * p_, p_) = M2 * G_->middleRows(i * p_, p_);
}
}
}
//******************************************************************************
template <size_t d>
Vector ShonanFactor<d>::evaluateError(const SOn &Q1, const SOn &Q2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const {
gttic(ShonanFactor_evaluateError);
const Matrix &M1 = Q1.matrix();
const Matrix &M2 = Q2.matrix();
if (M1.rows() != static_cast<int>(p_) || M2.rows() != static_cast<int>(p_))
throw std::invalid_argument("Invalid dimension SOn values passed to "
"ShonanFactor<d>::evaluateError");
const size_t dim = p_ * d; // Stiefel manifold dimension
Vector fQ2(dim), hQ1(dim);
// Vectorize and extract only d leftmost columns, i.e. vec(M2*P)
fQ2 << Eigen::Map<const Matrix>(M2.data(), dim, 1);
// Vectorize M1*P*R12
const Matrix Q1PR12 = M1.leftCols<d>() * M_;
hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
this->fillJacobians(M1, M2, H1, H2);
return fQ2 - hQ1;
}
/* ************************************************************************* */
// Explicit instantiation for d=2 and d=3
template class ShonanFactor<2>;
template class ShonanFactor<3>;
//******************************************************************************
} // namespace gtsam

91
gtsam/sfm/ShonanFactor.h Normal file
View File

@ -0,0 +1,91 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 ShonanFactor.h
* @date March 2019
* @author Frank Dellaert
* @brief Main factor type in Shonan averaging, on SO(n) pairs
*/
#pragma once
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <type_traits>
namespace gtsam {
/**
* ShonanFactor is a BetweenFactor that moves in SO(p), but will
* land on the SO(d) sub-manifold of SO(p) at the global minimum. It projects
* the SO(p) matrices down to a Stiefel manifold of p*d matrices.
*/
template <size_t d>
class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_; ///< dimensionality constants
boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
// Select Rot2 or Rot3 interface based template parameter d
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
public:
/// @name Constructor
/// @{
/// Constructor. Note we convert to d*p-dimensional noise model.
/// To save memory and mallocs, pass in the vectorized Lie algebra generators:
/// G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(p));
ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
const SharedNoiseModel &model = nullptr,
const boost::shared_ptr<Matrix> &G = nullptr);
/// @}
/// @name Testable
/// @{
/// print with optional string
void
print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
/// assert equality up to a tolerance
bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override;
/// @}
/// @name NoiseModelFactor2 methods
/// @{
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector
evaluateError(const SOn &Q1, const SOn &Q2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const override;
/// @}
private:
/// Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp
void fillJacobians(const Matrix &M1, const Matrix &M2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const;
};
// Explicit instantiation for d=2 and d=3 in .cpp file:
using ShonanFactor2 = ShonanFactor<2>;
using ShonanFactor3 = ShonanFactor<3>;
} // namespace gtsam

108
gtsam/sfm/ShonanGaugeFactor.h Normal file
View File

@ -0,0 +1,108 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 ShonanGaugeFactor.h
* @date March 2019
* @author Frank Dellaert
* @brief Factor used in Shonan Averaging to clamp down gauge freedom
*/
#pragma once
#include <gtsam/geometry/SOn.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/assign/list_of.hpp>
namespace gtsam {
/**
* The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving
* in the stabilizer.
*
* Details: SO(p) contains the p*3 Stiefel matrices of orthogonal frames: we
* take those to be the 3 columns on the left.
* The P*P skew-symmetric matrices associated with so(p) can be partitioned as
* (Appendix B in the ECCV paper):
* | [w] -K' |
* | K [g] |
* where w is the SO(3) space, K are the extra Stiefel diemnsions (wormhole!)
* and (g)amma are extra dimensions in SO(p) that do not modify the cost
* function. The latter corresponds to rotations SO(p-d), and so the first few
* values of p-d for d==3 with their corresponding dimensionality are {0:0, 1:0,
* 2:1, 3:3, ...}
*
* The ShonanGaugeFactor adds a unit Jacobian to these extra dimensions,
* essentially restricting the optimization to the Stiefel manifold.
*/
class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor {
// Row dimension, equal to the dimensionality of SO(p-d)
size_t rows_;
/// Constant Jacobian
boost::shared_ptr<JacobianFactor> whitenedJacobian_;
public:
/**
* Construct from key for an SO(p) matrix, for base dimension d (2 or 3)
* If parameter gamma is given, it acts as a precision = 1/sigma^2, and
* the Jacobian will be multiplied with 1/sigma = sqrt(gamma).
*/
ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
boost::optional<double> gamma = boost::none)
: NonlinearFactor(boost::assign::cref_list_of<1>(key)) {
if (p < d) {
throw std::invalid_argument("ShonanGaugeFactor must have p>=d.");
}
// Calculate dimensions
size_t q = p - d;
size_t P = SOn::Dimension(p); // dimensionality of SO(p)
rows_ = SOn::Dimension(q); // dimensionality of SO(q), the gauge
// Create constant Jacobian as a rows_*P matrix: there are rows_ penalized
// dimensions, but it is a bit tricky to find them among the P columns.
// The key is to look at how skew-symmetric matrices are laid out in SOn.h:
// the first tangent dimension will always be included, but beyond that we
// have to be careful. We always need to skip the d top-rows of the skew-
// symmetric matrix as they below to K, part of the Stiefel manifold.
Matrix A(rows_, P);
A.setZero();
double invSigma = gamma ? std::sqrt(*gamma) : 1.0;
size_t i = 0, j = 0, n = p - 1 - d;
while (i < rows_) {
A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n);
i += n;
j += n + d; // skip d columns
n -= 1;
}
// TODO(frank): assign the right one in the right columns
whitenedJacobian_ =
boost::make_shared<JacobianFactor>(key, A, Vector::Zero(rows_));
}
/// Destructor
virtual ~ShonanGaugeFactor() {}
/// Calculate the error of the factor: always zero
double error(const Values &c) const override { return 0; }
/// get the dimension of the factor (number of rows on linearization)
size_t dim() const override { return rows_; }
/// linearize to a GaussianFactor
boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
return whitenedJacobian_;
}
};
// \ShonanGaugeFactor
} // namespace gtsam

360
gtsam/sfm/tests/testShonanAveraging.cpp Normal file
View File

@ -0,0 +1,360 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 testShonanAveraging.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Unit tests for Shonan Averaging algorithm
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <algorithm>
#include <iostream>
#include <map>
#include <random>
using namespace std;
using namespace gtsam;
template <size_t d>
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
template <size_t d>
using Pose = typename std::conditional<d == 2, Pose2, Pose3>::type;
ShonanAveraging3 fromExampleName(
const std::string &name,
ShonanAveraging3::Parameters parameters = ShonanAveraging3::Parameters()) {
string g2oFile = findExampleDataFile(name);
return ShonanAveraging3(g2oFile, parameters);
}
static const ShonanAveraging3 kShonan = fromExampleName("toyExample.g2o");
static std::mt19937 kRandomNumberGenerator(42);
/* ************************************************************************* */
TEST(ShonanAveraging3, checkConstructor) {
EXPECT_LONGS_EQUAL(5, kShonan.nrUnknowns());
EXPECT_LONGS_EQUAL(15, kShonan.D().rows());
EXPECT_LONGS_EQUAL(15, kShonan.D().cols());
auto D = kShonan.denseD();
EXPECT_LONGS_EQUAL(15, D.rows());
EXPECT_LONGS_EQUAL(15, D.cols());
EXPECT_LONGS_EQUAL(15, kShonan.Q().rows());
EXPECT_LONGS_EQUAL(15, kShonan.Q().cols());
auto Q = kShonan.denseQ();
EXPECT_LONGS_EQUAL(15, Q.rows());
EXPECT_LONGS_EQUAL(15, Q.cols());
EXPECT_LONGS_EQUAL(15, kShonan.L().rows());
EXPECT_LONGS_EQUAL(15, kShonan.L().cols());
auto L = kShonan.denseL();
EXPECT_LONGS_EQUAL(15, L.rows());
EXPECT_LONGS_EQUAL(15, L.cols());
}
/* ************************************************************************* */
TEST(ShonanAveraging3, buildGraphAt) {
auto graph = kShonan.buildGraphAt(5);
EXPECT_LONGS_EQUAL(7, graph.size());
}
/* ************************************************************************* */
TEST(ShonanAveraging3, checkOptimality) {
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations); // lift to 4!
auto Lambda = kShonan.computeLambda(random);
EXPECT_LONGS_EQUAL(15, Lambda.rows());
EXPECT_LONGS_EQUAL(15, Lambda.cols());
EXPECT_LONGS_EQUAL(45, Lambda.nonZeros());
auto lambdaMin = kShonan.computeMinEigenValue(random);
// EXPECT_DOUBLES_EQUAL(-5.2964625490657866, lambdaMin,
// 1e-4); // Regression test
EXPECT_DOUBLES_EQUAL(-414.87376657555996, lambdaMin,
1e-4); // Regression test
EXPECT(!kShonan.checkOptimality(random));
}
/* ************************************************************************* */
TEST(ShonanAveraging3, tryOptimizingAt3) {
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
Values initial = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations); // convert to SOn
EXPECT(!kShonan.checkOptimality(initial));
const Values result = kShonan.tryOptimizingAt(3, initial);
EXPECT(kShonan.checkOptimality(result));
auto lambdaMin = kShonan.computeMinEigenValue(result);
EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
1e-4); // Regression test
EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(3, result), 1e-4);
const Values SO3Values = kShonan.roundSolution(result);
EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
}
/* ************************************************************************* */
TEST(ShonanAveraging3, tryOptimizingAt4) {
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations); // lift to 4!
const Values result = kShonan.tryOptimizingAt(4, random);
EXPECT(kShonan.checkOptimality(result));
EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(4, result), 1e-3);
auto lambdaMin = kShonan.computeMinEigenValue(result);
EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
1e-4); // Regression test
const Values SO3Values = kShonan.roundSolution(result);
EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
}
/* ************************************************************************* */
TEST(ShonanAveraging3, MakeATangentVector) {
Vector9 v;
v << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Matrix expected(5, 5);
expected << 0, 0, 0, 0, -4, //
0, 0, 0, 0, -5, //
0, 0, 0, 0, -6, //
0, 0, 0, 0, 0, //
4, 5, 6, 0, 0;
const Vector xi_1 = ShonanAveraging3::MakeATangentVector(5, v, 1);
const auto actual = SOn::Hat(xi_1);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(ShonanAveraging3, LiftTo) {
auto I = genericValue(Rot3());
Values initial{{0, I}, {1, I}, {2, I}};
Values lifted = ShonanAveraging3::LiftTo<Rot3>(5, initial);
EXPECT(assert_equal(SOn(5), lifted.at<SOn>(0)));
}
/* ************************************************************************* */
TEST(ShonanAveraging3, CheckWithEigen) {
// control randomness
static std::mt19937 rng(0);
Vector descentDirection = Vector::Random(15); // for use below
const Values randomRotations = kShonan.initializeRandomly(rng);
Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
// Optimize
const Values Qstar3 = kShonan.tryOptimizingAt(3, random);
// Compute Eigenvalue with Spectra solver
double lambda = kShonan.computeMinEigenValue(Qstar3);
// Check Eigenvalue with slow Eigen version, converts matrix A to dense matrix!
const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3);
auto A = kShonan.computeA(S);
bool computeEigenvectors = false;
Eigen::EigenSolver<Matrix> eigenSolver(Matrix(A), computeEigenvectors);
auto lambdas = eigenSolver.eigenvalues().real();
double minEigenValue = lambdas(0);
for (int i = 1; i < lambdas.size(); i++)
minEigenValue = min(lambdas(i), minEigenValue);
// Actual check
EXPECT_DOUBLES_EQUAL(minEigenValue, lambda, 1e-12);
// Construct test descent direction (as minEigenVector is not predictable
// across platforms, being one from a basically flat 3d- subspace)
// Check descent
Values initialQ4 =
ShonanAveraging3::LiftwithDescent(4, Qstar3, descentDirection);
EXPECT_LONGS_EQUAL(5, initialQ4.size());
// TODO(frank): uncomment this regression test: currently not repeatable
// across platforms.
// Matrix expected(4, 4);
// expected << 0.0459224, -0.688689, -0.216922, 0.690321, //
// 0.92381, 0.191931, 0.255854, 0.21042, //
// -0.376669, 0.301589, 0.687953, 0.542111, //
// -0.0508588, 0.630804, -0.643587, 0.43046;
// EXPECT(assert_equal(SOn(expected), initialQ4.at<SOn>(0), 1e-5));
}
/* ************************************************************************* */
TEST(ShonanAveraging3, initializeWithDescent) {
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
const Values Qstar3 = kShonan.tryOptimizingAt(3, random);
Vector minEigenVector;
double lambdaMin = kShonan.computeMinEigenValue(Qstar3, &minEigenVector);
Values initialQ4 =
kShonan.initializeWithDescent(4, Qstar3, minEigenVector, lambdaMin);
EXPECT_LONGS_EQUAL(5, initialQ4.size());
}
/* ************************************************************************* */
TEST(ShonanAveraging3, run) {
auto initial = kShonan.initializeRandomly(kRandomNumberGenerator);
auto result = kShonan.run(initial, 5);
EXPECT_DOUBLES_EQUAL(0, kShonan.cost(result.first), 1e-3);
EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, result.second,
1e-4); // Regression test
}
/* ************************************************************************* */
namespace klaus {
// The data in the file is the Colmap solution
const Rot3 wR0(0.9992281076190063, -0.02676080288219576, -0.024497002638379624,
-0.015064701622500615);
const Rot3 wR1(0.998239108728862, -0.049543805396343954, -0.03232420352077356,
-0.004386230477751116);
const Rot3 wR2(0.9925378735259738, -0.07993768981394891, 0.0825062894866454,
-0.04088089479075661);
} // namespace klaus
TEST(ShonanAveraging3, runKlaus) {
using namespace klaus;
// Initialize a Shonan instance without the Karcher mean
ShonanAveraging3::Parameters parameters;
parameters.setKarcherWeight(0);
// Load 3 pose example taken in Klaus by Shicong
static const ShonanAveraging3 shonan =
fromExampleName("Klaus3.g2o", parameters);
// Check nr poses
EXPECT_LONGS_EQUAL(3, shonan.nrUnknowns());
// Colmap uses the Y-down vision frame, and the first 3 rotations are close to
// identity. We check that below. Note tolerance is quite high.
static const Rot3 identity;
EXPECT(assert_equal(identity, wR0, 0.2));
EXPECT(assert_equal(identity, wR1, 0.2));
EXPECT(assert_equal(identity, wR2, 0.2));
// Get measurements
const Rot3 R01 = shonan.measured(0);
const Rot3 R12 = shonan.measured(1);
const Rot3 R02 = shonan.measured(2);
// Regression test to make sure data did not change.
EXPECT(assert_equal(Rot3(0.9995433591728293, -0.022048798853273946,
-0.01796327847857683, 0.010210006313668573),
R01));
// Check Colmap solution agrees OK with relative rotation measurements.
EXPECT(assert_equal(R01, wR0.between(wR1), 0.1));
EXPECT(assert_equal(R12, wR1.between(wR2), 0.1));
EXPECT(assert_equal(R02, wR0.between(wR2), 0.1));
// Run Shonan (with prior on first rotation)
auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
auto result = shonan.run(initial, 5);
EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
EXPECT_DOUBLES_EQUAL(-9.2259161494467889e-05, result.second,
1e-4); // Regression
// Get Shonan solution in new frame R (R for result)
const Rot3 rR0 = result.first.at<Rot3>(0);
const Rot3 rR1 = result.first.at<Rot3>(1);
const Rot3 rR2 = result.first.at<Rot3>(2);
// rR0 = rRw * wR0 => rRw = rR0 * wR0.inverse()
// rR1 = rRw * wR1
// rR2 = rRw * wR2
const Rot3 rRw = rR0 * wR0.inverse();
EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
}
/* ************************************************************************* */
TEST(ShonanAveraging3, runKlausKarcher) {
using namespace klaus;
// Load 3 pose example taken in Klaus by Shicong
static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o");
// Run Shonan (with Karcher mean prior)
auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
auto result = shonan.run(initial, 5);
EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
EXPECT_DOUBLES_EQUAL(-1.361402670507772e-05, result.second,
1e-4); // Regression test
// Get Shonan solution in new frame R (R for result)
const Rot3 rR0 = result.first.at<Rot3>(0);
const Rot3 rR1 = result.first.at<Rot3>(1);
const Rot3 rR2 = result.first.at<Rot3>(2);
const Rot3 rRw = rR0 * wR0.inverse();
EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
}
/* ************************************************************************* */
TEST(ShonanAveraging2, runKlausKarcher) {
// Load 2D toy example
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
// lmParams.setVerbosityLM("SUMMARY");
string g2oFile = findExampleDataFile("noisyToyGraph.txt");
ShonanAveraging2::Parameters parameters(lmParams);
auto measurements = parseMeasurements<Rot2>(g2oFile);
ShonanAveraging2 shonan(measurements, parameters);
EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
// Check graph building
NonlinearFactorGraph graph = shonan.buildGraphAt(2);
EXPECT_LONGS_EQUAL(6, graph.size());
auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
auto result = shonan.run(initial, 2);
EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
}
/* ************************************************************************* */
// Test alpha/beta/gamma prior weighting.
TEST(ShonanAveraging3, PriorWeights) {
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
ShonanAveraging3::Parameters params(lmParams);
EXPECT_DOUBLES_EQUAL(0, params.alpha, 1e-9);
EXPECT_DOUBLES_EQUAL(1, params.beta, 1e-9);
EXPECT_DOUBLES_EQUAL(0, params.gamma, 1e-9);
double alpha = 100.0, beta = 200.0, gamma = 300.0;
params.setAnchorWeight(alpha);
params.setKarcherWeight(beta);
params.setGaugesWeight(gamma);
EXPECT_DOUBLES_EQUAL(alpha, params.alpha, 1e-9);
EXPECT_DOUBLES_EQUAL(beta, params.beta, 1e-9);
EXPECT_DOUBLES_EQUAL(gamma, params.gamma, 1e-9);
params.setKarcherWeight(0);
static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o", params);
for (auto i : {0,1,2}) {
const auto& m = shonan.measurement(i);
auto isotropic =
boost::static_pointer_cast<noiseModel::Isotropic>(m.noiseModel());
CHECK(isotropic != nullptr);
EXPECT_LONGS_EQUAL(3, isotropic->dim());
EXPECT_DOUBLES_EQUAL(0.2, isotropic->sigma(), 1e-9);
}
auto I = genericValue(Rot3());
Values initial{{0, I}, {1, I}, {2, I}};
EXPECT_DOUBLES_EQUAL(3.0756, shonan.cost(initial), 1e-4);
auto result = shonan.run(initial, 3, 3);
EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

121
gtsam/sfm/tests/testShonanFactor.cpp Normal file
View File

@ -0,0 +1,121 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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
* -------------------------------------------------------------------------- */
/**
* testFrobeniusFactor.cpp
*
* @file testFrobeniusFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Check evaluateError for various Frobenius norm
*/
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/sfm/ShonanFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
namespace so3 {
SO3 id;
Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1);
Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace so3
//******************************************************************************
namespace submanifold {
SO4 id;
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1.tail<3>());
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2.tail<3>());
SO4 Q2 = SO4::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace submanifold
/* ************************************************************************* */
TEST(ShonanFactor3, evaluateError) {
auto model = noiseModel::Isotropic::Sigma(3, 1.2);
for (const size_t p : {5, 4, 3}) {
Matrix M = Matrix::Identity(p, p);
M.topLeftCorner(3, 3) = submanifold::R1.matrix();
SOn Q1(M);
M.topLeftCorner(3, 3) = submanifold::R2.matrix();
SOn Q2(M);
auto factor = ShonanFactor3(1, 2, Rot3(::so3::R12.matrix()), p, model);
Matrix H1, H2;
factor.evaluateError(Q1, Q2, H1, H2);
// Test derivatives
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
}
/* ************************************************************************* */
TEST(ShonanFactor3, equivalenceToSO3) {
using namespace ::submanifold;
auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1));
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension
auto factor3 = FrobeniusBetweenFactor<SO3>(1, 2, R12, model);
auto factor4 = ShonanFactor3(1, 2, Rot3(R12.matrix()), 4, model);
const Matrix3 E3(factor3.evaluateError(R1, R2).data());
const Matrix43 E4(
factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data());
EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9));
EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9));
}
/* ************************************************************************* */
TEST(ShonanFactor2, evaluateError) {
auto model = noiseModel::Isotropic::Sigma(1, 1.2);
const Rot2 R1(0.3), R2(0.5), R12(0.2);
for (const size_t p : {5, 4, 3, 2}) {
Matrix M = Matrix::Identity(p, p);
M.topLeftCorner(2, 2) = R1.matrix();
SOn Q1(M);
M.topLeftCorner(2, 2) = R2.matrix();
SOn Q2(M);
auto factor = ShonanFactor2(1, 2, R12, p, model);
Matrix H1, H2;
factor.evaluateError(Q1, Q2, H1, H2);
// Test derivatives
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

106
gtsam/sfm/tests/testShonanGaugeFactor.cpp Normal file
View File

@ -0,0 +1,106 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 testShonanGaugeFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Unit tests for ShonanGaugeFactor class
*/
#include <gtsam/sfm/ShonanGaugeFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <map>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
// Check dimensions of all low-dim GaugeFactors
TEST(ShonanAveraging, GaugeFactorLows) {
constexpr Key key(123);
EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 2, 2).dim());
EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 3, 2).dim());
EXPECT_LONGS_EQUAL(1, ShonanGaugeFactor(key, 4, 2).dim()); // SO(4-2) -> 1
EXPECT_LONGS_EQUAL(3, ShonanGaugeFactor(key, 5, 2).dim()); // SO(5-2) -> 3
EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 3, 3).dim());
EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 4, 3).dim());
EXPECT_LONGS_EQUAL(1, ShonanGaugeFactor(key, 5, 3).dim()); // SO(5-3) -> 1
}
/* ************************************************************************* */
// Check ShonanGaugeFactor for SO(6)
TEST(ShonanAveraging, GaugeFactorSO6) {
constexpr Key key(666);
ShonanGaugeFactor factor(key, 6); // For SO(6)
Matrix A = Matrix::Zero(3, 15); // SO(6-3) = SO(3) == 3-dimensional gauge
A(0, 0) = 1; // first 2 of 6^th skew column, which has 5 non-zero entries
A(1, 1) = 1; // then we skip 3 tangent dimensions
A(2, 5) = 1; // first of 5th skew colum, which has 4 non-zero entries above
// diagonal.
JacobianFactor linearized(key, A, Vector::Zero(3));
Values values;
EXPECT_LONGS_EQUAL(3, factor.dim());
EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast<JacobianFactor>(
factor.linearize(values))));
}
/* ************************************************************************* */
// Check ShonanGaugeFactor for SO(7)
TEST(ShonanAveraging, GaugeFactorSO7) {
constexpr Key key(777);
ShonanGaugeFactor factor(key, 7); // For SO(7)
Matrix A = Matrix::Zero(6, 21); // SO(7-3) = SO(4) == 6-dimensional gauge
A(0, 0) = 1; // first 3 of 7^th skew column, which has 6 non-zero entries
A(1, 1) = 1;
A(2, 2) = 1; // then we skip 3 tangent dimensions
A(3, 6) = 1; // first 2 of 6^th skew column, which has 5 non-zero entries
A(4, 7) = 1; // then we skip 3 tangent dimensions
A(5, 11) = 1; // first of 5th skew colum, which has 4 non-zero entries above
// diagonal.
JacobianFactor linearized(key, A, Vector::Zero(6));
Values values;
EXPECT_LONGS_EQUAL(6, factor.dim());
EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast<JacobianFactor>(
factor.linearize(values))));
}
/* ************************************************************************* */
// Check ShonanGaugeFactor for SO(6), with base SO(2)
TEST(ShonanAveraging, GaugeFactorSO6over2) {
constexpr Key key(602);
double gamma = 4;
ShonanGaugeFactor factor(key, 6, 2, gamma); // For SO(6), base SO(2)
Matrix A = Matrix::Zero(6, 15); // SO(6-2) = SO(4) == 6-dimensional gauge
A(0, 0) = 2; // first 3 of 6^th skew column, which has 5 non-zero entries
A(1, 1) = 2;
A(2, 2) = 2; // then we skip only 2 tangent dimensions
A(3, 5) = 2; // first 2 of 5^th skew column, which has 4 non-zero entries
A(4, 6) = 2; // then we skip only 2 tangent dimensions
A(5, 9) = 2; // first of 4th skew colum, which has 3 non-zero entries above
// diagonal.
JacobianFactor linearized(key, A, Vector::Zero(6));
Values values;
EXPECT_LONGS_EQUAL(6, factor.dim());
EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast<JacobianFactor>(
factor.linearize(values))));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

118
gtsam/slam/FrobeniusFactor.cpp
View File

@ -18,118 +18,38 @@
#include <gtsam/slam/FrobeniusFactor.h>
#include <gtsam/base/timing.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <cmath>
#include <iostream>
#include <vector>
using namespace std;
namespace gtsam {
//******************************************************************************
boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit) {
boost::shared_ptr<noiseModel::Isotropic>
ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
double sigma = 1.0;
if (model != nullptr) {
if (model->dim() != 6) {
if (!defaultToUnit)
throw std::runtime_error("Can only convert Pose3 noise models");
} else {
auto sigmas = model->sigmas().head(3).eval();
if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) {
if (!defaultToUnit)
auto sigmas = model->sigmas();
size_t n = sigmas.size();
if (n == 1) {
sigma = sigmas(0); // Rot2
goto exit;
}
if (n == 3 || n == 6) {
sigma = sigmas(2); // Pose2, Rot3, or Pose3
if (sigmas(0) != sigma || sigmas(1) != sigma) {
if (!defaultToUnit) {
throw std::runtime_error("Can only convert isotropic rotation noise");
} else {
sigma = sigmas(0);
}
}
goto exit;
}
if (!defaultToUnit) {
throw std::runtime_error("Can only convert Pose2/Pose3 noise models");
}
}
exit:
return noiseModel::Isotropic::Sigma(d, sigma);
}
//******************************************************************************
FrobeniusWormholeFactor::FrobeniusWormholeFactor(
Key j1, Key j2, const Rot3 &R12, size_t p, const SharedNoiseModel &model,
const boost::shared_ptr<Matrix> &G)
: NoiseModelFactor2<SOn, SOn>(ConvertPose3NoiseModel(model, p * 3), j1, j2),
M_(R12.matrix()), // 3*3 in all cases
p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4)
G_(G) {
if (noiseModel()->dim() != 3 * p_)
throw std::invalid_argument(
"FrobeniusWormholeFactor: model with incorrect dimension.");
if (!G) {
G_ = boost::make_shared<Matrix>();
*G_ = SOn::VectorizedGenerators(p); // expensive!
}
if (static_cast<size_t>(G_->rows()) != pp_ ||
static_cast<size_t>(G_->cols()) != SOn::Dimension(p))
throw std::invalid_argument("FrobeniusWormholeFactor: passed in generators "
"of incorrect dimension.");
}
//******************************************************************************
void FrobeniusWormholeFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
<< keyFormatter(key2()) << ")\n";
traits<Matrix>::Print(M_, " M: ");
noiseModel_->print(" noise model: ");
}
//******************************************************************************
bool FrobeniusWormholeFactor::equals(const NonlinearFactor &expected,
double tol) const {
auto e = dynamic_cast<const FrobeniusWormholeFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
p_ == e->p_ && M_ == e->M_;
}
//******************************************************************************
Vector FrobeniusWormholeFactor::evaluateError(
const SOn& Q1, const SOn& Q2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
gttic(FrobeniusWormholeFactorP_evaluateError);
const Matrix& M1 = Q1.matrix();
const Matrix& M2 = Q2.matrix();
assert(M1.rows() == p_ && M2.rows() == p_);
const size_t dim = 3 * p_; // Stiefel manifold dimension
Vector fQ2(dim), hQ1(dim);
// Vectorize and extract only d leftmost columns, i.e. vec(M2*P)
fQ2 << Eigen::Map<const Matrix>(M2.data(), dim, 1);
// Vectorize M1*P*R12
const Matrix Q1PR12 = M1.leftCols<3>() * M_;
hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
// If asked, calculate Jacobian as (M \otimes M1) * G
if (H1) {
const size_t p2 = 2 * p_;
Matrix RPxQ = Matrix::Zero(dim, pp_);
RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0);
RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1);
RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2);
*H1 = -RPxQ * (*G_);
}
if (H2) {
const size_t p2 = 2 * p_;
Matrix PxQ = Matrix::Zero(dim, pp_);
PxQ.block(0, 0, p_, p_) = M2;
PxQ.block(p_, p_, p_, p_) = M2;
PxQ.block(p2, p2, p_, p_) = M2;
*H2 = PxQ * (*G_);
}
return fQ2 - hQ1;
}
//******************************************************************************
} // namespace gtsam
} // namespace gtsam

74
gtsam/slam/FrobeniusFactor.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -25,23 +26,24 @@
namespace gtsam {
/**
* When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3
* BetweenFactor noise model into an 9 or 16-dimensional isotropic noise
* When creating (any) FrobeniusFactor we can convert a Rot/Pose
* BetweenFactor noise model into a n-dimensional isotropic noise
* model used to weight the Frobenius norm. If the noise model passed is
* null we return a Dim-dimensional isotropic noise model with sigma=1.0. If
* not, we we check if the 3-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an
* null we return a n-dimensional isotropic noise model with sigma=1.0. If
* not, we we check if the d-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an
* error. If defaultToUnit == false throws an exception on unexepcted input.
*/
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit = true);
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic>
ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
bool defaultToUnit = true);
/**
* FrobeniusPrior calculates the Frobenius norm between a given matrix and an
* element of SO(3) or SO(4).
*/
template <class Rot>
class FrobeniusPrior : public NoiseModelFactor1<Rot> {
class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
using MatrixNN = typename Rot::MatrixNN;
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
@ -50,7 +52,7 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
/// Constructor
FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor1<Rot>(ConvertPose3NoiseModel(model, Dim), j) {
: NoiseModelFactor1<Rot>(ConvertNoiseModel(model, Dim), j) {
vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
}
@ -66,13 +68,13 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
* The template argument can be any fixed-size SO<N>.
*/
template <class Rot>
class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
class GTSAM_EXPORT FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
/// Constructor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(ConvertPose3NoiseModel(model, Dim), j1,
: NoiseModelFactor2<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
j2) {}
/// Error is just Frobenius norm between rotation matrices.
@ -106,7 +108,7 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(
ConvertPose3NoiseModel(model, Dim), j1, j2),
ConvertNoiseModel(model, Dim), j1, j2),
R12_(R12),
R2hat_H_R1_(R12.inverse().AdjointMap()) {}
@ -150,52 +152,4 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
/// @}
};
/**
* FrobeniusWormholeFactor is a BetweenFactor that moves in SO(p), but will
* land on the SO(3) sub-manifold of SO(p) at the global minimum. It projects
* the SO(p) matrices down to a Stiefel manifold of p*d matrices.
* TODO(frank): template on D=2 or 3
*/
class GTSAM_EXPORT FrobeniusWormholeFactor
: public NoiseModelFactor2<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_; ///< dimensionality constants
boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
public:
/// @name Constructor
/// @{
/// Constructor. Note we convert to 3*p-dimensional noise model.
/// To save memory and mallocs, pass in the vectorized Lie algebra generators:
/// G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(p));
FrobeniusWormholeFactor(Key j1, Key j2, const Rot3 &R12, size_t p = 4,
const SharedNoiseModel &model = nullptr,
const boost::shared_ptr<Matrix> &G = nullptr);
/// @}
/// @name Testable
/// @{
/// print with optional string
void
print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
/// assert equality up to a tolerance
bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override;
/// @}
/// @name NoiseModelFactor2 methods
/// @{
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector evaluateError(const SOn& Q1, const SOn& Q2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
/// @}
};
} // namespace gtsam

18
gtsam/slam/KarcherMeanFactor-inl.h
View File

@ -58,20 +58,22 @@ T FindKarcherMean(std::initializer_list<T>&& rotations) {
template <class T>
template <typename CONTAINER>
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d)
: NonlinearFactor(keys) {
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
boost::optional<double> beta)
: NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
if (d <= 0) {
throw std::invalid_argument(
"KarcherMeanFactor needs dimension for dynamic types.");
}
// Create the constant Jacobian made of D*D identity matrices,
// where D is the dimensionality of the manifold.
const auto I = Eigen::MatrixXd::Identity(d, d);
// Create the constant Jacobian made of d*d identity matrices,
// where d is the dimensionality of the manifold.
Matrix A = Matrix::Identity(d, d);
if (beta) A *= std::sqrt(*beta);
std::map<Key, Matrix> terms;
for (Key j : keys) {
terms[j] = I;
terms[j] = A;
}
jacobian_ =
boost::make_shared<JacobianFactor>(terms, Eigen::VectorXd::Zero(d));
whitenedJacobian_ =
boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
}
} // namespace gtsam

43
gtsam/slam/KarcherMeanFactor.h
View File

@ -30,44 +30,51 @@ namespace gtsam {
* PriorFactors.
*/
template <class T>
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations);
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
template <class T>
T FindKarcherMean(std::initializer_list<T>&& rotations);
template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
/**
* The KarcherMeanFactor creates a constraint on all SO(n) variables with
* given keys that the Karcher mean (see above) will stay the same. Note the
* mean itself is irrelevant to the constraint and is not a parameter: the
* constraint is implemented as enforcing that the sum of local updates is
* equal to zero, hence creating a rank-dim constraint. Note it is implemented as
* a soft constraint, as typically it is used to fix a gauge freedom.
* equal to zero, hence creating a rank-dim constraint. Note it is implemented
* as a soft constraint, as typically it is used to fix a gauge freedom.
* */
template <class T>
class KarcherMeanFactor : public NonlinearFactor {
template <class T> class KarcherMeanFactor : public NonlinearFactor {
// Compile time dimension: can be -1
enum { D = traits<T>::dimension };
// Runtime dimension: always >=0
size_t d_;
/// Constant Jacobian made of d*d identity matrices
boost::shared_ptr<JacobianFactor> jacobian_;
boost::shared_ptr<JacobianFactor> whitenedJacobian_;
enum {D = traits<T>::dimension};
public:
/// Construct from given keys.
public:
/**
* Construct from given keys.
* If parameter beta is given, it acts as a precision = 1/sigma^2, and
* the Jacobian will be multiplied with 1/sigma = sqrt(beta).
*/
template <typename CONTAINER>
KarcherMeanFactor(const CONTAINER& keys, int d=D);
KarcherMeanFactor(const CONTAINER &keys, int d = D,
boost::optional<double> beta = boost::none);
/// Destructor
virtual ~KarcherMeanFactor() {}
/// Calculate the error of the factor: always zero
double error(const Values& c) const override { return 0; }
double error(const Values &c) const override { return 0; }
/// get the dimension of the factor (number of rows on linearization)
size_t dim() const override { return D; }
size_t dim() const override { return d_; }
/// linearize to a GaussianFactor
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
return jacobian_;
boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
return whitenedJacobian_;
}
};
// \KarcherMeanFactor
} // namespace gtsam
} // namespace gtsam

2
gtsam/slam/dataset.cpp
View File

@ -570,7 +570,7 @@ GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, size_t maxIndex,
/* ************************************************************************* */
GraphAndValues load2D_robust(const string &filename,
noiseModel::Base::shared_ptr &model,
const noiseModel::Base::shared_ptr &model,
size_t maxIndex) {
return load2D(filename, model, maxIndex);
}

2
gtsam/slam/dataset.h
View File

@ -172,7 +172,7 @@ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,

24
gtsam/slam/expressions.h
View File

@ -75,30 +75,6 @@ inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
return Unit3_(x, &Rot3::unrotate, p);
}
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
namespace internal {
// define a rotate and unrotate for Vector3
inline Vector3 rotate(const Rot3& R, const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) {
return R.rotate(v, H1, H2);
}
inline Vector3 unrotate(const Rot3& R, const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) {
return R.unrotate(v, H1, H2);
}
} // namespace internal
inline Expression<Vector3> rotate(const Rot3_& R,
const Expression<Vector3>& v) {
return Expression<Vector3>(internal::rotate, R, v);
}
inline Expression<Vector3> unrotate(const Rot3_& R,
const Expression<Vector3>& v) {
return Expression<Vector3>(internal::unrotate, R, v);
}
#endif
// Projection
typedef Expression<Cal3_S2> Cal3_S2_;

48
gtsam/slam/tests/testFrobeniusFactor.cpp
View File

@ -188,54 +188,6 @@ TEST(FrobeniusBetweenFactorSO4, evaluateError) {
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
//******************************************************************************
namespace submanifold {
SO4 id;
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1.tail<3>());
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2.tail<3>());
SO4 Q2 = SO4::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace submanifold
/* ************************************************************************* */
TEST(FrobeniusWormholeFactor, evaluateError) {
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // dimension = 6 not 16
for (const size_t p : {5, 4, 3}) {
Matrix M = Matrix::Identity(p, p);
M.topLeftCorner(3, 3) = submanifold::R1.matrix();
SOn Q1(M);
M.topLeftCorner(3, 3) = submanifold::R2.matrix();
SOn Q2(M);
auto factor =
FrobeniusWormholeFactor(1, 2, Rot3(::so3::R12.matrix()), p, model);
Matrix H1, H2;
factor.evaluateError(Q1, Q2, H1, H2);
// Test derivatives
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
}
/* ************************************************************************* */
TEST(FrobeniusWormholeFactor, equivalenceToSO3) {
using namespace ::submanifold;
auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1));
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension
auto factor3 = FrobeniusBetweenFactor<SO3>(1, 2, R12, model);
auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model);
const Matrix3 E3(factor3.evaluateError(R1, R2).data());
const Matrix43 E4(
factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data());
EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9));
EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

6
gtsam_unstable/CMakeLists.txt
View File

@ -108,7 +108,7 @@ list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
# Wrap version for gtsam_unstable
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
if (GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
# Set up codegen
include(GtsamMatlabWrap)
@ -119,8 +119,8 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
endif()
# Wrap
wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}")
endif(GTSAM_INSTALL_MATLAB_TOOLBOX)
wrap_and_install_library(gtsam_unstable.i "gtsam" "" "${mexFlags}")
endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
# Build examples

6
gtsam_unstable/gtsam_unstable.h → gtsam_unstable/gtsam_unstable.i
View File

@ -12,10 +12,14 @@ class gtsam::Point2Vector;
class gtsam::Rot2;
class gtsam::Pose2;
class gtsam::Point3;
class gtsam::SO3;
class gtsam::SO4;
class gtsam::SOn;
class gtsam::Rot3;
class gtsam::Pose3;
virtual class gtsam::noiseModel::Base;
virtual class gtsam::noiseModel::Gaussian;
virtual class gtsam::noiseModel::Isotropic;
virtual class gtsam::imuBias::ConstantBias;
virtual class gtsam::NonlinearFactor;
virtual class gtsam::NoiseModelFactor;
@ -39,6 +43,7 @@ class gtsam::KeyVector;
class gtsam::LevenbergMarquardtParams;
class gtsam::ISAM2Params;
class gtsam::GaussianDensity;
class gtsam::LevenbergMarquardtOptimizer;
namespace gtsam {
@ -282,7 +287,6 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
void serializable() const; // enabling serialization functionality
};
#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::PoseRTV}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {

5
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -114,7 +114,7 @@ public:
/** implement functions needed to derive from Factor */
/* ************************************************************************* */
virtual double error(const Values& x) const {
double error(const Values &x) const override {
return whitenedError(x).squaredNorm();
}
@ -125,8 +125,7 @@ public:
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
/* This version of linearize recalculates the noise model each time */
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& x) const {
boost::shared_ptr<GaussianFactor> linearize(const Values &x) const override {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();

215
gtsam_unstable/timing/process_shonan_timing_results.py Normal file
View File

@ -0,0 +1,215 @@
"""
Process timing results from timeShonanAveraging
"""
import xlrd
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.ticker import FuncFormatter
import heapq
from collections import Counter
def make_combined_plot(name, p_values, times, costs, min_cost_range=10):
""" Make a plot that combines timing and SO(3) cost.
Arguments:
name: string of the plot title
p_values: list of p-values (int)
times: list of timings (seconds)
costs: list of costs (double)
Will calculate the range of the costs, default minimum range = 10.0
"""
min_cost = min(costs)
cost_range = max(max(costs)-min_cost,min_cost_range)
fig = plt.figure()
ax1 = fig.add_subplot(111)
ax1.plot(p_values, times, label="time")
ax1.set_ylabel('Time used to optimize \ seconds')
ax1.set_xlabel('p_value')
ax2 = ax1.twinx()
ax2.plot(p_values, costs, 'r', label="cost")
ax2.set_ylabel('Cost at SO(3) form')
ax2.set_xlabel('p_value')
ax2.set_xticks(p_values)
ax2.set_ylim(min_cost, min_cost + cost_range)
plt.title(name, fontsize=12)
ax1.legend(loc="upper left")
ax2.legend(loc="upper right")
plt.interactive(False)
plt.show()
def make_convergence_plot(name, p_values, times, costs, iter=10):
""" Make a bar that show the success rate for each p_value according to whether the SO(3) cost converges
Arguments:
name: string of the plot title
p_values: list of p-values (int)
times: list of timings (seconds)
costs: list of costs (double)
iter: int of iteration number for each p_value
"""
max_cost = np.mean(np.array(heapq.nlargest(iter, costs)))
# calculate mean costs for each p value
p_values = list(dict(Counter(p_values)).keys())
# make sure the iter number
iter = int(len(times)/len(p_values))
p_mean_cost = [np.mean(np.array(costs[i*iter:(i+1)*iter])) for i in range(len(p_values))]
p_max = p_values[p_mean_cost.index(max(p_mean_cost))]
# print(p_mean_cost)
# print(p_max)
#take mean and make the combined plot
mean_times = []
mean_costs = []
for p in p_values:
costs_tmp = costs[p_values.index(p)*iter:(p_values.index(p)+1)*iter]
mean_cost = sum(costs_tmp)/len(costs_tmp)
mean_costs.append(mean_cost)
times_tmp = times[p_values.index(p)*iter:(p_values.index(p)+1)*iter]
mean_time = sum(times_tmp)/len(times_tmp)
mean_times.append(mean_time)
make_combined_plot(name, p_values,mean_times, mean_costs)
# calculate the convergence rate for each p_value
p_success_rates = []
if p_mean_cost[0] >= 0.95*np.mean(np.array(costs)) and p_mean_cost[0] <= 1.05*np.mean(np.array(costs)):
p_success_rates = [ 1.0 for p in p_values]
else:
for p in p_values:
if p > p_max:
p_costs = costs[p_values.index(p)*iter:(p_values.index(p)+1)*iter]
# print(p_costs)
converged = [ int(p_cost < 0.3*max_cost) for p_cost in p_costs]
success_rate = sum(converged)/len(converged)
p_success_rates.append(success_rate)
else:
p_success_rates.append(0)
plt.bar(p_values, p_success_rates, align='center', alpha=0.5)
plt.xticks(p_values)
plt.yticks(np.arange(0, 1.2, 0.2), ['0%', '20%', '40%', '60%', '80%', '100%'])
plt.xlabel("p_value")
plt.ylabel("success rate")
plt.title(name, fontsize=12)
plt.show()
def make_eigen_and_bound_plot(name, p_values, times1, costPs, cost3s, times2, min_eigens, subounds):
""" Make a plot that combines time for optimizing, time for optimizing and compute min_eigen,
min_eigen, subound (subound = (f_R - f_SDP) / f_SDP).
Arguments:
name: string of the plot title
p_values: list of p-values (int)
times1: list of timings (seconds)
costPs: f_SDP
cost3s: f_R
times2: list of timings (seconds)
min_eigens: list of min_eigen (double)
subounds: list of subound (double)
"""
if dict(Counter(p_values))[5] != 1:
p_values = list(dict(Counter(p_values)).keys())
iter = int(len(times1)/len(p_values))
p_mean_times1 = [np.mean(np.array(times1[i*iter:(i+1)*iter])) for i in range(len(p_values))]
p_mean_times2 = [np.mean(np.array(times2[i*iter:(i+1)*iter])) for i in range(len(p_values))]
print("p_values \n", p_values)
print("p_mean_times_opti \n", p_mean_times1)
print("p_mean_times_eig \n", p_mean_times2)
p_mean_costPs = [np.mean(np.array(costPs[i*iter:(i+1)*iter])) for i in range(len(p_values))]
p_mean_cost3s = [np.mean(np.array(cost3s[i*iter:(i+1)*iter])) for i in range(len(p_values))]
p_mean_lambdas = [np.mean(np.array(min_eigens[i*iter:(i+1)*iter])) for i in range(len(p_values))]
print("p_mean_costPs \n", p_mean_costPs)
print("p_mean_cost3s \n", p_mean_cost3s)
print("p_mean_lambdas \n", p_mean_lambdas)
print("*******************************************************************************************************************")
else:
plt.figure(1)
plt.ylabel('Time used (seconds)')
plt.xlabel('p_value')
plt.plot(p_values, times1, 'r', label="time for optimizing")
plt.plot(p_values, times2, 'blue', label="time for optimizing and check")
plt.title(name, fontsize=12)
plt.legend(loc="best")
plt.interactive(False)
plt.show()
plt.figure(2)
plt.ylabel('Min eigen_value')
plt.xlabel('p_value')
plt.plot(p_values, min_eigens, 'r', label="min_eigen values")
plt.title(name, fontsize=12)
plt.legend(loc="best")
plt.interactive(False)
plt.show()
plt.figure(3)
plt.ylabel('sub_bounds')
plt.xlabel('p_value')
plt.plot(p_values, subounds, 'blue', label="sub_bounds")
plt.title(name, fontsize=12)
plt.legend(loc="best")
plt.show()
# Process arguments and call plot function
import argparse
import csv
import os
parser = argparse.ArgumentParser()
parser.add_argument("path")
args = parser.parse_args()
file_path = []
domain = os.path.abspath(args.path)
for info in os.listdir(args.path):
file_path.append(os.path.join(domain, info))
file_path.sort()
print(file_path)
# name of all the plots
names = {}
names[0] = 'tinyGrid3D vertex = 9, edge = 11'
names[1] = 'smallGrid3D vertex = 125, edge = 297'
names[2] = 'parking-garage vertex = 1661, edge = 6275'
names[3] = 'sphere2500 vertex = 2500, edge = 4949'
# names[4] = 'sphere_bignoise vertex = 2200, edge = 8647'
names[5] = 'torus3D vertex = 5000, edge = 9048'
names[6] = 'cubicle vertex = 5750, edge = 16869'
names[7] = 'rim vertex = 10195, edge = 29743'
# Parse CSV file
for key, name in names.items():
print(key, name)
# find according file to process
name_file = None
for path in file_path:
if name[0:3] in path:
name_file = path
if name_file == None:
print("The file %s is not in the path" % name)
continue
p_values, times1, costPs, cost3s, times2, min_eigens, subounds = [],[],[],[],[],[],[]
with open(name_file) as csvfile:
reader = csv.reader(csvfile, delimiter='\t')
for row in reader:
print(row)
p_values.append(int(row[0]))
times1.append(float(row[1]))
costPs.append(float(row[2]))
cost3s.append(float(row[3]))
if len(row) > 4:
times2.append(float(row[4]))
min_eigens.append(float(row[5]))
subounds.append(float(row[6]))
#plot
# make_combined_plot(name, p_values, times1, cost3s)
# make_convergence_plot(name, p_values, times1, cost3s)
make_eigen_and_bound_plot(name, p_values, times1, costPs, cost3s, times2, min_eigens, subounds)

182
gtsam_unstable/timing/timeShonanAveraging.cpp Normal file
View File

@ -0,0 +1,182 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 testShonanAveraging.cpp
* @date September 2019
* @author Jing Wu
* @author Frank Dellaert
* @brief Timing script for Shonan Averaging algorithm
*/
#include "gtsam/base/Matrix.h"
#include "gtsam/base/Vector.h"
#include "gtsam/geometry/Point3.h"
#include "gtsam/geometry/Rot3.h"
#include <gtsam/base/timing.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <CppUnitLite/TestHarness.h>
#include <chrono>
#include <fstream>
#include <iostream>
#include <map>
using namespace std;
using namespace gtsam;
// string g2oFile = findExampleDataFile("toyExample.g2o");
// save a single line of timing info to an output stream
void saveData(size_t p, double time1, double costP, double cost3, double time2,
double min_eigenvalue, double suBound, std::ostream* os) {
*os << (int)p << "\t" << time1 << "\t" << costP << "\t" << cost3 << "\t"
<< time2 << "\t" << min_eigenvalue << "\t" << suBound << endl;
}
void checkR(const Matrix& R) {
Matrix R2 = R.transpose();
Matrix actual_R = R2 * R;
assert_equal(Rot3(),Rot3(actual_R));
}
void saveResult(string name, const Values& values) {
ofstream myfile;
myfile.open("shonan_result_of_" + name + ".dat");
size_t nrSO3 = values.count<SO3>();
myfile << "#Type SO3 Number " << nrSO3 << "\n";
for (int i = 0; i < nrSO3; ++i) {
Matrix R = values.at<SO3>(i).matrix();
// Check if the result of R.Transpose*R satisfy orthogonal constraint
checkR(R);
myfile << i;
for (int m = 0; m < 3; ++m) {
for (int n = 0; n < 3; ++n) {
myfile << " " << R(m, n);
}
}
myfile << "\n";
}
myfile.close();
cout << "Saved shonan_result.dat file" << endl;
}
void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses) {
ofstream myfile;
myfile.open("shonan_result_of_" + name + ".g2o");
size_t nrSO3 = values.count<SO3>();
for (int i = 0; i < nrSO3; ++i) {
Matrix R = values.at<SO3>(i).matrix();
// Check if the result of R.Transpose*R satisfy orthogonal constraint
checkR(R);
myfile << "VERTEX_SE3:QUAT" << " ";
myfile << i << " ";
myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
Vector quaternion = Rot3(R).quaternion();
myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) << " " << quaternion(0);
myfile << "\n";
}
myfile.close();
cout << "Saved shonan_result.g2o file" << endl;
}
void saveResultQuat(const Values& values) {
ofstream myfile;
myfile.open("shonan_result.dat");
size_t nrSOn = values.count<SOn>();
for (int i = 0; i < nrSOn; ++i) {
GTSAM_PRINT(values.at<SOn>(i));
Rot3 R = Rot3(values.at<SOn>(i).matrix());
float x = R.toQuaternion().x();
float y = R.toQuaternion().y();
float z = R.toQuaternion().z();
float w = R.toQuaternion().w();
myfile << "QuatSO3 " << i;
myfile << "QuatSO3 " << i << " " << w << " " << x << " " << y << " " << z << "\n";
myfile.close();
}
}
int main(int argc, char* argv[]) {
// primitive argument parsing:
if (argc > 3) {
throw runtime_error("Usage: timeShonanAveraging [g2oFile]");
}
string g2oFile;
try {
if (argc > 1)
g2oFile = argv[argc - 1];
else
g2oFile = string(
"/home/jingwu/git/SESync/data/sphere2500.g2o");
} catch (const exception& e) {
cerr << e.what() << '\n';
exit(1);
}
// Create a csv output file
size_t pos1 = g2oFile.find("data/");
size_t pos2 = g2oFile.find(".g2o");
string name = g2oFile.substr(pos1 + 5, pos2 - pos1 - 5);
cout << name << endl;
ofstream csvFile("shonan_timing_of_" + name + ".csv");
// Create Shonan averaging instance from the file.
// ShonanAveragingParameters parameters;
// double sigmaNoiseInRadians = 0 * M_PI / 180;
// parameters.setNoiseSigma(sigmaNoiseInRadians);
static const ShonanAveraging3 kShonan(g2oFile);
// increase p value and try optimize using Shonan Algorithm. use chrono for
// timing
size_t pMin = 3;
Values Qstar;
Vector minEigenVector;
double CostP = 0, Cost3 = 0, lambdaMin = 0, suBound = 0;
cout << "(int)p" << "\t" << "time1" << "\t" << "costP" << "\t" << "cost3" << "\t"
<< "time2" << "\t" << "MinEigenvalue" << "\t" << "SuBound" << endl;
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
for (size_t p = pMin; p < 6; p++) {
// Randomly initialize at lowest level, initialize by line search after that
const Values initial =
(p > pMin) ? kShonan.initializeWithDescent(p, Qstar, minEigenVector,
lambdaMin)
: ShonanAveraging::LiftTo<Rot3>(pMin, randomRotations);
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
// optimizing
const Values result = kShonan.tryOptimizingAt(p, initial);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> timeUsed1 =
chrono::duration_cast<chrono::duration<double>>(t2 - t1);
lambdaMin = kShonan.computeMinEigenValue(result, &minEigenVector);
chrono::steady_clock::time_point t3 = chrono::steady_clock::now();
chrono::duration<double> timeUsed2 =
chrono::duration_cast<chrono::duration<double>>(t3 - t1);
Qstar = result;
CostP = kShonan.costAt(p, result);
const Values SO3Values = kShonan.roundSolution(result);
Cost3 = kShonan.cost(SO3Values);
suBound = (Cost3 - CostP) / CostP;
saveData(p, timeUsed1.count(), CostP, Cost3, timeUsed2.count(),
lambdaMin, suBound, &cout);
saveData(p, timeUsed1.count(), CostP, Cost3, timeUsed2.count(),
lambdaMin, suBound, &csvFile);
}
saveResult(name, kShonan.roundSolution(Qstar));
// saveG2oResult(name, kShonan.roundSolution(Qstar), kShonan.Poses());
return 0;
}

12
matlab/+gtsam/Point2.m Normal file
View File

@ -0,0 +1,12 @@
function pt = Point2(varargin)
% Point2 shim
if nargin == 2 && isa(varargin{1}, 'double')
pt = [varargin{1} varargin{2}]';
elseif nargin == 1
pt = varargin{1};
elseif nargin == 0
pt = [0 0]';
else
error('Arguments do not match any overload of Point2 shim');
end
end

12
matlab/+gtsam/Point3.m Normal file
View File

@ -0,0 +1,12 @@
function pt = Point3(varargin)
% Point3 shim
if nargin == 3 && isa(varargin{1}, 'double')
pt = [varargin{1} varargin{2} varargin{3}]';
elseif nargin == 1
pt = varargin{1};
elseif nargin == 0
pt = [0 0 0]';
else
error('Arguments do not match any overload of Point3 shim');
end
end

6
matlab/+gtsam/cylinderSampleProjection.m
View File

@ -50,9 +50,9 @@ for i = 1:cylinderNum
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
rayCameraToPoint = pose.translation().between(sampledPoint3);
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid);
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3);
% Condition 1: all points in front of the cylinders'
% surfaces are visible

20
matlab/+gtsam/cylinderSampleProjectionStereo.m
View File

@ -25,20 +25,20 @@ for i = 1:cylinderNum
% For Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transformTo(sampledPoint3);
if sampledPoint3local.z < 0
if sampledPoint3local(3) < 0
continue;
end
% measurements
Z.du = K.fx() * K.baseline() / sampledPoint3local.z;
Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
Z.du = K.fx() * K.baseline() / sampledPoint3local(3);
Z.uL = K.fx() * sampledPoint3local(1) / sampledPoint3local(3) + K.px();
Z.uR = Z.uL + Z.du;
Z.v = K.fy() / sampledPoint3local.z + K.py();
Z.v = K.fy() / sampledPoint3local(3) + K.py();
% ignore points not visible in the scene
if Z.uL < 0 || Z.uL >= imageSize.x || ...
Z.uR < 0 || Z.uR >= imageSize.x || ...
Z.v < 0 || Z.v >= imageSize.y
if Z.uL < 0 || Z.uL >= imageSize(1) || ...
Z.uR < 0 || Z.uR >= imageSize(1) || ...
Z.v < 0 || Z.v >= imageSize(2)
continue;
end
@ -54,9 +54,9 @@ for i = 1:cylinderNum
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
rayCameraToPoint = sampledPoint3 - pose.translation();
rayCameraToCylinder = cylinders{k}.centroid - pose.translation();
rayCylinderToPoint = sampledPoint3 - cylinders{k}.centroid;
% Condition 1: all points in front of the cylinders'
% surfaces are visible

6
matlab/+gtsam/cylinderSampling.m
View File

@ -12,8 +12,8 @@ function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
% sample the points
for i = 1:pointsNum
theta = 2 * pi * rand;
x = radius * cos(theta) + baseCentroid.x;
y = radius * sin(theta) + baseCentroid.y;
x = radius * cos(theta) + baseCentroid(1);
y = radius * sin(theta) + baseCentroid(2);
z = height * rand;
points3{i,1} = Point3([x,y,z]');
end
@ -22,5 +22,5 @@ function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
cylinder.radius = radius;
cylinder.height = height;
cylinder.Points = points3;
cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
cylinder.centroid = Point3(baseCentroid(1), baseCentroid(2), height/2);
end

2
matlab/+gtsam/plotCamera.m
View File

@ -1,7 +1,7 @@
function plotCamera(pose, axisLength)
hold on
C = pose.translation().vector();
C = pose.translation();
R = pose.rotation().matrix();
xAxis = C+R(:,1)*axisLength;

16
matlab/+gtsam/plotFlyingResults.m
View File

@ -13,7 +13,7 @@ set(gcf, 'Position', [80,1,1800,1000]);
%% plot all the cylinders and sampled points
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
axis([0, options.fieldSize(1), 0, options.fieldSize(2), 0, options.height + 30]);
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Height (m)');
@ -50,8 +50,8 @@ for i = 1:cylinderNum
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
X = X + cylinders{i}.centroid.x;
Y = Y + cylinders{i}.centroid.y;
X = X + cylinders{i}.centroid(1);
Y = Y + cylinders{i}.centroid(2);
Z = Z * cylinders{i}.height;
h_cylinder{i} = surf(X,Y,Z);
@ -76,7 +76,7 @@ for i = 1:posesSize
%plotCamera(poses{i}, 3);
gRp = poses{i}.rotation().matrix(); % rotation from pose to global
C = poses{i}.translation().vector();
C = poses{i}.translation();
axisLength = 3;
xAxis = C+gRp(:,1)*axisLength;
@ -111,14 +111,14 @@ for i = 1:posesSize
for j = 1:pointSize
if ~isempty(pts3d{j}.cov{i})
hold on
h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
h_point{j} = plot3(pts3d{j}.data(1), pts3d{j}.data(2), pts3d{j}.data(3));
h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data(1); pts3d{j}.data(2); pts3d{j}.data(3)], ...
pts3d{j}.cov{i}, options.plot.covarianceScale);
end
end
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
axis([0, options.fieldSize(1), 0, options.fieldSize(2), 0, options.height + 30]);
drawnow
@ -158,7 +158,7 @@ for i = 1 : posesSize
hold on
campos([poses{i}.x, poses{i}.y, poses{i}.z]);
camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
camtarget([options.fieldSize(1)/2, options.fieldSize(2)/2, 0]);
camlight(hlight, 'headlight');
if options.writeVideo

6
matlab/+gtsam/plotPoint2.m
View File

@ -1,10 +1,10 @@
function plotPoint2(p,color,P)
% plotPoint2 shows a Point2, possibly with covariance matrix
if size(color,2)==1
plot(p.x,p.y,[color '*']);
plot(p(1),p(2),[color '*']);
else
plot(p.x,p.y,color);
plot(p(1),p(2),color);
end
if exist('P', 'var') && (~isempty(P))
gtsam.covarianceEllipse([p.x;p.y],P,color(1));
gtsam.covarianceEllipse([p(1);p(2)],P,color(1));
end

6
matlab/+gtsam/plotPoint3.m
View File

@ -1,12 +1,12 @@
function plotPoint3(p, color, P)
%PLOTPOINT3 Plot a Point3 with an optional covariance matrix
if size(color,2)==1
plot3(p.x,p.y,p.z,[color '*']);
plot3(p(1),p(2),p(3),[color '*']);
else
plot3(p.x,p.y,p.z,color);
plot3(p(1),p(2),p(3),color);
end
if exist('P', 'var')
gtsam.covarianceEllipse3D([p.x;p.y;p.z],P);
gtsam.covarianceEllipse3D([p(1);p(2);p(3)],P);
end
end

2
matlab/+gtsam/plotPose3.m
View File

@ -4,7 +4,7 @@ if nargin<3,axisLength=0.1;end
% get rotation and translation (center)
gRp = pose.rotation().matrix(); % rotation from pose to global
C = pose.translation().vector();
C = pose.translation();
if ~isempty(axisLength)
% draw the camera axes

2
matlab/+gtsam/points2DTrackStereo.m
View File

@ -38,7 +38,7 @@ graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
%% initialize graph and values
initialEstimate = Values;
for i = 1:pointsNum
point_j = points3d{i}.data.retract(0.05*randn(3,1));
point_j = points3d{i}.data + (0.05*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
end

24
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -46,7 +46,7 @@ options.camera.horizon = 60;
% camera baseline
options.camera.baseline = 0.05;
% camera focal length
options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
options.camera.f = round(options.camera.resolution(1) * options.camera.horizon / ...
options.camera.fov);
% camera focal baseline
options.camera.fB = options.camera.f * options.camera.baseline;
@ -54,15 +54,15 @@ options.camera.fB = options.camera.f * options.camera.baseline;
options.camera.disparity = options.camera.fB / options.camera.horizon;
% Monocular Camera Calibration
options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2);
options.camera.resolution(1)/2, options.camera.resolution(2)/2);
% Stereo Camera Calibration
options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
options.camera.resolution(1)/2, options.camera.resolution(2)/2, options.camera.disparity);
% write video output
options.writeVideo = true;
% the testing field size (unit: meter)
options.fieldSize = Point2([100, 100]');
options.fieldSize = Point2(100, 100);
% camera flying speed (unit: meter / second)
options.speed = 20;
% camera flying height
@ -113,14 +113,14 @@ theta = 0;
i = 1;
while i <= cylinderNum
theta = theta + 2*pi/10;
x = 40 * rand * cos(theta) + options.fieldSize.x/2;
y = 20 * rand * sin(theta) + options.fieldSize.y/2;
baseCentroid{i} = Point2([x, y]');
x = 40 * rand * cos(theta) + options.fieldSize(1)/2;
y = 20 * rand * sin(theta) + options.fieldSize(2)/2;
baseCentroid{i} = Point2(x, y);
% prevent two cylinders interact with each other
regenerate = false;
for j = 1:i-1
if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
if i > 1 && norm(baseCentroid{i} - baseCentroid{j}) < options.cylinder.radius * 2
regenerate = true;
break;
end
@ -146,17 +146,17 @@ while 1
angle = 0.1*pi*(rand-0.5);
inc_t = Point3(distance * cos(angle), ...
distance * sin(angle), 0);
t = t.compose(inc_t);
t = t + inc_t;
if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10;
if t(1) > options.fieldSize(1) - 10 || t(2) > options.fieldSize(2) - 10;
break;
end
%t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
% 15, 10]');
camera = PinholeCameraCal3_S2.Lookat(t, ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.camera.monoK);
Point3(options.fieldSize(1)/2, options.fieldSize(2)/2, 0), ...
Point3(0,0,1), options.camera.monoK);
cameraPoses{end+1} = camera.pose;
end

4
matlab/gtsam_examples/MonocularVOExample.m
View File

@ -15,14 +15,14 @@ import gtsam.*
%% Create two cameras and corresponding essential matrix E
aRb = Rot3.Yaw(pi/2);
aTb = Point3 (0.1, 0, 0);
aTb = [.1, 0, 0]';
identity=Pose3;
aPb = Pose3(aRb, aTb);
cameraA = CalibratedCamera(identity);
cameraB = CalibratedCamera(aPb);
%% Create 5 points
P = { Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5) };
P = { [0, 0, 1]', [-0.1, 0, 1]', [0.1, 0, 1]', [0, 0.5, 0.5]', [0, -0.5, 0.5]' };
%% Project points in both cameras
for i=1:5

9
matlab/gtsam_examples/PlanarSLAMExample.m
View File

@ -71,9 +71,12 @@ marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals);
plot2DPoints(result, 'b', marginals);
plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-');
plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-');
plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-');
p_j1 = result.atPoint2(j1);
p_j2 = result.atPoint2(j2);
plot([result.atPose2(i1).x; p_j1(1)],[result.atPose2(i1).y; p_j1(2)], 'c-');
plot([result.atPose2(i2).x; p_j1(1)],[result.atPose2(i2).y; p_j1(2)], 'c-');
plot([result.atPose2(i3).x; p_j2(1)],[result.atPose2(i3).y; p_j2(2)], 'c-');
axis([-0.6 4.8 -1 1])
axis equal
view(2)

11
matlab/gtsam_examples/PlanarSLAMExample_sampling.m
View File

@ -60,15 +60,18 @@ for j=1:2
S{j}=chol(Q{j}); % for sampling
end
plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-');
plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-');
plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-');
p_j1 = sample.atPoint2(j1);
p_j2 = sample.atPoint2(j2);
plot([sample.atPose2(i1).x; p_j1(1)],[sample.atPose2(i1).y; p_j1(2)], 'c-');
plot([sample.atPose2(i2).x; p_j1(1)],[sample.atPose2(i2).y; p_j1(2)], 'c-');
plot([sample.atPose2(i3).x; p_j2(1)],[sample.atPose2(i3).y; p_j2(2)], 'c-');
view(2); axis auto; axis equal
%% Do Sampling on point 2
N=1000;
for s=1:N
delta = S{2}*randn(2,1);
proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2));
proposedPoint = Point2(point{2} + delta);
plotPoint2(proposedPoint,'k.')
end

2
matlab/gtsam_examples/Pose2SLAMwSPCG.m
View File

@ -54,7 +54,7 @@ initialEstimate.print(sprintf('\nInitial estimate:\n'));
%% Optimize using Levenberg-Marquardt optimization with SubgraphSolver
params = gtsam.LevenbergMarquardtParams;
subgraphParams = gtsam.SubgraphSolverParameters;
params.setLinearSolverType('CONJUGATE_GRADIENT');
params.setLinearSolverType('ITERATIVE');
params.setIterativeParams(subgraphParams);
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimize();

4
matlab/gtsam_examples/SBAExample.m
View File

@ -47,7 +47,7 @@ end
%% Add Gaussian priors for a pose and a landmark to constrain the system
cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
firstCamera = PinholeCameraCal3_S2(truth.cameras{1}.pose, truth.K);
graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise));
graph.add(PriorFactorPinholeCameraCal3_S2(symbol('c',1), firstCamera, cameraPriorNoise));
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
@ -64,7 +64,7 @@ for i=1:size(truth.cameras,2)
initialEstimate.insert(symbol('c',i), camera_i);
end
for j=1:size(truth.points,2)
point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
point_j = Point3(truth.points{j} + 0.1*randn(3,1));
initialEstimate.insert(symbol('p',j), point_j);
end
initialEstimate.print(sprintf('\nInitial estimate:\n '));

2
matlab/gtsam_examples/SFMExample.m
View File

@ -58,7 +58,7 @@ for i=1:size(truth.cameras,2)
initialEstimate.insert(symbol('x',i), pose_i);
end
for j=1:size(truth.points,2)
point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
point_j = Point3(truth.points{j} + 0.1*randn(3,1));
initialEstimate.insert(symbol('p',j), point_j);
end
initialEstimate.print(sprintf('\nInitial estimate:\n '));

8
matlab/gtsam_examples/StereoVOExample.m
View File

@ -51,10 +51,10 @@ graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l
initialEstimate = Values;
initialEstimate.insert(x1, first_pose);
% noisy estimate for pose 2
initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)));
initialEstimate.insert(l1, Point3( 1, 1, 5));
initialEstimate.insert(l2, Point3(-1, 1, 5));
initialEstimate.insert(l3, Point3( 0,-.5, 5));
initialEstimate.insert(x2, Pose3(Rot3(), [0.1, -.1, 1.1]'));
initialEstimate.insert(l1, [ 1, 1, 5]');
initialEstimate.insert(l2, [-1, 1, 5]');
initialEstimate.insert(l3, [ 0,-.5, 5]');
%% optimize
fprintf(1,'Optimizing\n'); tic

2
matlab/gtsam_examples/VisualizeMEstimators.m
View File

@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename)
rho = zeros(size(x));
for i = 1:size(x, 2)
w(i) = model.weight(x(i));
rho(i) = model.residual(x(i));
rho(i) = model.loss(x(i));
end
psi = w .* x;

2
matlab/gtsam_tests/testPlanarSLAMExample.m
View File

@ -66,4 +66,4 @@ CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
point_1 = result.atPoint2(symbol('l',1));
marginals.marginalCovariance(symbol('l',1));
CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4));
CHECK('point_1.equals(Point2(2,2),1e-4)',norm(point_1 - Point2(2,2)) < 1e-4);

2
matlab/gtsam_tests/testSFMExample.m
View File

@ -69,5 +69,5 @@ end
for j=1:size(truth.points,2)
point_j = result.atPoint3(symbol('p',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
CHECK('point_j.equals(truth.points{j},1e-5)',norm(point_j - truth.points{j}) < 1e-5)
end

2
matlab/gtsam_tests/testStereoVOExample.m
View File

@ -65,4 +65,4 @@ pose_x1 = result.atPose3(x1);
CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
point_l1 = result.atPoint3(l1);
CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4));
CHECK('point_1.equals(expected_l1,1e-4)',norm(point_l1 - expected_l1) < 1e-4);

8
matlab/gtsam_tests/testValues.m
View File

@ -5,8 +5,8 @@ values = Values;
E = EssentialMatrix(Rot3,Unit3);
tol = 1e-9;
values.insert(0, Point2);
values.insert(1, Point3);
values.insert(0, Point2(0, 0));
values.insert(1, Point3(0, 0, 0));
values.insert(2, Rot2);
values.insert(3, Pose2);
values.insert(4, Rot3);
@ -21,8 +21,8 @@ values.insert(10, imuBias.ConstantBias);
values.insert(11, [1;2;3]);
values.insert(12, [1 2;3 4]);
EXPECT('at',values.atPoint2(0).equals(Point2,tol));
EXPECT('at',values.atPoint3(1).equals(Point3,tol));
EXPECT('at',values.atPoint2(0) == Point2(0, 0));
EXPECT('at',values.atPoint3(1) == Point3(0, 0, 0));
EXPECT('at',values.atRot2(2).equals(Rot2,tol));
EXPECT('at',values.atPose2(3).equals(Pose2,tol));
EXPECT('at',values.atRot3(4).equals(Rot3,tol));

2
matlab/gtsam_tests/testVisualISAMExample.m
View File

@ -51,5 +51,5 @@ end
for j=1:size(truth.points,2)
point_j = result.atPoint3(symbol('l',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
CHECK('point_j.equals(truth.points{j},1e-5)',norm(point_j - truth.points{j}) < 1e-5)
end

12
matlab/unstable_examples/+imuSimulator/IMUComparison.m
View File

@ -28,19 +28,19 @@ currentVelocityGlobalIMUbody = currentVelocityGlobal;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
positions(:,1) = currentPoseGlobal.translation;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Trajectory estimate (integrated in the navigation frame)
positionsIMUnav = zeros(3, length(times)+1);
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation;
posesIMUnav(1).p = positionsIMUnav(:,1);
posesIMUnav(1).R = poses(1).R;
% Trajectory estimate (integrated in the body frame)
positionsIMUbody = zeros(3, length(times)+1);
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation;
posesIMUbody(1).p = positionsIMUbody(:,1);
posesIMUbody(1).R = poses(1).R;
@ -120,9 +120,9 @@ for t = times
currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;
positions(:,i) = currentPoseGlobal.translation;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation;
% -
poses(i).p = positions(:,i);
posesIMUbody(i).p = positionsIMUbody(:,i);

4
matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
View File

@ -28,7 +28,7 @@ currentVelocityGlobal = velocity;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
positions(:,1) = currentPoseGlobal.translation;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
@ -112,7 +112,7 @@ for t = times
end
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positions(:,i) = currentPoseGlobal.translation;
i = i + 1;
end

2
matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m
View File

@ -7,7 +7,7 @@ measuredOmega = omega1Body;
% Acceleration measurement (in this simple toy example no other forces
% act on the body and the only acceleration is the centripetal Coriolis acceleration)
measuredAcc = Point3(cross(omega1Body, velocity1Body)).vector;
measuredAcc = Point3(cross(omega1Body, velocity1Body));
acc_omega = [ measuredAcc; measuredOmega ];
end

6
matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m
View File

@ -6,16 +6,16 @@ import gtsam.*;
% Calculate gyro measured rotation rate by transforming body rotation rate
% into the IMU frame.
measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body)).vector;
measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body));
% Transform both velocities into IMU frame, accounting for the velocity
% induced by rigid body rotation on a lever arm (Coriolis effect).
velocity1inertial = imuFrame.rotation.unrotate( ...
Point3(velocity1Body + cross(omega1Body, imuFrame.translation.vector))).vector;
Point3(velocity1Body + cross(omega1Body, imuFrame.translation)));
imu2in1 = Rot3.Expmap(measuredOmega * deltaT);
velocity2inertial = imu2in1.rotate(imuFrame.rotation.unrotate( ...
Point3(velocity2Body + cross(omega2Body, imuFrame.translation.vector)))).vector;
Point3(velocity2Body + cross(omega2Body, imuFrame.translation))));
% Acceleration in IMU frame
measuredAcc = (velocity2inertial - velocity1inertial) / deltaT;

30
matlab/unstable_examples/+imuSimulator/coriolisExample.m
View File

@ -190,13 +190,13 @@ for i = 1:length(times)
newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
% Store data
positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector;
positionsInFixedGT(:,1) = currentPoseFixedGT.translation;
velocityInFixedGT(:,1) = currentVelocityFixedGT;
positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector;
%velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector;
positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector;
positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation;
%velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity;
positionsEstimates(:,i) = currentPoseEstimate.translation;
velocitiesEstimates(:,i) = currentVelocityEstimate;
currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation;
currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix;
else
@ -204,18 +204,18 @@ for i = 1:length(times)
% Update the position and velocity
% x_t = x_0 + v_0*dt + 1/2*a_0*dt^2
% v_t = v_0 + a_0*dt
currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ...
currentPositionFixedGT = Point3(currentPoseFixedGT.translation ...
+ currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT);
currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT;
currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation
% Rotate pose in fixed frame to get pose in rotating frame
previousPositionRotatingGT = currentPoseRotatingGT.translation.vector;
previousPositionRotatingGT = currentPoseRotatingGT.translation;
currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame);
inverseCurrentRotatingFrame = (currentRotatingFrame.inverse);
currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT);
currentPositionRotatingGT = currentPoseRotatingGT.translation.vector;
currentPositionRotatingGT = currentPoseRotatingGT.translation;
% Get velocity in rotating frame by treating it like a position and using compose
% Maybe Luca knows a better way to do this within GTSAM.
@ -230,11 +230,11 @@ for i = 1:length(times)
% - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT;
% Store GT (ground truth) poses
positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector;
positionsInFixedGT(:,i) = currentPoseFixedGT.translation;
velocityInFixedGT(:,i) = currentVelocityFixedGT;
positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector;
positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation;
velocityInRotatingGT(:,i) = currentVelocityRotatingGT;
currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector;
currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation;
currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix;
%% Estimate trajectory in rotating frame using GTSAM (ground truth measurements)
@ -303,9 +303,9 @@ for i = 1:length(times)
currentVelocityEstimate = isam.calculateEstimate(currentVelKey);
currentBias = isam.calculateEstimate(currentBiasKey);
positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
biasEstimates(:,i) = currentBias.vector;
positionsEstimates(:,i) = currentPoseEstimate.translation;
velocitiesEstimates(:,i) = currentVelocityEstimate;
biasEstimates(:,i) = currentBias;
% In matrix form: R_error = R_gt'*R_estimate
% Perform Logmap on the rotation matrix to get a vector

10
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
View File

@ -151,14 +151,14 @@ hold on;
if options.includeCameraFactors
b = [-1000 2000 -2000 2000 -30 30];
for i = 1:size(metadata.camera.gtLandmarkPoints,2)
p = metadata.camera.gtLandmarkPoints(i).vector;
p = metadata.camera.gtLandmarkPoints(i);
if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6))
plot3(p(1), p(2), p(3), 'k+');
end
end
pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0));
for i = 1:length(pointsToPlot)
p = pointsToPlot(i).vector;
p = pointsToPlot(i);
plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10);
end
end
@ -233,9 +233,9 @@ for k=1:numMonteCarloRuns
for i=0:options.trajectoryLength
% compute estimation errors
currentPoseKey = symbol('x', i);
gtPosition = gtValues.at(currentPoseKey).translation.vector;
estPosition = estimate.at(currentPoseKey).translation.vector;
estR = estimate.at(currentPoseKey).rotation.matrix;
gtPosition = gtValues.atPose3(currentPoseKey).translation;
estPosition = estimate.atPose3(currentPoseKey).translation;
estR = estimate.atPose3(currentPoseKey).rotation.matrix;
errPosition = estPosition - gtPosition;
% compute covariances:

8
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -14,7 +14,7 @@ graph = NonlinearFactorGraph;
for i=0:length(measurements)
% Get the current pose
currentPoseKey = symbol('x', i);
currentPose = values.at(currentPoseKey);
currentPose = values.atPose3(currentPoseKey);
if i==0
%% first time step, add priors
@ -26,11 +26,11 @@ for i=0:length(measurements)
% IMU velocity and bias priors
if options.includeIMUFactors == 1
currentVelKey = symbol('v', 0);
currentVel = values.at(currentVelKey).vector;
currentVel = values.atPoint3(currentVelKey);
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
currentBiasKey = symbol('b', 0);
currentBias = values.at(currentBiasKey);
currentBias = values.atPoint3(currentBiasKey);
graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias));
end
@ -155,7 +155,7 @@ for i=0:length(measurements)
if options.includeCameraFactors == 1
for j = 1:length(measurements(i).landmarks)
cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1);
cameraPixelMeasurement = measurements(i).landmarks(j).vector;
cameraPixelMeasurement = measurements(i).landmarks(j);
% Only add the measurement if it is in the image frame (based on calibration)
if(cameraPixelMeasurement(1) > 0 && cameraPixelMeasurement(2) > 0 ...
&& cameraPixelMeasurement(1) < 2*metadata.camera.calibration.px ...

6
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -40,7 +40,7 @@ if options.useRealData == 1
%% gt Between measurements
if options.includeBetweenFactors == 1 && i > 0
prevPose = values.at(currentPoseKey - 1);
prevPose = values.atPose3(currentPoseKey - 1);
deltaPose = prevPose.between(currentPose);
measurements(i).deltaVector = Pose3.Logmap(deltaPose);
end
@ -65,7 +65,7 @@ if options.useRealData == 1
IMUdeltaPose = IMUPose1.between(IMUPose2);
IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose);
IMUdeltaRotVector = IMUdeltaPoseVector(1:3);
IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame
IMUdeltaPositionVector = IMUPose2.translation - IMUPose1.translation; % translation in absolute frame
measurements(i).imu(j).deltaT = deltaT;
@ -88,7 +88,7 @@ if options.useRealData == 1
%% gt GPS measurements
if options.includeGPSFactors == 1 && i > 0
gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector;
gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation;
measurements(i).gpsPositionVector = gpsPositionVector;
end

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