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

Merge branch 'develop' into feature/RobustShonan

release/4.3a0
Varun Agrawal 2020-11-30 17:52:07 -05:00
parent 7391c103ec 2d22258ed1
commit bf93527ffc
38 changed files with 452 additions and 273 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -66,6 +66,8 @@ function configure()
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${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_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \
-DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DBOOST_ROOT=$BOOST_ROOT \ -DBOOST_ROOT=$BOOST_ROOT \
-DBoost_NO_SYSTEM_PATHS=ON \ -DBoost_NO_SYSTEM_PATHS=ON \

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

@ -63,17 +63,17 @@ jobs:
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy 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 "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)" echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
if [ "${{ matrix.compiler }}" = "gcc" ]; then if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}" echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "::set-env name=CXX::g++-${{ matrix.version }}" echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}" echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
echo "::set-env name=CXX::clang++-${{ matrix.version }}" echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi fi
- name: Check Boost version - name: Check Boost version
if: runner.os == 'Linux' if: runner.os == 'Linux'
@ -83,3 +83,9 @@ jobs:
if: runner.os == 'Linux' if: runner.os == 'Linux'
run: | run: |
bash .github/scripts/unix.sh -t bash .github/scripts/unix.sh -t
- name: Upload build directory
uses: actions/upload-artifact@v2
if: matrix.build_type == 'Release'
with:
name: gtsam-${{ matrix.name }}-${{ matrix.build_type }}
path: ${{ github.workspace }}/build/

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

@ -40,14 +40,20 @@ jobs:
brew install ProfFan/robotics/boost brew install ProfFan/robotics/boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }} brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}" echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "::set-env name=CXX::g++-${{ matrix.version }}" echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang" echo "CC=clang" >> $GITHUB_ENV
echo "::set-env name=CXX::clang++" echo "CXX=clang++" >> $GITHUB_ENV
fi fi
- name: Build and Test (macOS) - name: Build and Test (macOS)
if: runner.os == 'macOS' if: runner.os == 'macOS'
run: | run: |
bash .github/scripts/unix.sh -t bash .github/scripts/unix.sh -t
- name: Upload build directory
uses: actions/upload-artifact@v2
if: matrix.build_type == 'Release'
with:
name: gtsam-${{ matrix.name }}-${{ matrix.build_type }}
path: ${{ github.workspace }}/build/

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

@ -19,7 +19,7 @@ jobs:
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [ name: [
ubuntu-18.04-gcc-5, ubuntu-18.04-gcc-5,
# ubuntu-18.04-gcc-9, # TODO Disabled for now because of timeouts ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9, ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1, macOS-10.15-xcode-11.3.1,
ubuntu-18.04-gcc-5-tbb, ubuntu-18.04-gcc-5-tbb,
@ -33,11 +33,10 @@ jobs:
compiler: gcc compiler: gcc
version: "5" version: "5"
# TODO Disabled for now because of timeouts - name: ubuntu-18.04-gcc-9
# - name: ubuntu-18.04-gcc-9 os: ubuntu-18.04
# os: ubuntu-18.04 compiler: gcc
# compiler: gcc version: "9"
# version: "9"
- name: ubuntu-18.04-clang-9 - name: ubuntu-18.04-clang-9
os: ubuntu-18.04 os: ubuntu-18.04
@ -77,12 +76,12 @@ jobs:
if [ "${{ matrix.compiler }}" = "gcc" ]; then if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}" echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "::set-env name=CXX::g++-${{ matrix.version }}" echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}" echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
echo "::set-env name=CXX::clang++-${{ matrix.version }}" echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi fi
- name: Install (macOS) - name: Install (macOS)
if: runner.os == 'macOS' if: runner.os == 'macOS'
@ -92,17 +91,17 @@ jobs:
brew install ProfFan/robotics/boost brew install ProfFan/robotics/boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }} brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}" echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "::set-env name=CXX::g++-${{ matrix.version }}" echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang" echo "CC=clang" >> $GITHUB_ENV
echo "::set-env name=CXX::clang++" echo "CXX=clang++" >> $GITHUB_ENV
fi fi
- name: Set GTSAM_WITH_TBB Flag - name: Set GTSAM_WITH_TBB Flag
if: matrix.flag == 'tbb' if: matrix.flag == 'tbb'
run: | run: |
echo "::set-env name=GTSAM_WITH_TBB::ON" echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
echo "GTSAM Uses TBB" echo "GTSAM Uses TBB"
- name: Build (Linux) - name: Build (Linux)
if: runner.os == 'Linux' if: runner.os == 'Linux'

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

@ -24,6 +24,7 @@ jobs:
ubuntu-gcc-deprecated, ubuntu-gcc-deprecated,
ubuntu-gcc-quaternions, ubuntu-gcc-quaternions,
ubuntu-gcc-tbb, ubuntu-gcc-tbb,
ubuntu-cayleymap,
] ]
build_type: [Debug, Release] build_type: [Debug, Release]
@ -47,6 +48,12 @@ jobs:
version: "9" version: "9"
flag: tbb flag: tbb
- name: ubuntu-cayleymap
os: ubuntu-18.04
compiler: gcc
version: "9"
flag: cayley
steps: steps:
- name: Checkout - name: Checkout
uses: actions/checkout@master uses: actions/checkout@master
@ -64,17 +71,17 @@ jobs:
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy 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 "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)" echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
if [ "${{ matrix.compiler }}" = "gcc" ]; then if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "::set-env name=CC::gcc-${{ matrix.version }}" echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "::set-env name=CXX::g++-${{ matrix.version }}" echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else else
sudo apt-get install -y clang-${{ matrix.version }} g++-multilib sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
echo "::set-env name=CC::clang-${{ matrix.version }}" echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
echo "::set-env name=CXX::clang++-${{ matrix.version }}" echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi fi
- name: Install (macOS) - name: Install (macOS)
@ -83,32 +90,39 @@ jobs:
brew install cmake ninja boost brew install cmake ninja boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }} brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}" echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
echo "::set-env name=CXX::g++-${{ matrix.version }}" echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
else else
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "::set-env name=CC::clang" echo "CC=clang" >> $GITHUB_ENV
echo "::set-env name=CXX::clang++" echo "CXX=clang++" >> $GITHUB_ENV
fi fi
- name: Set Allow Deprecated Flag - name: Set Allow Deprecated Flag
if: matrix.flag == 'deprecated' if: matrix.flag == 'deprecated'
run: | run: |
echo "::set-env name=GTSAM_ALLOW_DEPRECATED_SINCE_V41::ON" echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV
echo "Allow deprecated since version 4.1" echo "Allow deprecated since version 4.1"
- name: Set Use Quaternions Flag - name: Set Use Quaternions Flag
if: matrix.flag == 'quaternions' if: matrix.flag == 'quaternions'
run: | run: |
echo "::set-env name=GTSAM_USE_QUATERNIONS::ON" echo "GTSAM_USE_QUATERNIONS=ON" >> $GITHUB_ENV
echo "Use Quaternions for rotations" echo "Use Quaternions for rotations"
- name: Set GTSAM_WITH_TBB Flag - name: Set GTSAM_WITH_TBB Flag
if: matrix.flag == 'tbb' if: matrix.flag == 'tbb'
run: | run: |
echo "::set-env name=GTSAM_WITH_TBB::ON" echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
echo "GTSAM Uses TBB" echo "GTSAM Uses TBB"
- name: Use Cayley Transform for Rot3
if: matrix.flag == 'cayley'
run: |
echo "GTSAM_POSE3_EXPMAP=OFF" >> $GITHUB_ENV
echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV
echo "GTSAM Uses Cayley map for Rot3"
- name: Build & Test - name: Build & Test
run: | run: |
bash .github/scripts/unix.sh -t bash .github/scripts/unix.sh -t

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

@ -18,16 +18,19 @@ jobs:
# Github Actions requires a single row to be added to the build 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. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [ name: [
windows-2016-cl, #TODO This build keeps timing out, need to understand why.
# windows-2016-cl,
windows-2019-cl, windows-2019-cl,
] ]
build_type: [Debug, Release] build_type: [Debug, Release]
build_unstable: [ON] build_unstable: [ON]
include: include:
- name: windows-2016-cl
os: windows-2016 #TODO This build keeps timing out, need to understand why.
compiler: cl # - name: windows-2016-cl
# os: windows-2016
# compiler: cl
- name: windows-2019-cl - name: windows-2019-cl
os: windows-2019 os: windows-2019
@ -50,17 +53,17 @@ jobs:
# See: https://github.com/DaanDeMeyer/doctest/runs/231595515 # See: https://github.com/DaanDeMeyer/doctest/runs/231595515
# See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413 # See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413
scoop install gcc --global scoop install gcc --global
echo "::set-env name=CC::gcc" echo "CC=gcc" >> $GITHUB_ENV
echo "::set-env name=CXX::g++" echo "CXX=g++" >> $GITHUB_ENV
} elseif ("${{ matrix.compiler }}" -eq "clang") { } elseif ("${{ matrix.compiler }}" -eq "clang") {
echo "::set-env name=CC::clang" echo "CC=clang" >> $GITHUB_ENV
echo "::set-env name=CXX::clang++" echo "CXX=clang++" >> $GITHUB_ENV
} else { } else {
echo "::set-env name=CC::${{ matrix.compiler }}" echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV
echo "::set-env name=CXX::${{ matrix.compiler }}" echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV
} }
# Scoop modifies the PATH so we make the modified PATH global. # Scoop modifies the PATH so we make the modified PATH global.
echo "::set-env name=PATH::$env:PATH" echo "$env:PATH" >> $GITHUB_PATH
- name: Build (Windows) - name: Build (Windows)
if: runner.os == 'Windows' if: runner.os == 'Windows'
run: | run: |
@ -73,3 +76,9 @@ jobs:
cmake --build build --config ${{ matrix.build_type }} --target check.base cmake --build build --config ${{ matrix.build_type }} --target check.base
cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable
cmake --build build --config ${{ matrix.build_type }} --target check.linear cmake --build build --config ${{ matrix.build_type }} --target check.linear
- name: Upload build directory
uses: actions/upload-artifact@v2
if: matrix.build_type == 'Release'
with:
name: gtsam-${{ matrix.name }}-${{ matrix.build_type }}
path: ${{ github.workspace }}/build/

77
cmake/GtsamMatlabWrap.cmake
View File

@ -1,51 +1,64 @@
# Check / set dependent variables for MATLAB wrapper
if(GTSAM_INSTALL_MATLAB_TOOLBOX)
find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED)
if(NOT Matlab_MEX_COMPILER)
message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.")
endif()
if(GTSAM_BUILD_TYPE_POSTFIXES)
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
endif()
if(NOT BUILD_SHARED_LIBS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
endif()
endif()
# Set up cache options # Set up cache options
option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF) option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF)
set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation") set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox") set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox")
if(NOT GTSAM_TOOLBOX_INSTALL_PATH) if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox") set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
endif() endif()
# GTSAM_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static # GTSAM_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static
# are already compiled into the library by the linker # are already compiled into the library by the linker
if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32) if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32)
message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).") message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
endif() endif()
# Try to automatically configure mex path set(MEX_COMMAND ${Matlab_MEX_COMPILER} CACHE PATH "Path to MATLAB MEX compiler")
if(APPLE) set(MATLAB_ROOT ${Matlab_ROOT_DIR} CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
file(GLOB matlab_bin_directories "/Applications/MATLAB*/bin")
set(mex_program_name "mex")
elseif(WIN32)
file(GLOB matlab_bin_directories "C:/Program Files*/MATLAB/*/bin")
set(mex_program_name "mex.bat")
else()
file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
set(mex_program_name "mex")
endif()
# Try to automatically configure mex path from provided custom `bin` path.
if(GTSAM_CUSTOM_MATLAB_PATH) if(GTSAM_CUSTOM_MATLAB_PATH)
set(matlab_bin_directories ${GTSAM_CUSTOM_MATLAB_PATH}) set(matlab_bin_directory ${GTSAM_CUSTOM_MATLAB_PATH})
endif()
# Run find_program explicitly putting $PATH after our predefined program if(WIN32)
# directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents set(mex_program_name "mex.bat")
# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is else()
# on the system path. set(mex_program_name "mex")
list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred endif()
find_program(MEX_COMMAND ${mex_program_name}
PATHS ${matlab_bin_directories} ENV PATH # Run find_program explicitly putting $PATH after our predefined program
NO_DEFAULT_PATH) # directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
mark_as_advanced(FORCE MEX_COMMAND) # finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
# Now that we have mex, trace back to find the Matlab installation root # on the system path.
get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH) find_program(MEX_COMMAND ${mex_program_name}
get_filename_component(mex_path "${MEX_COMMAND}" PATH) PATHS ${matlab_bin_directory} ENV PATH
if(mex_path MATCHES ".*/win64$") NO_DEFAULT_PATH)
get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
else() mark_as_advanced(FORCE MEX_COMMAND)
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE) # Now that we have mex, trace back to find the Matlab installation root
get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
get_filename_component(mex_path "${MEX_COMMAND}" PATH)
if(mex_path MATCHES ".*/win64$")
get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
else()
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
endif()
endif() endif()
set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
# User-friendly wrapping function. Builds a mex module from the provided # User-friendly wrapping function. Builds a mex module from the provided

23
cmake/HandleGeneralOptions.cmake
View File

@ -24,6 +24,7 @@ option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available"
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) option(GTSAM_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_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_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF)
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
@ -31,16 +32,14 @@ if(NOT MSVC AND NOT XCODE_VERSION)
option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON)
endif() endif()
# Options relating to MATLAB wrapper # Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa.
# TODO: Check for matlab mex binary before handling building of binaries if(GTSAM_POSE3_EXPMAP)
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) message(STATUS "GTSAM_POSE3_EXPMAP=ON, enabling GTSAM_ROT3_EXPMAP as well")
set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE)
elseif(GTSAM_ROT3_EXPMAP)
message(STATUS "GTSAM_ROT3_EXPMAP=ON, enabling GTSAM_POSE3_EXPMAP as well")
set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE)
endif()
# Set the default Python version. This is later updated in HandlePython.cmake.
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
# Check / set dependent variables for MATLAB wrapper
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
endif()

5
cmake/HandlePython.cmake
View File

@ -1,4 +1,5 @@
if(GTSAM_BUILD_PYTHON) # Set Python version if either Python or MATLAB wrapper is requested.
if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
# Get info about the Python3 interpreter # Get info about the Python3 interpreter
# https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
@ -14,7 +15,9 @@ if(GTSAM_BUILD_PYTHON)
"The version of Python to build the wrappers against." "The version of Python to build the wrappers against."
FORCE) FORCE)
endif() endif()
endif()
if(GTSAM_BUILD_PYTHON)
if(GTSAM_UNSTABLE_BUILD_PYTHON) if(GTSAM_UNSTABLE_BUILD_PYTHON)
if (NOT GTSAM_BUILD_UNSTABLE) if (NOT GTSAM_BUILD_UNSTABLE)
message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.") message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")

2
doc/Doxyfile.in
View File

@ -1188,7 +1188,7 @@ USE_MATHJAX = YES
# MathJax, but it is strongly recommended to install a local copy of MathJax # MathJax, but it is strongly recommended to install a local copy of MathJax
# before deployment. # before deployment.
MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest MATHJAX_RELPATH = https://cdn.mathjax.org/mathjax/latest
# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension # The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension
# names that should be enabled during MathJax rendering. # names that should be enabled during MathJax rendering.

2
gtsam/base/Matrix.h
View File

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

41
gtsam/base/TestableAssertions.h
View File

@ -23,6 +23,7 @@
#include <boost/optional.hpp> #include <boost/optional.hpp>
#include <map> #include <map>
#include <iostream> #include <iostream>
#include <sstream>
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
@ -349,4 +350,44 @@ bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) {
return false; return false;
} }
/**
* Capture std out via cout stream and compare against string.
*/
template<class V>
bool assert_stdout_equal(const std::string& expected, const V& actual) {
// Redirect output to buffer so we can compare
std::stringstream buffer;
// Save the original output stream so we can reset later
std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
// We test against actual std::cout for faithful reproduction
std::cout << actual;
// Get output string and reset stdout
std::string actual_ = buffer.str();
std::cout.rdbuf(old);
return assert_equal(expected, actual_);
}
/**
* Capture print function output and compare against string.
*/
template<class V>
bool assert_print_equal(const std::string& expected, const V& actual) {
// Redirect output to buffer so we can compare
std::stringstream buffer;
// Save the original output stream so we can reset later
std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
// We test against actual std::cout for faithful reproduction
actual.print();
// Get output string and reset stdout
std::string actual_ = buffer.str();
std::cout.rdbuf(old);
return assert_equal(expected, actual_);
}
} // \namespace gtsam } // \namespace gtsam

16
gtsam/base/Vector.cpp
View File

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

8
gtsam/base/Vector.h
View File

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

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

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

12
gtsam/geometry/Rot3M.cpp
View File

@ -176,7 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
// Create a fixed-size matrix // Create a fixed-size matrix
Matrix3 A = R.matrix(); Matrix3 A = R.matrix();
// Mathematica closed form optimization (procrastination?) gone wild:
// Check if (A+I) is invertible. Same as checking for -1 eigenvalue.
if ((A + I_3x3).determinant() == 0.0) {
throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation");
}
// Mathematica closed form optimization.
// The following are the essential computations for the following algorithm
// 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3
// 2. Compute the Cayley transform C = 2 * P^{-1} * (A-I)
// 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z.
const double a = A(0, 0), b = A(0, 1), c = A(0, 2); const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
const double d = A(1, 0), e = A(1, 1), f = A(1, 2); const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
const double g = A(2, 0), h = A(2, 1), i = A(2, 2); const double g = A(2, 0), h = A(2, 1), i = A(2, 2);

2
gtsam/geometry/SimpleCamera.cpp
View File

@ -21,6 +21,7 @@
namespace gtsam { namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
SimpleCamera simpleCamera(const Matrix34& P) { SimpleCamera simpleCamera(const Matrix34& P) {
// P = [A|a] = s K cRw [I|-T], with s the unknown scale // P = [A|a] = s K cRw [I|-T], with s the unknown scale
@ -45,5 +46,6 @@ namespace gtsam {
return SimpleCamera(Pose3(wRc, T), return SimpleCamera(Pose3(wRc, T),
Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
} }
#endif
} }

17
gtsam/geometry/SimpleCamera.h
View File

@ -19,14 +19,23 @@
#pragma once #pragma once
#include <gtsam/geometry/BearingRange.h> #include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
namespace gtsam { namespace gtsam {
/// A simple camera class with a Cal3_S2 calibration /// Convenient aliases for Pinhole camera classes with different calibrations.
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2; /// Also needed as forward declarations in the wrapper.
using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
//TODO Need to fix issue 621 for this to work with wrapper
// using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
// using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** /**
* @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x
* Use PinholeCameraCal3_S2 instead * Use PinholeCameraCal3_S2 instead
@ -140,4 +149,6 @@ struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
template <typename T> template <typename T>
struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {}; struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
#endif
} // namespace gtsam } // namespace gtsam

11
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -16,6 +16,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
@ -127,6 +128,16 @@ TEST(Cal3_S2, between) {
} }
/* ************************************************************************* */
TEST(Cal3_S2, Print) {
Cal3_S2 cal(5, 5, 5, 5, 5);
std::stringstream os;
os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
<< ", py:" << cal.py() << "}";
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

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

@ -18,6 +18,7 @@
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/base/testLie.h> #include <gtsam/base/testLie.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/assign/std/vector.hpp> // for operator += #include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
@ -906,9 +907,9 @@ TEST(Pose3 , ChartDerivatives) {
Pose3 id; Pose3 id;
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id,id); CHECK_CHART_DERIVATIVES(id,id);
// CHECK_CHART_DERIVATIVES(id,T2); CHECK_CHART_DERIVATIVES(id,T2);
// CHECK_CHART_DERIVATIVES(T2,id); CHECK_CHART_DERIVATIVES(T2,id);
// CHECK_CHART_DERIVATIVES(T2,T3); CHECK_CHART_DERIVATIVES(T2,T3);
} }
} }
@ -1028,32 +1029,13 @@ TEST(Pose3, Create) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, print) { TEST(Pose3, Print) {
std::stringstream redirectStream;
std::streambuf* ssbuf = redirectStream.rdbuf();
std::streambuf* oldbuf = std::cout.rdbuf();
// redirect cout to redirectStream
std::cout.rdbuf(ssbuf);
Pose3 pose(Rot3::identity(), Point3(1, 2, 3)); Pose3 pose(Rot3::identity(), Point3(1, 2, 3));
// output is captured to redirectStream
pose.print();
// Generate the expected output // Generate the expected output
std::stringstream expected; std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n";
Point3 translation(1, 2, 3);
// Add expected rotation EXPECT(assert_print_equal(expected, pose));
expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
expected << "t: 1 2 3\n";
// reset cout to the original stream
std::cout.rdbuf(oldbuf);
// Get substring corresponding to translation part
std::string actual = redirectStream.str();
CHECK_EQUAL(expected.str(), actual);
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -807,15 +807,15 @@ TEST(Rot3, RQ_derivative) {
test_xyz.push_back(VecAndErr{{0, 0, 0}, error}); test_xyz.push_back(VecAndErr{{0, 0, 0}, error});
test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error}); test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error});
test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error}); test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error});
test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, error}); test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, 1e-8});
test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error}); test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error});
test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error}); test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error});
test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error}); test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error});
test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error}); test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error});
// Test close to singularity // Test close to singularity
test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-8}); test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-7});
test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-8}); test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-7});
test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4}); test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4});
test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4}); test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4});

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

@ -26,6 +26,8 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
static const Cal3_S2 K(625, 625, 0, 0, 0); static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
@ -149,6 +151,8 @@ TEST( SimpleCamera, simpleCamera)
CHECK(assert_equal(expected, actual,1e-1)); CHECK(assert_equal(expected, actual,1e-1));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */

57
gtsam/gtsam.i
View File

@ -329,7 +329,7 @@ virtual class Value {
}; };
#include <gtsam/base/GenericValue.h> #include <gtsam/base/GenericValue.h>
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}> template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value { virtual class GenericValue : gtsam::Value {
void serializable() const; void serializable() const;
}; };
@ -1059,52 +1059,12 @@ class PinholeCamera {
void serialize() const; void serialize() const;
}; };
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
virtual class SimpleCamera {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
const gtsam::Point3& upVector);
// Testable
void print(string s) const;
bool equals(const gtsam::SimpleCamera& camera, double tol) const;
// Standard Interface
gtsam::Pose3 pose() const;
gtsam::Cal3_S2 calibration() const;
// Manifold
gtsam::SimpleCamera retract(Vector d) const;
Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
size_t dim() const;
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
double range(const gtsam::Point3& point);
double range(const gtsam::Pose3& pose);
// enabling serialization functionality
void serialize() const;
};
gtsam::SimpleCamera simpleCamera(const Matrix& P);
// Some typedefs for common camera types // Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above // PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2; typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified; //typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler; typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
@ -2069,7 +2029,7 @@ class NonlinearFactorGraph {
gtsam::KeySet keys() const; gtsam::KeySet keys() const;
gtsam::KeyVector keyVector() const; gtsam::KeyVector keyVector() const;
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}> template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
// NonlinearFactorGraph // NonlinearFactorGraph
@ -2493,7 +2453,7 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
Vector, Matrix}> Vector, Matrix}>
VALUE calculateEstimate(size_t key) const; VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const; gtsam::Values calculateBestEstimate() const;
@ -2527,12 +2487,11 @@ class NonlinearISAM {
//************************************************************************* //*************************************************************************
// Nonlinear factor types // Nonlinear factor types
//************************************************************************* //*************************************************************************
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/CalibratedCamera.h> #include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/StereoPoint2.h> #include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/nonlinear/PriorFactor.h> #include <gtsam/nonlinear/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}> template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
virtual class PriorFactor : gtsam::NoiseModelFactor { virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const; T prior() const;
@ -2556,7 +2515,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3_S2,
gtsam::imuBias::ConstantBias}> gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor { virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation // Constructor - forces exact evaluation
@ -2675,7 +2634,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
gtsam::Point2 measured() const; gtsam::Point2 measured() const;
}; };
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2; //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler; typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;

4
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -200,6 +200,10 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
currentValues = system.advance(prevValues, alpha, direction); currentValues = system.advance(prevValues, alpha, direction);
currentError = system.error(currentValues); currentError = system.error(currentValues);
// User hook:
if (params.iterationHook)
params.iterationHook(iteration, prevError, currentError);
// Maybe show output // Maybe show output
if (params.verbosity >= NonlinearOptimizerParams::ERROR) if (params.verbosity >= NonlinearOptimizerParams::ERROR)
std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl;

4
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -98,6 +98,10 @@ void NonlinearOptimizer::defaultOptimize() {
// Update newError for either printouts or conditional-end checks: // Update newError for either printouts or conditional-end checks:
newError = error(); newError = error();
// User hook:
if (params.iterationHook)
params.iterationHook(iterations(), currentError, newError);
// Maybe show output // Maybe show output
if (params.verbosity >= NonlinearOptimizerParams::VALUES) if (params.verbosity >= NonlinearOptimizerParams::VALUES)
values().print("newValues"); values().print("newValues");

2
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -81,7 +81,7 @@ protected:
public: public:
/** A shared pointer to this class */ /** A shared pointer to this class */
typedef boost::shared_ptr<const NonlinearOptimizer> shared_ptr; using shared_ptr = boost::shared_ptr<const NonlinearOptimizer>;
/// @name Standard interface /// @name Standard interface
/// @{ /// @{

60
gtsam/nonlinear/NonlinearOptimizerParams.h
View File

@ -38,21 +38,12 @@ public:
SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR
}; };
size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) size_t maxIterations = 100; ///< The maximum iterations to stop iterating (default 100)
double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) double relativeErrorTol = 1e-5; ///< The maximum relative error decrease to stop iterating (default 1e-5)
double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double absoluteErrorTol = 1e-5; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
double errorTol; ///< The maximum total error to stop iterating (default 0.0) double errorTol = 0.0; ///< The maximum total error to stop iterating (default 0.0)
Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT)
Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD)
NonlinearOptimizerParams() :
maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol(
0.0), verbosity(SILENT), orderingType(Ordering::COLAMD),
linearSolverType(MULTIFRONTAL_CHOLESKY) {}
virtual ~NonlinearOptimizerParams() {
}
virtual void print(const std::string& str = "") const;
size_t getMaxIterations() const { return maxIterations; } size_t getMaxIterations() const { return maxIterations; }
double getRelativeErrorTol() const { return relativeErrorTol; } double getRelativeErrorTol() const { return relativeErrorTol; }
@ -71,6 +62,37 @@ public:
static Verbosity verbosityTranslator(const std::string &s) ; static Verbosity verbosityTranslator(const std::string &s) ;
static std::string verbosityTranslator(Verbosity value) ; static std::string verbosityTranslator(Verbosity value) ;
/** Type for an optional user-provided hook to be called after each
* internal optimizer iteration. See iterationHook below. */
using IterationHook = std::function<
void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>;
/** Optional user-provided iteration hook to be called after each
* optimization iteration (Default: none).
* Note that `IterationHook` is defined as a std::function<> with this
* signature:
* \code
* void(size_t iteration, double errorBefore, double errorAfter)
* \endcode
* which allows binding by means of a reference to a regular function:
* \code
* void foo(size_t iteration, double errorBefore, double errorAfter);
* // ...
* lmOpts.iterationHook = &foo;
* \endcode
* or to a C++11 lambda (preferred if you need to capture additional
* context variables, such that the optimizer object itself, the factor graph,
* etc.):
* \code
* lmOpts.iterationHook = [&](size_t iter, double oldError, double newError)
* {
* // ...
* };
* \endcode
* or to the result of a properly-formed `std::bind` call.
*/
IterationHook iterationHook;
/** See NonlinearOptimizerParams::linearSolverType */ /** See NonlinearOptimizerParams::linearSolverType */
enum LinearSolverType { enum LinearSolverType {
MULTIFRONTAL_CHOLESKY, MULTIFRONTAL_CHOLESKY,
@ -81,10 +103,16 @@ public:
CHOLMOD, /* Experimental Flag */ CHOLMOD, /* Experimental Flag */
}; };
LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer
boost::optional<Ordering> ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) boost::optional<Ordering> ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty)
IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers.
NonlinearOptimizerParams() = default;
virtual ~NonlinearOptimizerParams() {
}
virtual void print(const std::string& str = "") const;
inline bool isMultifrontal() const { inline bool isMultifrontal() const {
return (linearSolverType == MULTIFRONTAL_CHOLESKY) return (linearSolverType == MULTIFRONTAL_CHOLESKY)
|| (linearSolverType == MULTIFRONTAL_QR); || (linearSolverType == MULTIFRONTAL_QR);

13
gtsam/nonlinear/tests/testFunctorizedFactor.cpp
View File

@ -19,6 +19,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/FunctorizedFactor.h> #include <gtsam/nonlinear/FunctorizedFactor.h>
#include <gtsam/nonlinear/factorTesting.h> #include <gtsam/nonlinear/factorTesting.h>
@ -115,16 +116,6 @@ TEST(FunctorizedFactor, Print) {
auto factor = auto factor =
MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier)); MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
// redirect output to buffer so we can compare
stringstream buffer;
streambuf *old = cout.rdbuf(buffer.rdbuf());
factor.print();
// get output string and reset stdout
string actual = buffer.str();
cout.rdbuf(old);
string expected = string expected =
" keys = { X0 }\n" " keys = { X0 }\n"
" noise model: unit (9) \n" " noise model: unit (9) \n"
@ -135,7 +126,7 @@ TEST(FunctorizedFactor, Print) {
"]\n" "]\n"
" noise model sigmas: 1 1 1 1 1 1 1 1 1\n"; " noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
CHECK_EQUAL(expected, actual); EXPECT(assert_print_equal(expected, factor));
} }
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/nonlinear/tests/testValues.cpp
View File

@ -595,15 +595,7 @@ TEST(Values, Demangle) {
values.insert(key1, v); values.insert(key1, v);
string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, 3>)\n[\n 5, 6, 7\n]\n\n"; string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, 3>)\n[\n 5, 6, 7\n]\n\n";
stringstream buffer; EXPECT(assert_print_equal(expected, values));
streambuf * old = cout.rdbuf(buffer.rdbuf());
values.print();
string actual = buffer.str();
cout.rdbuf(old);
EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/sfm/MFAS.cpp
View File

@ -121,8 +121,8 @@ MFAS::MFAS(const TranslationEdges& relativeTranslations,
} }
} }
vector<Key> MFAS::computeOrdering() const { KeyVector MFAS::computeOrdering() const {
vector<Key> ordering; // Nodes in MFAS order (result). KeyVector ordering; // Nodes in MFAS order (result).
// A graph is an unordered map from keys to nodes. Each node contains a list // A graph is an unordered map from keys to nodes. Each node contains a list
// of its adjacent nodes. Create the graph from the edgeWeights. // of its adjacent nodes. Create the graph from the edgeWeights.
@ -140,7 +140,7 @@ vector<Key> MFAS::computeOrdering() const {
map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const { map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const {
// Find the ordering. // Find the ordering.
vector<Key> ordering = computeOrdering(); KeyVector ordering = computeOrdering();
// Create a map from the node key to its position in the ordering. This makes // Create a map from the node key to its position in the ordering. This makes
// it easier to lookup positions of different nodes. // it easier to lookup positions of different nodes.

2
gtsam/sfm/MFAS.h
View File

@ -84,7 +84,7 @@ class MFAS {
* @brief Computes the 1D MFAS ordering of nodes in the graph * @brief Computes the 1D MFAS ordering of nodes in the graph
* @return orderedNodes: vector of nodes in the obtained order * @return orderedNodes: vector of nodes in the obtained order
*/ */
std::vector<Key> computeOrdering() const; KeyVector computeOrdering() const;
/** /**
* @brief Computes the outlier weights of the graph. We define the outlier * @brief Computes the outlier weights of the graph. We define the outlier

10
gtsam/sfm/tests/testMFAS.cpp
View File

@ -25,7 +25,7 @@ using namespace gtsam;
vector<MFAS::KeyPair> edges = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1), vector<MFAS::KeyPair> edges = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1),
make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)}; make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)};
// nodes in the graph // nodes in the graph
vector<Key> nodes = {Key(0), Key(1), Key(2), Key(3)}; KeyVector nodes = {Key(0), Key(1), Key(2), Key(3)};
// weights from projecting in direction-1 (bad direction, outlier accepted) // weights from projecting in direction-1 (bad direction, outlier accepted)
vector<double> weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75}; vector<double> weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75};
// weights from projecting in direction-2 (good direction, outlier rejected) // weights from projecting in direction-2 (good direction, outlier rejected)
@ -47,10 +47,10 @@ map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &edges,
TEST(MFAS, OrderingWeights2) { TEST(MFAS, OrderingWeights2) {
MFAS mfas_obj(getEdgeWeights(edges, weights2)); MFAS mfas_obj(getEdgeWeights(edges, weights2));
vector<Key> ordered_nodes = mfas_obj.computeOrdering(); KeyVector ordered_nodes = mfas_obj.computeOrdering();
// ground truth (expected) ordering in this example // ground truth (expected) ordering in this example
vector<Key> gt_ordered_nodes = {0, 1, 3, 2}; KeyVector gt_ordered_nodes = {0, 1, 3, 2};
// check if the expected ordering is obtained // check if the expected ordering is obtained
for (size_t i = 0; i < ordered_nodes.size(); i++) { for (size_t i = 0; i < ordered_nodes.size(); i++) {
@ -77,10 +77,10 @@ TEST(MFAS, OrderingWeights2) {
TEST(MFAS, OrderingWeights1) { TEST(MFAS, OrderingWeights1) {
MFAS mfas_obj(getEdgeWeights(edges, weights1)); MFAS mfas_obj(getEdgeWeights(edges, weights1));
vector<Key> ordered_nodes = mfas_obj.computeOrdering(); KeyVector ordered_nodes = mfas_obj.computeOrdering();
// "ground truth" expected ordering in this example // "ground truth" expected ordering in this example
vector<Key> gt_ordered_nodes = {3, 0, 1, 2}; KeyVector gt_ordered_nodes = {3, 0, 1, 2};
// check if the expected ordering is obtained // check if the expected ordering is obtained
for (size_t i = 0; i < ordered_nodes.size(); i++) { for (size_t i = 0; i < ordered_nodes.size(); i++) {

4
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -361,8 +361,12 @@ TEST(Similarity3, AlignPose3) {
vector<Pose3Pair> correspondences{bTa1, bTa2}; vector<Pose3Pair> correspondences{bTa1, bTa2};
// Cayley transform cannot accommodate 180 degree rotations,
// hence we only test for Expmap
#ifdef GTSAM_ROT3_EXPMAP
Similarity3 actual_aSb = Similarity3::Align(correspondences); Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb)); EXPECT(assert_equal(expected_aSb, actual_aSb));
#endif
} }
//****************************************************************************** //******************************************************************************

17
gtsam_unstable/linear/QPSParser.cpp
View File

@ -81,7 +81,7 @@ class QPSVisitor {
varname_to_key; // Variable QPS string name to key varname_to_key; // Variable QPS string name to key
std::unordered_map<Key, std::unordered_map<Key, Matrix11>> std::unordered_map<Key, std::unordered_map<Key, Matrix11>>
H; // H from hessian H; // H from hessian
double f; // Constant term of quadratic cost double f = 0; // Constant term of quadratic cost
std::string obj_name; // the objective function has a name in the QPS std::string obj_name; // the objective function has a name in the QPS
std::string name_; // the quadratic program has a name in the QPS std::string name_; // the quadratic program has a name in the QPS
std::unordered_map<Key, double> std::unordered_map<Key, double>
@ -175,10 +175,11 @@ class QPSVisitor {
string var_ = fromChars<1>(vars); string var_ = fromChars<1>(vars);
string row_ = fromChars<3>(vars); string row_ = fromChars<3>(vars);
double coefficient = at_c<5>(vars); double coefficient = at_c<5>(vars);
if (row_ == obj_name) if (row_ == obj_name) {
f = -coefficient; f = -coefficient;
else } else {
b[row_] = coefficient; b[row_] = coefficient;
}
if (debug) { if (debug) {
cout << "Added RHS for Var: " << var_ << " Row: " << row_ cout << "Added RHS for Var: " << var_ << " Row: " << row_
@ -194,15 +195,17 @@ class QPSVisitor {
string row2_ = fromChars<7>(vars); string row2_ = fromChars<7>(vars);
double coefficient1 = at_c<5>(vars); double coefficient1 = at_c<5>(vars);
double coefficient2 = at_c<9>(vars); double coefficient2 = at_c<9>(vars);
if (row1_ == obj_name) if (row1_ == obj_name) {
f = -coefficient1; f = -coefficient1;
else } else {
b[row1_] = coefficient1; b[row1_] = coefficient1;
}
if (row2_ == obj_name) if (row2_ == obj_name) {
f = -coefficient2; f = -coefficient2;
else } else {
b[row2_] = coefficient2; b[row2_] = coefficient2;
}
if (debug) { if (debug) {
cout << "Added RHS for Var: " << var_ << " Row: " << row1_ cout << "Added RHS for Var: " << var_ << " Row: " << row1_

27
gtsam_unstable/slam/serialization.cpp
View File

@ -43,7 +43,6 @@ typedef PriorFactor<Pose3> PriorFactorPose3;
typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2; typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2; typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera; typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2; typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera; typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
@ -68,7 +67,6 @@ typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2; typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2; typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera; typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef NonlinearEquality<PinholeCameraCal3_S2> NonlinearEqualityPinholeCameraCal3_S2; typedef NonlinearEquality<PinholeCameraCal3_S2> NonlinearEqualityPinholeCameraCal3_S2;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera; typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
@ -77,10 +75,8 @@ typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2; typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3; typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint; typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point; typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera; typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2; typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D; typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
@ -90,6 +86,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2; typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//TODO fix issue 621
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2; //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2; typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@ -129,7 +126,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
GTSAM_VALUE_EXPORT(gtsam::StereoCamera); GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
@ -150,7 +146,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3");
BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2");
BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera");
BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera");
BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector");
@ -174,7 +169,6 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3")
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D");
@ -182,9 +176,7 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint");
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
@ -192,12 +184,29 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
//TODO Fix issue 621
//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); //BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D");
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
#endif
/* ************************************************************************* */ /* ************************************************************************* */
// Actual implementations of functions // Actual implementations of functions
/* ************************************************************************* */ /* ************************************************************************* */

15
python/gtsam/tests/test_SimpleCamera.py
View File

@ -14,11 +14,12 @@ import unittest
import numpy as np import numpy as np
import gtsam import gtsam
from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2 as SimpleCamera
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
K = Cal3_S2(625, 625, 0, 0, 0) K = Cal3_S2(625, 625, 0, 0, 0)
class TestSimpleCamera(GtsamTestCase): class TestSimpleCamera(GtsamTestCase):
def test_constructor(self): def test_constructor(self):
@ -29,15 +30,15 @@ class TestSimpleCamera(GtsamTestCase):
def test_level2(self): def test_level2(self):
# Create a level camera, looking in Y-direction # Create a level camera, looking in Y-direction
pose2 = Pose2(0.4,0.3,math.pi/2.0) pose2 = Pose2(0.4, 0.3, math.pi/2.0)
camera = SimpleCamera.Level(K, pose2, 0.1) camera = SimpleCamera.Level(K, pose2, 0.1)
# expected # expected
x = Point3(1,0,0) x = Point3(1, 0, 0)
y = Point3(0,0,-1) y = Point3(0, 0, -1)
z = Point3(0,1,0) z = Point3(0, 1, 0)
wRc = Rot3(x,y,z) wRc = Rot3(x, y, z)
expected = Pose3(wRc,Point3(0.4,0.3,0.1)) expected = Pose3(wRc, Point3(0.4, 0.3, 0.1))
self.gtsamAssertEquals(camera.pose(), expected, 1e-9) self.gtsamAssertEquals(camera.pose(), expected, 1e-9)

52
tests/testNonlinearOptimizer.cpp
View File

@ -566,6 +566,58 @@ TEST( NonlinearOptimizer, logfile )
// EXPECT(actual.str()==expected.str()); // EXPECT(actual.str()==expected.str());
} }
/* ************************************************************************* */
TEST( NonlinearOptimizer, iterationHook_LM )
{
NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());
Point2 x0(3,3);
Values c0;
c0.insert(X(1), x0);
// Levenberg-Marquardt
LevenbergMarquardtParams lmParams;
size_t lastIterCalled = 0;
lmParams.iterationHook = [&](size_t iteration, double oldError, double newError)
{
// Tests:
lastIterCalled = iteration;
EXPECT(newError<oldError);
// Example of evolution printout:
//std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
};
LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();
EXPECT(lastIterCalled>5);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, iterationHook_CG )
{
NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());
Point2 x0(3,3);
Values c0;
c0.insert(X(1), x0);
// Levenberg-Marquardt
NonlinearConjugateGradientOptimizer::Parameters cgParams;
size_t lastIterCalled = 0;
cgParams.iterationHook = [&](size_t iteration, double oldError, double newError)
{
// Tests:
lastIterCalled = iteration;
EXPECT(newError<oldError);
// Example of evolution printout:
//std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
};
NonlinearConjugateGradientOptimizer(fg, c0, cgParams).optimize();
EXPECT(lastIterCalled>5);
}
/* ************************************************************************* */ /* ************************************************************************* */
//// Minimal traits example //// Minimal traits example
struct MyType : public Vector3 { struct MyType : public Vector3 {

29
tests/testSerializationSLAM.cpp
View File

@ -89,10 +89,8 @@ typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2; typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3; typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint; typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point; typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera; typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2; typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D; typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
@ -102,6 +100,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2; typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//TODO Fix issue 621 for this to work
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2; //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2; typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@ -145,7 +144,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
GTSAM_VALUE_EXPORT(gtsam::StereoCamera); GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
@ -190,9 +188,9 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point");
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2");
BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
@ -200,6 +198,7 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
//TODO fix issue 621
//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); //BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
@ -352,9 +351,9 @@ TEST (testSerializationSLAM, factors) {
RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1); RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1);
RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1); RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1);
RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1); RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1);
RangeFactorSimpleCameraPoint rangeFactorSimpleCameraPoint(a13, a05, 2.0, model1); RangeFactorPinholeCameraCal3_S2Point rangeFactorPinholeCameraCal3_S2Point(a13, a05, 2.0, model1);
RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1); RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1);
RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1); RangeFactorPinholeCameraCal3_S2 rangeFactorPinholeCameraCal3_S2(a13, b13, 2.0, model1);
BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2); BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2);
@ -405,9 +404,9 @@ TEST (testSerializationSLAM, factors) {
graph += rangeFactorPose2; graph += rangeFactorPose2;
graph += rangeFactorPose3; graph += rangeFactorPose3;
graph += rangeFactorCalibratedCameraPoint; graph += rangeFactorCalibratedCameraPoint;
graph += rangeFactorSimpleCameraPoint; graph += rangeFactorPinholeCameraCal3_S2Point;
graph += rangeFactorCalibratedCamera; graph += rangeFactorCalibratedCamera;
graph += rangeFactorSimpleCamera; graph += rangeFactorPinholeCameraCal3_S2;
graph += bearingRangeFactor2D; graph += bearingRangeFactor2D;
@ -463,9 +462,9 @@ TEST (testSerializationSLAM, factors) {
EXPECT(equalsObj<RangeFactorPose2>(rangeFactorPose2)); EXPECT(equalsObj<RangeFactorPose2>(rangeFactorPose2));
EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3)); EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3));
EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint)); EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
EXPECT(equalsObj<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint)); EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
EXPECT(equalsObj<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera)); EXPECT(equalsObj<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
EXPECT(equalsObj<RangeFactorSimpleCamera>(rangeFactorSimpleCamera)); EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
EXPECT(equalsObj<BearingRangeFactor2D>(bearingRangeFactor2D)); EXPECT(equalsObj<BearingRangeFactor2D>(bearingRangeFactor2D));
@ -521,9 +520,9 @@ TEST (testSerializationSLAM, factors) {
EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2)); EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2));
EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3)); EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3));
EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint)); EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
EXPECT(equalsXML<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint)); EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
EXPECT(equalsXML<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera)); EXPECT(equalsXML<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
EXPECT(equalsXML<RangeFactorSimpleCamera>(rangeFactorSimpleCamera)); EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
EXPECT(equalsXML<BearingRangeFactor2D>(bearingRangeFactor2D)); EXPECT(equalsXML<BearingRangeFactor2D>(bearingRangeFactor2D));
@ -579,9 +578,9 @@ TEST (testSerializationSLAM, factors) {
EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2)); EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2));
EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3)); EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3));
EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint)); EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
EXPECT(equalsBinary<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint)); EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
EXPECT(equalsBinary<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera)); EXPECT(equalsBinary<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
EXPECT(equalsBinary<RangeFactorSimpleCamera>(rangeFactorSimpleCamera)); EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
EXPECT(equalsBinary<BearingRangeFactor2D>(bearingRangeFactor2D)); EXPECT(equalsBinary<BearingRangeFactor2D>(bearingRangeFactor2D));