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

Merge branch 'develop' into feature/smartFactorsWithExtrinsicCalibration

release/4.3a0
lcarlone 2021-03-22 19:19:23 -04:00
parent 00eee7cd19 74e4fc392c
commit 4df78be0f0
87 changed files with 2995 additions and 1549 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -0,0 +1,18 @@
### Script to install Boost
BOOST_FOLDER=boost_${BOOST_VERSION//./_}
# Download Boost
wget https://dl.bintray.com/boostorg/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
# Unzip
tar -zxf ${BOOST_FOLDER}.tar.gz
# Bootstrap
cd ${BOOST_FOLDER}/
./bootstrap.sh
# Build and install
sudo ./b2 install
# Rebuild ld cache
sudo ldconfig

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

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

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

@ -12,6 +12,7 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
BOOST_VERSION: 1.67.0
strategy:
fail-fast: false
@ -44,7 +45,8 @@ jobs:
steps:
- name: Checkout
uses: actions/checkout@master
uses: actions/checkout@v2
- name: Install (Linux)
if: runner.os == 'Linux'
run: |
@ -61,11 +63,8 @@ jobs:
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
sudo apt-get install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
@ -75,10 +74,11 @@ jobs:
echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi
- name: Check Boost version
if: runner.os == 'Linux'
- name: Install Boost
run: |
echo "BOOST_ROOT = $BOOST_ROOT"
bash .github/scripts/boost.sh
- name: Build and Test (Linux)
if: runner.os == 'Linux'
run: |

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

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

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

@ -12,6 +12,7 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
PYTHON_VERSION: ${{ matrix.python_version }}
strategy:
fail-fast: false
matrix:
@ -22,10 +23,12 @@ jobs:
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1,
ubuntu-18.04-gcc-5-tbb,
# ubuntu-18.04-gcc-5-tbb,
]
build_type: [Debug, Release]
#TODO update wrapper to prevent OOM
# build_type: [Debug, Release]
build_type: [Release]
python_version: [3]
include:
- name: ubuntu-18.04-gcc-5
@ -43,20 +46,28 @@ jobs:
compiler: clang
version: "9"
#NOTE temporarily added this as it is a required check.
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
compiler: clang
version: "9"
build_type: Debug
python_version: "3"
- name: macOS-10.15-xcode-11.3.1
os: macOS-10.15
compiler: xcode
version: "11.3.1"
- name: ubuntu-18.04-gcc-5-tbb
os: ubuntu-18.04
compiler: gcc
version: "5"
flag: tbb
# - name: ubuntu-18.04-gcc-5-tbb
# os: ubuntu-18.04
# compiler: gcc
# version: "5"
# flag: tbb
steps:
- name: Checkout
uses: actions/checkout@master
uses: actions/checkout@v2
- name: Install (Linux)
if: runner.os == 'Linux'
run: |
@ -88,7 +99,7 @@ jobs:
run: |
brew tap ProfFan/robotics
brew install cmake ninja
brew install ProfFan/robotics/boost
brew install boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV

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

@ -12,6 +12,7 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
GTSAM_BUILD_UNSTABLE: ON
BOOST_VERSION: 1.67.0
strategy:
fail-fast: false
@ -56,7 +57,7 @@ jobs:
steps:
- name: Checkout
uses: actions/checkout@master
uses: actions/checkout@v2
- name: Install (Linux)
if: runner.os == 'Linux'
@ -69,10 +70,7 @@ jobs:
fi
sudo apt-get -y update
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@ -84,6 +82,11 @@ jobs:
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi
- name: Install Boost
if: runner.os == 'Linux'
run: |
bash .github/scripts/boost.sh
- name: Install (macOS)
if: runner.os == 'macOS'
run: |

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

@ -38,7 +38,7 @@ jobs:
steps:
- name: Checkout
uses: actions/checkout@master
uses: actions/checkout@v2
- name: Install (Windows)
if: runner.os == 'Windows'
shell: powershell
@ -64,12 +64,21 @@ jobs:
}
# Scoop modifies the PATH so we make the modified PATH global.
echo "$env:PATH" >> $GITHUB_PATH
- name: Download and install Boost
uses: MarkusJx/install-boost@v1.0.1
id: install-boost
with:
boost_version: 1.72.0
toolset: msvc14.2
- name: Build (Windows)
if: runner.os == 'Windows'
env:
BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }}
run: |
cmake -E remove_directory build
echo "BOOST_ROOT_1_72_0: ${env:BOOST_ROOT_1_72_0}"
cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT_1_72_0}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT_1_72_0}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT_1_72_0}\lib"
cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
cmake --build build --config ${{ matrix.build_type }} --target gtsam
cmake --build build --config ${{ matrix.build_type }} --target gtsam_unstable
cmake --build build --config ${{ matrix.build_type }} --target wrap

27
CMakeLists.txt
View File

@ -60,17 +60,6 @@ include(cmake/HandleGlobalBuildFlags.cmake) # Build flags
# Build CppUnitLite
add_subdirectory(CppUnitLite)
# This is the new wrapper
if(GTSAM_BUILD_PYTHON)
# Need to set this for the wrap package so we don't use the default value.
set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION}
CACHE STRING "The Python version to use for wrapping")
add_subdirectory(wrap)
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
add_subdirectory(python)
endif()
# Build GTSAM library
add_subdirectory(gtsam)
@ -88,8 +77,22 @@ if (GTSAM_BUILD_UNSTABLE)
add_subdirectory(gtsam_unstable)
endif()
# This is the new wrapper
if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
# Need to set this for the wrap package so we don't use the default value.
set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION}
CACHE STRING "The Python version to use for wrapping")
add_subdirectory(wrap)
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
endif()
# Python toolbox
if(GTSAM_BUILD_PYTHON)
add_subdirectory(python)
endif()
# Matlab toolbox
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
if(GTSAM_INSTALL_MATLAB_TOOLBOX)
add_subdirectory(matlab)
endif()

3
cmake/CMakeLists.txt
View File

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

437
cmake/GtsamMatlabWrap.cmake
View File

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

10
cmake/HandleUninstall.cmake
View File

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

26
cmake/README.md
View File

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

49
cmake/example_project/CMakeLists.txt
View File

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

32
cmake/example_project/README.md
View File

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

23
cmake/example_project/SayGoodbye.cpp
View File

@ -1,23 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SayGoodbye.cpp
* @brief Example script for example project
* @author Richard Roberts
*/
#include <example/PrintExamples.h>
int main(int argc, char *argv[]) {
example::PrintExamples().sayGoodbye();
return 0;
}

23
cmake/example_project/SayHello.cpp
View File

@ -1,23 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SayHello.cpp
* @brief Example script for example project
* @author Richard Roberts
*/
#include <example/PrintExamples.h>
int main(int argc, char *argv[]) {
example::PrintExamples().sayHello();
return 0;
}

31
cmake/example_project/example.h
View File

@ -1,31 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file example.h
* @brief Example wrapper interface file
* @author Richard Roberts
*/
// This is an interface file for automatic MATLAB wrapper generation. See
// gtsam.h for full documentation and more examples.
#include <example/PrintExamples.h>
namespace example {
class PrintExamples {
PrintExamples();
void sayHello() const;
void sayGoodbye() const;
};
}

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

@ -1,44 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file print_examples.cpp
* @brief Example library file
* @author Richard Roberts
*/
#include <iostream>
#include <example/PrintExamples.h>
namespace example {
void PrintExamples::sayHello() const {
std::cout << internal::getHelloString() << std::endl;
}
void PrintExamples::sayGoodbye() const {
std::cout << internal::getGoodbyeString() << std::endl;
}
namespace internal {
std::string getHelloString() {
return "Hello!";
}
std::string getGoodbyeString() {
return "See you soon!";
}
} // namespace internal
} // namespace example

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

@ -1,42 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file print_examples.h
* @brief Example library file
* @author Richard Roberts
*/
#pragma once
#include <iostream>
#include <string>
namespace example {
class PrintExamples {
public:
/// Print a greeting
void sayHello() const;
/// Print a farewell
void sayGoodbye() const;
};
namespace internal {
std::string getHelloString();
std::string getGoodbyeString();
} // namespace internal
} // namespace example

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

@ -1,43 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testExample.cpp
* @brief Unit tests for example
* @author Richard Roberts
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <example/PrintExamples.h>
using namespace gtsam;
TEST(Example, HelloString) {
const std::string expectedString = "Hello!";
EXPECT(assert_equal(expectedString, example::internal::getHelloString()));
}
TEST(Example, GoodbyeString) {
const std::string expectedString = "See you soon!";
EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString()));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

15
gtsam/CMakeLists.txt
View File

@ -204,18 +204,3 @@ else()
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
endif()
endif()
# Create the matlab toolbox for the gtsam library
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
# Set up codegen
include(GtsamMatlabWrap)
# Generate, build and install toolbox
set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
if(NOT BUILD_SHARED_LIBS)
list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
endif()
# Wrap
wrap_and_install_library(gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
endif ()

39
gtsam/geometry/OrientedPlane3.cpp
View File

@ -18,7 +18,7 @@
*/
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
using namespace std;
@ -28,12 +28,13 @@ namespace gtsam {
/* ************************************************************************* */
void OrientedPlane3::print(const string& s) const {
Vector4 coeffs = planeCoefficients();
cout << s << " : " << coeffs << endl;
cout << s << " : " << coeffs.transpose() << endl;
}
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp,
OptionalJacobian<3, 6> Hr) const {
OrientedPlane3 OrientedPlane3::transform(const Pose3& xr,
OptionalJacobian<3, 3> Hp,
OptionalJacobian<3, 6> Hr) const {
Matrix23 D_rotated_plane;
Matrix22 D_rotated_pose;
Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
@ -42,15 +43,14 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
double pred_d = n_.unitVector().dot(xr.translation()) + d_;
if (Hr) {
*Hr = Matrix::Zero(3,6);
Hr->setZero();
Hr->block<2, 3>(0, 0) = D_rotated_plane;
Hr->block<1, 3>(2, 3) = unit_vec;
}
if (Hp) {
Vector2 hpp = n_.basis().transpose() * xr.translation();
*Hp = Z_3x3;
Hp->setZero();
Hp->block<2, 2>(0, 0) = D_rotated_pose;
Hp->block<1, 2>(2, 0) = hpp;
Hp->block<1, 2>(2, 0) = n_.basis().transpose() * xr.translation();
(*Hp)(2, 2) = 1;
}
@ -58,25 +58,20 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
}
/* ************************************************************************* */
Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
Vector2 n_error = -n_.localCoordinates(plane.n_);
return Vector3(n_error(0), n_error(1), d_ - plane.d_);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1,
Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix22 H_n_error_this, H_n_error_other;
Vector2 n_error = n_.errorVector(other.normal(), H1 ? &H_n_error_this : 0,
Vector2 n_error = n_.errorVector(other.n_, H1 ? &H_n_error_this : 0,
H2 ? &H_n_error_other : 0);
double d_error = d_ - other.d_;
if (H1) {
*H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1;
*H1 << H_n_error_this, Z_2x1, 0, 0, 1;
}
if (H2) {
*H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1;
*H2 << H_n_error_other, Z_2x1, 0, 0, -1;
}
return Vector3(n_error(0), n_error(1), d_error);
@ -84,11 +79,11 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobia
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::retract(const Vector3& v,
OptionalJacobian<3,3> H) const {
OptionalJacobian<3, 3> H) const {
Matrix22 H_n;
Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
Unit3 n_retract(n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
if (H) {
*H << H_n, Vector2::Zero(), 0, 0, 1;
*H << H_n, Z_2x1, 0, 0, 1;
}
return OrientedPlane3(n_retract, d_ + v(2));
}
@ -100,4 +95,4 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
return Vector3(n_local(0), n_local(1), -d_local);
}
}
} // namespace gtsam

63
gtsam/geometry/OrientedPlane3.h
View File

@ -20,22 +20,21 @@
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>
#include <string>
namespace gtsam {
/**
* @brief Represents an infinite plane in 3D, which is composed of a planar normal and its
* perpendicular distance to the origin.
* Currently it provides a transform of the plane, and a norm 1 differencing of two planes.
* @brief Represents an infinite plane in 3D, which is composed of a planar
* normal and its perpendicular distance to the origin.
* Currently it provides a transform of the plane, and a norm 1 differencing of
* two planes.
* Refer to Trevor12iros for more math details.
*/
class GTSAM_EXPORT OrientedPlane3 {
private:
Unit3 n_; ///< The direction of the planar normal
double d_; ///< The perpendicular distance to this plane
@ -53,19 +52,17 @@ public:
}
/// Construct from a Unit3 and a distance
OrientedPlane3(const Unit3& s, double d) :
n_(s), d_(d) {
OrientedPlane3(const Unit3& n, double d) :
n_(n), d_(d) {
}
/// Construct from a vector of plane coefficients
OrientedPlane3(const Vector4& vec) :
n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
}
explicit OrientedPlane3(const Vector4& vec)
: n_(vec(0), vec(1), vec(2)), d_(vec(3)) {}
/// Construct from four numbers of plane coeffcients (a, b, c, d)
OrientedPlane3(double a, double b, double c, double d) {
Point3 p(a, b, c);
n_ = Unit3(p);
n_ = Unit3(a, b, c);
d_ = d;
}
@ -90,37 +87,18 @@ public:
* @return the transformed plane
*/
OrientedPlane3 transform(const Pose3& xr,
OptionalJacobian<3, 3> Hp = boost::none,
OptionalJacobian<3, 6> Hr = boost::none) const;
/**
* @deprecated the static method has wrong Jacobian order,
* please use the member method transform()
* @param The raw plane
* @param xr a transformation in current coordiante
* @param Hr optional jacobian wrpt the pose transformation
* @param Hp optional Jacobian wrpt the destination plane
* @return the transformed plane
*/
static OrientedPlane3 Transform(const OrientedPlane3& plane,
const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
OptionalJacobian<3, 3> Hp = boost::none) {
return plane.transform(xr, Hp, Hr);
}
/** Computes the error between two planes.
* The error is a norm 1 difference in tangent space.
* @param the other plane
*/
Vector3 error(const OrientedPlane3& plane) const;
OptionalJacobian<3, 3> Hp = boost::none,
OptionalJacobian<3, 6> Hr = boost::none) const;
/** Computes the error between the two planes, with derivatives.
* This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
* Unit3::localCoordinates. This one has correct derivatives.
* This uses Unit3::errorVector, as opposed to the other .error() in this
* class, which uses Unit3::localCoordinates. This one has correct
* derivatives.
* NOTE(hayk): The derivatives are zero when normals are exactly orthogonal.
* @param the other plane
* @param other the other plane
*/
Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
Vector3 errorVector(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// Dimensionality of tangent space = 3 DOF
@ -134,7 +112,8 @@ public:
}
/// The retract function
OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const;
OrientedPlane3 retract(const Vector3& v,
OptionalJacobian<3, 3> H = boost::none) const;
/// The local coordinates function
Vector3 localCoordinates(const OrientedPlane3& s) const;
@ -166,5 +145,5 @@ template<> struct traits<const OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
} // namespace gtsam
} // namespace gtsam

4
gtsam/geometry/Point3.h
View File

@ -35,9 +35,11 @@ namespace gtsam {
typedef Vector3 Point3;
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
using Point3Pair = std::pair<Point3, Point3>;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
using Point3Pairs = std::vector<Point3Pair>;
/// distance between two points
GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,

1
gtsam/geometry/Pose3.h
View File

@ -389,6 +389,7 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
// Convenience typedef
using Pose3Pair = std::pair<Pose3, Pose3>;
using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;

39
gtsam_unstable/geometry/Similarity3.cpp → gtsam/geometry/Similarity3.cpp
View File

@ -13,9 +13,10 @@
* @file Similarity3.cpp
* @brief Implementation of Similarity3 transform
* @author Paul Drews
* @author John Lambert
*/
#include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Manifold.h>
@ -24,13 +25,12 @@
namespace gtsam {
using std::vector;
using PointPairs = vector<Point3Pair>;
namespace {
/// Subtract centroids from point pairs.
static PointPairs subtractCentroids(const PointPairs &abPointPairs,
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
const Point3Pair &centroids) {
PointPairs d_abPointPairs;
Point3Pairs d_abPointPairs;
for (const Point3Pair& abPair : abPointPairs) {
Point3 da = abPair.first - centroids.first;
Point3 db = abPair.second - centroids.second;
@ -40,7 +40,7 @@ static PointPairs subtractCentroids(const PointPairs &abPointPairs,
}
/// Form inner products x and y and calculate scale.
static const double calculateScale(const PointPairs &d_abPointPairs,
static const double calculateScale(const Point3Pairs &d_abPointPairs,
const Rot3 &aRb) {
double x = 0, y = 0;
Point3 da, db;
@ -55,7 +55,7 @@ static const double calculateScale(const PointPairs &d_abPointPairs,
}
/// Form outer product H.
static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
static Matrix3 calculateH(const Point3Pairs &d_abPointPairs) {
Matrix3 H = Z_3x3;
for (const Point3Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose();
@ -63,17 +63,20 @@ static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
return H;
}
/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb,
/// This method estimates the similarity transform from differences point pairs,
// given a known or estimated rotation and point centroids.
static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
const Point3Pair &centroids) {
const double s = calculateScale(d_abPointPairs, aRb);
// dividing aTb by s is required because the registration cost function
// minimizes ||a - sRb - t||, whereas Sim(3) computes s(Rb + t)
const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
return Similarity3(aRb, aTb, s);
}
/// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
static Similarity3 alignGivenR(const PointPairs &abPointPairs,
static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
const Rot3 &aRb) {
auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
@ -154,7 +157,7 @@ Point3 Similarity3::operator*(const Point3& p) const {
return transformFrom(p);
}
Similarity3 Similarity3::Align(const PointPairs &abPointPairs) {
Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
// Refer to Chapter 3 of
// http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
if (abPointPairs.size() < 3)
@ -174,18 +177,20 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
// calculate rotation
vector<Rot3> rotations;
PointPairs abPointPairs;
Point3Pairs abPointPairs;
rotations.reserve(n);
abPointPairs.reserve(n);
Pose3 wTa, wTb;
// Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b"
Pose3 aTi, bTi;
for (const Pose3Pair &abPair : abPosePairs) {
std::tie(wTa, wTb) = abPair;
rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
abPointPairs.emplace_back(wTa.translation(), wTb.translation());
std::tie(aTi, bTi) = abPair;
const Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse());
rotations.emplace_back(aRb);
abPointPairs.emplace_back(aTi.translation(), bTi.translation());
}
const Rot3 aRb = FindKarcherMean<Rot3>(rotations);
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
return alignGivenR(abPointPairs, aRb);
return alignGivenR(abPointPairs, aRb_estimate);
}
Matrix4 Similarity3::wedge(const Vector7 &xi) {

58
gtsam_unstable/geometry/Similarity3.h → gtsam/geometry/Similarity3.h
View File

@ -13,6 +13,7 @@
* @file Similarity3.h
* @brief Implementation of Similarity3 transform
* @author Paul Drews
* @author John Lambert
*/
#pragma once
@ -22,7 +23,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam_unstable/dllexport.h>
#include <gtsam/dllexport.h>
namespace gtsam {
@ -52,54 +53,54 @@ public:
/// @{
/// Default constructor
GTSAM_UNSTABLE_EXPORT Similarity3();
GTSAM_EXPORT Similarity3();
/// Construct pure scaling
GTSAM_UNSTABLE_EXPORT Similarity3(double s);
GTSAM_EXPORT Similarity3(double s);
/// Construct from GTSAM types
GTSAM_UNSTABLE_EXPORT Similarity3(const Rot3& R, const Point3& t, double s);
GTSAM_EXPORT Similarity3(const Rot3& R, const Point3& t, double s);
/// Construct from Eigen types
GTSAM_UNSTABLE_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s);
GTSAM_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s);
/// Construct from matrix [R t; 0 s^-1]
GTSAM_UNSTABLE_EXPORT Similarity3(const Matrix4& T);
GTSAM_EXPORT Similarity3(const Matrix4& T);
/// @}
/// @name Testable
/// @{
/// Compare with tolerance
GTSAM_UNSTABLE_EXPORT bool equals(const Similarity3& sim, double tol) const;
GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const;
/// Exact equality
GTSAM_UNSTABLE_EXPORT bool operator==(const Similarity3& other) const;
GTSAM_EXPORT bool operator==(const Similarity3& other) const;
/// Print with optional string
GTSAM_UNSTABLE_EXPORT void print(const std::string& s) const;
GTSAM_EXPORT void print(const std::string& s) const;
GTSAM_UNSTABLE_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p);
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p);
/// @}
/// @name Group
/// @{
/// Return an identity transform
GTSAM_UNSTABLE_EXPORT static Similarity3 identity();
GTSAM_EXPORT static Similarity3 identity();
/// Composition
GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& S) const;
GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const;
/// Return the inverse
GTSAM_UNSTABLE_EXPORT Similarity3 inverse() const;
GTSAM_EXPORT Similarity3 inverse() const;
/// @}
/// @name Group action on Point3
/// @{
/// Action on a point p is s*(R*p+t)
GTSAM_UNSTABLE_EXPORT Point3 transformFrom(const Point3& p, //
GTSAM_EXPORT Point3 transformFrom(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
@ -114,20 +115,27 @@ public:
* This group action satisfies the compatibility condition.
* For more details, refer to: https://en.wikipedia.org/wiki/Group_action
*/
GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const;
GTSAM_EXPORT Pose3 transformFrom(const Pose3& T) const;
/** syntactic sugar for transformFrom */
GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const;
GTSAM_EXPORT Point3 operator*(const Point3& p) const;
/**
* Create Similarity3 by aligning at least three point pairs
*/
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
GTSAM_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
/**
* Create Similarity3 by aligning at least two pose pairs
* Create the Similarity3 object that aligns at least two pose pairs.
* Each pair is of the form (aTi, bTi).
* Given a list of pairs in frame a, and a list of pairs in frame b, Align()
* will compute the best-fit Similarity3 aSb transformation to align them.
* First, the rotation aRb will be computed as the average (Karcher mean) of
* many estimates aRb (from each pair). Afterwards, the scale factor will be computed
* using the algorithm described here:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
*/
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
GTSAM_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
/// @}
/// @name Lie Group
@ -136,12 +144,12 @@ public:
/** Log map at the identity
* \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
*/
GTSAM_UNSTABLE_EXPORT static Vector7 Logmap(const Similarity3& s, //
GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, //
OptionalJacobian<7, 7> Hm = boost::none);
/** Exponential map at the identity
*/
GTSAM_UNSTABLE_EXPORT static Similarity3 Expmap(const Vector7& v, //
GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, //
OptionalJacobian<7, 7> Hm = boost::none);
/// Chart at the origin
@ -162,17 +170,17 @@ public:
* @return 4*4 element of Lie algebra that can be exponentiated
* TODO(frank): rename to Hat, make part of traits
*/
GTSAM_UNSTABLE_EXPORT static Matrix4 wedge(const Vector7& xi);
GTSAM_EXPORT static Matrix4 wedge(const Vector7& xi);
/// Project from one tangent space to another
GTSAM_UNSTABLE_EXPORT Matrix7 AdjointMap() const;
GTSAM_EXPORT Matrix7 AdjointMap() const;
/// @}
/// @name Standard interface
/// @{
/// Calculate 4*4 matrix group equivalent
GTSAM_UNSTABLE_EXPORT const Matrix4 matrix() const;
GTSAM_EXPORT const Matrix4 matrix() const;
/// Return a GTSAM rotation
const Rot3& rotation() const {
@ -191,7 +199,7 @@ public:
/// Convert to a rigid body pose (R, s*t)
/// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
GTSAM_UNSTABLE_EXPORT operator Pose3() const;
GTSAM_EXPORT operator Pose3() const;
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
inline static size_t Dim() {

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

@ -31,7 +31,7 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
//*******************************************************************************
TEST (OrientedPlane3, getMethods) {
TEST(OrientedPlane3, getMethods) {
Vector4 c;
c << -1, 0, 0, 5;
OrientedPlane3 plane1(c);
@ -50,44 +50,27 @@ TEST (OrientedPlane3, getMethods) {
//*******************************************************************************
OrientedPlane3 Transform_(const OrientedPlane3& plane, const Pose3& xr) {
return OrientedPlane3::Transform(plane, xr);
}
OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) {
return plane.transform(xr);
}
TEST (OrientedPlane3, transform) {
TEST(OrientedPlane3, transform) {
gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0),
gtsam::Point3(2.0, 3.0, 4.0));
gtsam::Point3(2.0, 3.0, 4.0));
OrientedPlane3 plane(-1, 0, 0, 5);
OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose,
none, none);
OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none);
EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-5));
EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-5));
OrientedPlane3 transformedPlane = plane.transform(pose, none, none);
EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5));
// Test the jacobians of transform
Matrix actualH1, expectedH1, actualH2, expectedH2;
{
// because the Transform class uses a wrong order of Jacobians in interface
OrientedPlane3::Transform(plane, pose, actualH1, none);
expectedH1 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
OrientedPlane3::Transform(plane, pose, none, actualH2);
expectedH2 = numericalDerivative21(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
}
{
plane.transform(pose, actualH1, none);
expectedH1 = numericalDerivative21(transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
plane.transform(pose, none, actualH2);
expectedH2 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
}
expectedH1 = numericalDerivative21(transform_, plane, pose);
plane.transform(pose, actualH1, none);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
expectedH2 = numericalDerivative22(transform_, plane, pose);
plane.transform(pose, none, actualH2);
EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
}
//*******************************************************************************
@ -109,7 +92,6 @@ inline static Vector randomVector(const Vector& minLimits,
//*******************************************************************************
TEST(OrientedPlane3, localCoordinates_retract) {
size_t numIterations = 10000;
Vector4 minPlaneLimit, maxPlaneLimit;
minPlaneLimit << -1.0, -1.0, -1.0, 0.01;
@ -119,7 +101,6 @@ TEST(OrientedPlane3, localCoordinates_retract) {
minXiLimit << -M_PI, -M_PI, -10.0;
maxXiLimit << M_PI, M_PI, 10.0;
for (size_t i = 0; i < numIterations; i++) {
// Create a Plane
OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit));
Vector v12 = randomVector(minXiLimit, maxXiLimit);
@ -138,22 +119,24 @@ TEST(OrientedPlane3, localCoordinates_retract) {
}
//*******************************************************************************
TEST (OrientedPlane3, error2) {
TEST(OrientedPlane3, errorVector) {
OrientedPlane3 plane1(-1, 0.1, 0.2, 5);
OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4);
// Hard-coded regression values, to ensure the result doesn't change.
EXPECT(assert_equal((Vector) Z_3x1, plane1.errorVector(plane1), 1e-8));
EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5));
EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4),
plane1.errorVector(plane2), 1e-5));
// Test the jacobians of transform
Matrix33 actualH1, expectedH1, actualH2, expectedH2;
Vector3 actual = plane1.errorVector(plane2, actualH1, actualH2);
EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), Vector2(actual[0], actual[1])));
EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()),
Vector2(actual[0], actual[1])));
EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f = //
boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2);
@ -162,19 +145,19 @@ TEST (OrientedPlane3, error2) {
}
//*******************************************************************************
TEST (OrientedPlane3, jacobian_retract) {
TEST(OrientedPlane3, jacobian_retract) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
Matrix33 H_actual;
boost::function<OrientedPlane3(const Vector3&)> f =
boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
{
Vector3 v (-0.1, 0.2, 0.3);
Vector3 v(-0.1, 0.2, 0.3);
plane.retract(v, H_actual);
Matrix H_expected_numerical = numericalDerivative11(f, v);
EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));
}
{
Vector3 v (0, 0, 0);
Vector3 v(0, 0, 0);
plane.retract(v, H_actual);
Matrix H_expected_numerical = numericalDerivative11(f, v);
EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));

54
gtsam_unstable/geometry/tests/testSimilarity3.cpp → gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -16,7 +16,7 @@
* @author Zhaoyang Lv
*/
#include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
@ -259,19 +259,27 @@ TEST(Similarity3, GroupAction) {
//******************************************************************************
// Group action on Pose3
// Estimate Sim(3) object "aSb" from pose pairs {(aTi, bTi)}
// In the example below, let the "a" frame be the "world" frame below,
// and let the "b" frame be the "egovehicle" frame.
// Suppose within the egovehicle frame, we know the poses of two objects "o1" and "o2"
TEST(Similarity3, GroupActionPose3) {
Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Suppose we know the pose of the egovehicle in the world frame
Similarity3 wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
// Suppose they are 3d cuboids detected by onboard sensor, in the egovehicle frame
Pose3 eTo1(Rot3(), Point3(0, 0, 0));
Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create destination poses
Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
// Create destination poses (two objects now living in the world/city "w" frame)
// Desired to place the objects into the world frame for tracking
Pose3 expected_wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 expected_wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1)));
EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2)));
// objects now live in the world frame, instead of in the egovehicle frame
EXPECT(assert_equal(expected_wTo1, wSe.transformFrom(eTo1)));
EXPECT(assert_equal(expected_wTo2, wSe.transformFrom(eTo2)));
}
// Test left group action compatibility.
@ -346,26 +354,28 @@ TEST(Similarity3, AlignPoint3_3) {
//******************************************************************************
// Align with Pose3 Pairs
TEST(Similarity3, AlignPose3) {
Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
Similarity3 expected_wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
// Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
Pose3 eTo1(Rot3(), Point3(0, 0, 0));
Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create destination poses
Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
// Create destination poses (same two objects, but instead living in the world/city "w" frame)
Pose3 wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
Pose3Pair bTa1(make_pair(Tb1, Ta1));
Pose3Pair bTa2(make_pair(Tb2, Ta2));
Pose3Pair wTe1(make_pair(wTo1, eTo1));
Pose3Pair wTe2(make_pair(wTo2, eTo2));
vector<Pose3Pair> correspondences{bTa1, bTa2};
vector<Pose3Pair> correspondences{wTe1, wTe2};
// Cayley transform cannot accommodate 180 degree rotations,
// hence we only test for Expmap
#ifdef GTSAM_ROT3_EXPMAP
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
// Recover the transformation wSe (i.e. world_S_egovehicle)
Similarity3 actual_wSe = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_wSe, actual_wSe));
#endif
}

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

@ -501,6 +501,7 @@ TEST(actualH, Serialization) {
EXPECT(serializationTestHelpers::equalsBinary(p));
}
/* ************************************************************************* */
int main() {
srand(time(nullptr));

260
gtsam/gtsam.i
View File

@ -5,117 +5,7 @@
* These are the current classes available through the matlab and python wrappers,
* add more functions/classes as they are available.
*
* IMPORTANT: the python wrapper supports keyword arguments for functions/methods. Hence, the
* argument names matter. An implementation restriction is that in overloaded methods
* or functions, arguments of different types *have* to have different names.
*
* Requirements:
* Classes must start with an uppercase letter
* - Can wrap a typedef
* Only one Method/Constructor per line, though methods/constructors can extend across multiple lines
* Methods can return
* - Eigen types: Matrix, Vector
* - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char
* - void
* - Any class with which be copied with boost::make_shared()
* - boost::shared_ptr of any object type
* Constructors
* - Overloads are supported, but arguments of different types *have* to have different names
* - A class with no constructors can be returned from other functions but not allocated directly in MATLAB
* Methods
* - Constness has no effect
* - Specify by-value (not reference) return types, even if C++ method returns reference
* - Must start with a letter (upper or lowercase)
* - Overloads are supported
* Static methods
* - Must start with a letter (upper or lowercase) and use the "static" keyword
* - The first letter will be made uppercase in the generated MATLAB interface
* - Overloads are supported, but arguments of different types *have* to have different names
* Arguments to functions any of
* - Eigen types: Matrix, Vector
* - Eigen types and classes as an optionally const reference
* - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char
* - Any class with which be copied with boost::make_shared() (except Eigen)
* - boost::shared_ptr of any object type (except Eigen)
* Comments can use either C++ or C style, with multiple lines
* Namespace definitions
* - Names of namespaces must start with a lowercase letter
* - start a namespace with "namespace {"
* - end a namespace with exactly "}"
* - Namespaces can be nested
* Namespace usage
* - Namespaces can be specified for classes in arguments and return values
* - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName"
* Includes in C++ wrappers
* - All includes will be collected and added in a single file
* - All namespaces must have angle brackets: <path>
* - No default includes will be added
* Global/Namespace functions
* - Functions specified outside of a class are global
* - Can be overloaded with different arguments
* - Can have multiple functions of the same name in different namespaces
* Using classes defined in other modules
* - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error
* Virtual inheritance
* - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace
* - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {"
* - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and
* also "virtual class ns::Derived;"
* - Pure virtual (abstract) classes should list no constructors in this interface file
* - Virtual classes must have a clone() function in C++ (though it does not have to be included
* in the MATLAB interface). clone() will be called whenever an object copy is needed, instead
* of using the copy constructor (which is used for non-virtual objects).
* - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree
* virtual boost::shared_ptr<CLASS_NAME> clone() const;
* Class Templates
* - Basic templates are supported either with an explicit list of types to instantiate,
* e.g. template<T = {gtsam::Pose2, gtsam::Rot2, gtsam::Point3}> class Class1 { ... };
* or with typedefs, e.g.
* template<T, U> class Class2 { ... };
* typedef Class2<Type1, Type2> MyInstantiatedClass;
* - In the class definition, appearances of the template argument(s) will be replaced with their
* instantiated types, e.g. 'void setValue(const T& value);'.
* - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();'
* - To create new instantiations in other modules, you must copy-and-paste the whole class definition
* into the new module, but use only your new instantiation types.
* - When forward-declaring template instantiations, use the generated/typedefed name, e.g.
* class gtsam::Class1Pose2;
* class gtsam::MyInstantiatedClass;
* Boost.serialization within Matlab:
* - you need to mark classes as being serializable in the markup file (see this file for an example).
* - There are two options currently, depending on the class. To "mark" a class as serializable,
* add a function with a particular signature so that wrap will catch it.
* - Add "void serialize()" to a class to create serialization functions for a class.
* Adding this flag subsumes the serializable() flag below. Requirements:
* - A default constructor must be publicly accessible
* - Must not be an abstract base class
* - The class must have an actual boost.serialization serialize() function.
* - Add "void serializable()" to a class if you only want the class to be serialized as a
* part of a container (such as noisemodel). This version does not require a publicly
* accessible default constructor.
* Forward declarations and class definitions for Pybind:
* - Need to specify the base class (both this forward class and base class are declared in an external Pybind header)
* This is so Pybind can generate proper inheritance.
* Example when wrapping a gtsam-based project:
* // forward declarations
* virtual class gtsam::NonlinearFactor
* virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor
* // class definition
* #include <MyFactor.h>
* virtual class MyFactor : gtsam::NoiseModelFactor {...};
* - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class
* - This will cause an ambiguity problem in Pybind header file
*/
/**
* Status:
* - TODO: default values for arguments
* - WORKAROUND: make multiple versions of the same function for different configurations of default arguments
* - TODO: Handle gtsam::Rot3M conversions to quaternions
* - TODO: Parse return of const ref arguments
* - TODO: Parse std::string variants and convert directly to special string
* - TODO: Add enum support
* - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load
* Please refer to the wrapping docs: https://github.com/borglab/wrap/blob/master/README.md
*/
namespace gtsam {
@ -144,6 +34,9 @@ class KeyList {
void remove(size_t key);
void serialize() const;
// enable pickling in python
void pickle() const;
};
// Actually a FastSet<Key>
@ -169,6 +62,9 @@ class KeySet {
bool count(size_t key) const; // returns true if value exists
void serialize() const;
// enable pickling in python
void pickle() const;
};
// Actually a vector<Key>
@ -190,6 +86,9 @@ class KeyVector {
void push_back(size_t key) const;
void serialize() const;
// enable pickling in python
void pickle() const;
};
// Actually a FastMap<Key,int>
@ -361,6 +260,9 @@ class Point2 {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
// std::vector<gtsam::Point2>
@ -422,6 +324,9 @@ class StereoPoint2 {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Point3.h>
@ -446,6 +351,18 @@ class Point3 {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Point3.h>
class Point3Pairs {
Point3Pairs();
size_t size() const;
bool empty() const;
gtsam::Point3Pair at(size_t n) const;
void push_back(const gtsam::Point3Pair& point_pair);
};
#include <gtsam/geometry/Rot2.h>
@ -491,6 +408,9 @@ class Rot2 {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/SO3.h>
@ -653,6 +573,9 @@ class Rot3 {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Pose2.h>
@ -708,6 +631,9 @@ class Pose2 {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Pose3.h>
@ -764,6 +690,18 @@ class Pose3 {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Pose3.h>
class Pose3Pairs {
Pose3Pairs();
size_t size() const;
bool empty() const;
gtsam::Pose3Pair at(size_t n) const;
void push_back(const gtsam::Pose3Pair& pose_pair);
};
// std::vector<gtsam::Pose3>
@ -797,6 +735,15 @@ class Unit3 {
size_t dim() const;
gtsam::Unit3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Unit3& s) const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
// enabling function to compare objects
bool equals(const gtsam::Unit3& expected, double tol) const;
};
#include <gtsam/geometry/EssentialMatrix.h>
@ -856,6 +803,9 @@ class Cal3_S2 {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Cal3DS2_Base.h>
@ -884,6 +834,9 @@ virtual class Cal3DS2_Base {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Cal3DS2.h>
@ -905,6 +858,9 @@ virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Cal3Unified.h>
@ -931,6 +887,9 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Cal3_S2Stereo.h>
@ -988,6 +947,9 @@ class Cal3Bundler {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/CalibratedCamera.h>
@ -1018,6 +980,9 @@ class CalibratedCamera {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/PinholeCamera.h>
@ -1056,8 +1021,34 @@ class PinholeCamera {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Similarity3.h>
class Similarity3 {
// Standard Constructors
Similarity3();
Similarity3(double s);
Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s);
Similarity3(const Matrix& R, const Vector& t, double s);
Similarity3(const Matrix& T);
gtsam::Pose3 transformFrom(const gtsam::Pose3& T);
static gtsam::Similarity3 Align(const gtsam::Point3Pairs & abPointPairs);
static gtsam::Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs);
// Standard Interface
const Matrix matrix() const;
const gtsam::Rot3& rotation();
const gtsam::Point3& translation();
double scale() const;
};
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h>
// Some typedefs for common camera types
@ -1103,6 +1094,9 @@ class StereoCamera {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/triangulation.h>
@ -1557,6 +1551,9 @@ class VectorValues {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/linear/GaussianFactor.h>
@ -1618,6 +1615,9 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/linear/HessianFactor.h>
@ -1649,6 +1649,9 @@ virtual class HessianFactor : gtsam::GaussianFactor {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/linear/GaussianFactorGraph.h>
@ -1728,10 +1731,13 @@ class GaussianFactorGraph {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/linear/GaussianConditional.h>
virtual class GaussianConditional : gtsam::GaussianFactor {
virtual class GaussianConditional : gtsam::JacobianFactor {
//Constructors
GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
@ -1791,6 +1797,8 @@ virtual class GaussianBayesNet {
gtsam::KeySet keys() const;
bool exists(size_t idx) const;
void saveGraph(const string& s) const;
gtsam::GaussianConditional* front() const;
gtsam::GaussianConditional* back() const;
void push_back(gtsam::GaussianConditional* conditional);
@ -2033,6 +2041,9 @@ class Ordering {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -2071,6 +2082,10 @@ class NonlinearFactorGraph {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
void saveGraph(const string& s) const;
};
@ -2128,6 +2143,9 @@ class Values {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
// New in 4.0, we have to specialize every insert/update/at to generate wrappers
// Instead of the old:
// void insert(size_t j, const gtsam::Value& value);
@ -2527,6 +2545,9 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
@ -2538,6 +2559,9 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/nonlinear/NonlinearEquality.h>
@ -2623,7 +2647,7 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
const BEARING& measuredBearing, const RANGE& measuredRange,
const gtsam::noiseModel::Base* noiseModel);
BearingRange<POSE, POINT, BEARING, RANGE> measured() const;
gtsam::BearingRange<POSE, POINT, BEARING, RANGE> measured() const;
// enabling serialization functionality
void serialize() const;
@ -2763,8 +2787,8 @@ class SfmTrack {
double r;
double g;
double b;
// TODO Need to close wrap#10 to allow this to work.
// std::vector<pair<size_t, gtsam::Point2>> measurements;
std::vector<pair<size_t, gtsam::Point2>> measurements;
size_t number_measurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
@ -2774,6 +2798,9 @@ class SfmTrack {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
// enabling function to compare objects
bool equals(const gtsam::SfmTrack& expected, double tol) const;
};
@ -2790,6 +2817,9 @@ class SfmData {
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
// enabling function to compare objects
bool equals(const gtsam::SfmData& expected, double tol) const;
};

25
gtsam/linear/GaussianBayesNet.cpp
View File

@ -12,15 +12,16 @@
/**
* @file GaussianBayesNet.cpp
* @brief Chordal Bayes Net, the result of eliminating a factor graph
* @author Frank Dellaert
* @author Frank Dellaert, Varun Agrawal
*/
#include <gtsam/base/timing.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/base/timing.h>
#include <boost/range/adaptor/reversed.hpp>
#include <fstream>
using namespace std;
using namespace gtsam;
@ -204,5 +205,23 @@ namespace gtsam {
}
/* ************************************************************************* */
void GaussianBayesNet::saveGraph(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::ofstream of(s.c_str());
of << "digraph G{\n";
for (auto conditional : boost::adaptors::reverse(*this)) {
typename GaussianConditional::Frontals frontals = conditional->frontals();
Key me = frontals.front();
typename GaussianConditional::Parents parents = conditional->parents();
for (Key p : parents)
of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
}
of << "}";
of.close();
}
/* ************************************************************************* */
} // namespace gtsam

10
gtsam/linear/GaussianBayesNet.h
View File

@ -177,6 +177,16 @@ namespace gtsam {
*/
VectorValues backSubstituteTranspose(const VectorValues& gx) const;
/**
* @brief Save the GaussianBayesNet as an image. Requires `dot` to be
* installed.
*
* @param s The name of the figure.
* @param keyFormatter Formatter to use for styling keys in the graph.
*/
void saveGraph(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// @}
private:

1
gtsam/linear/LossFunctions.h
View File

@ -28,6 +28,7 @@
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/singleton.hpp>
#include <boost/serialization/version.hpp>
namespace gtsam {
namespace noiseModel {

1
gtsam/linear/SubgraphBuilder.h
View File

@ -23,6 +23,7 @@
#include <boost/serialization/nvp.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/version.hpp>
#include <vector>

68
gtsam/navigation/ConstantVelocityFactor.h Normal file
View File

@ -0,0 +1,68 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ConstantVelocityFactor.h
* @brief Maintain a constant velocity motion model between two NavStates
* @author Asa Hammond
*/
#include <gtsam/navigation/NavState.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Binary factor for applying a constant velocity model to a moving body represented as a NavState.
* The only measurement is dt, the time delta between the states.
*/
class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
double dt_;
public:
using Base = NoiseModelFactor2<NavState, NavState>;
public:
ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
: NoiseModelFactor2<NavState, NavState>(model, i, j), dt_(dt) {}
~ConstantVelocityFactor() override{};
/**
* @brief Caclulate error: (x2 - x1.update(dt)))
* where X1 and X1 are NavStates and dt is
* the time difference in seconds between the states.
* @param x1 NavState for key a
* @param x2 NavState for key b
* @param H1 optional jacobian in x1
* @param H2 optional jacobian in x2
* @return * Vector
*/
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
boost::optional<gtsam::Matrix &> H1 = boost::none,
boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
// only used to use update() below
static const Vector3 b_accel{0.0, 0.0, 0.0};
static const Vector3 b_omega{0.0, 0.0, 0.0};
Matrix99 predicted_H_x1;
NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {});
Matrix99 error_H_predicted;
Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2);
if (H1) {
*H1 = error_H_predicted * predicted_H_x1;
}
return error;
}
};
} // namespace gtsam

6
gtsam/navigation/NavState.cpp
View File

@ -136,12 +136,12 @@ Vector9 NavState::localCoordinates(const NavState& g, //
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
Matrix3 D_dR_R, D_dt_R, D_dv_R;
const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
const Point3 dt = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
Vector9 xi;
Matrix3 D_xi_R;
xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv;
xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
if (H1) {
*H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
D_dt_R, -I_3x3, Z_3x3, //

81
gtsam/navigation/tests/testConstantVelocityFactor.cpp Normal file
View File

@ -0,0 +1,81 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testConstantVelocityFactor.cpp
* @brief Unit test for ConstantVelocityFactor
* @author Asa Hammond
*/
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ConstantVelocityFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <list>
/* ************************************************************************* */
TEST(ConstantVelocityFactor, VelocityFactor) {
using namespace gtsam;
const double tol{1e-5};
const Key x1 = Key{1};
const Key x2 = Key{2};
const double dt{1.0};
// moving upward with groundtruth velocity"
const auto origin = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 0.0}}, Velocity3{0.0, 0.0, 0.0}};
const auto state0 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 0.0}}, Velocity3{0.0, 0.0, 1.0}};
const auto state1 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 1.0}}, Velocity3{0.0, 0.0, 1.0}};
const auto state2 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}};
const auto state3 = NavState{Pose3{Rot3::Yaw(M_PI_2), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}};
const double mu{1000};
const auto noise_model = noiseModel::Constrained::All(9, mu);
const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model);
// positions are the same, secondary state has velocity 1.0 in z,
const auto state0_err_origin = factor.evaluateError(origin, state0);
EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished(), state0_err_origin, tol));
// same velocities, different position
// second state agrees with initial state + velocity * dt
const auto state1_err_state0 = factor.evaluateError(state0, state1);
EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(), state1_err_state0, tol));
// same velocities, same position, different rotations
// second state agrees with initial state + velocity * dt
// as we assume that omega is 0.0 this is the same as the above case
// TODO: this should respect omega and actually fail in this case
const auto state3_err_state2 = factor.evaluateError(state0, state1);
EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(), state3_err_state2, tol));
// both bodies have the same velocity,
// but state2.pose() does not agree with state0.update()
// error comes from this position difference
const auto state2_err_state0 = factor.evaluateError(state0, state2);
EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0).finished(), state2_err_state0, tol));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
gtsam/nonlinear/LevenbergMarquardtParams.cpp
View File

@ -37,6 +37,8 @@ LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTrans
return LevenbergMarquardtParams::SILENT;
if (s == "SUMMARY")
return LevenbergMarquardtParams::SUMMARY;
if (s == "TERMINATION")
return LevenbergMarquardtParams::TERMINATION;
if (s == "LAMBDA")
return LevenbergMarquardtParams::LAMBDA;
if (s == "TRYLAMBDA")

10
gtsam/sfm/TranslationRecovery.cpp
View File

@ -81,6 +81,7 @@ void TranslationRecovery::addPrior(
const double scale, NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel) const {
auto edge = relativeTranslations_.begin();
if (edge == relativeTranslations_.end()) return;
graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0),
priorNoiseModel);
graph->emplace_shared<PriorFactor<Point3> >(
@ -102,6 +103,15 @@ Values TranslationRecovery::initalizeRandomly() const {
insert(edge.key1());
insert(edge.key2());
}
// If there are no valid edges, but zero-distance edges exist, initialize one
// of the nodes in a connected component of zero-distance edges.
if (initial.empty() && !sameTranslationNodes_.empty()) {
for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
Key optimizedKey = optimizedAndDuplicateKeys.first;
initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
}
}
return initial;
}

60
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -2,7 +2,7 @@
* OrientedPlane3Factor.cpp
*
* Created on: Jan 29, 2014
* Author: Natesh Srinivasan
* Author: Natesh Srinivasan
*/
#include "OrientedPlane3Factor.h"
@ -14,15 +14,42 @@ namespace gtsam {
//***************************************************************************
void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "OrientedPlane3Factor Factor on " << landmarkKey_ << "\n";
cout << s << (s == "" ? "" : "\n");
cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", "
<< keyFormatter(key2()) << ")\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
const OrientedPlane3& plane, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Matrix36 predicted_H_pose;
Matrix33 predicted_H_plane, error_H_predicted;
OrientedPlane3 predicted_plane = plane.transform(pose,
H2 ? &predicted_H_plane : nullptr, H1 ? &predicted_H_pose : nullptr);
Vector3 err = predicted_plane.errorVector(
measured_p_, (H1 || H2) ? &error_H_predicted : nullptr);
// Apply the chain rule to calculate the derivatives.
if (H1) {
*H1 = error_H_predicted * predicted_H_pose;
}
if (H2) {
*H2 = error_H_predicted * predicted_H_plane;
}
return err;
}
//***************************************************************************
void OrientedPlane3DirectionPrior::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "Prior Factor on " << landmarkKey_ << "\n";
cout << s << (s == "" ? "" : "\n");
cout << s << "Prior Factor on " << keyFormatter(key()) << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
@ -36,26 +63,17 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
}
//***************************************************************************
Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H) const {
Vector OrientedPlane3DirectionPrior::evaluateError(
const OrientedPlane3& plane, boost::optional<Matrix&> H) const {
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Matrix2 H_p;
Vector e = n_hat_p.error(n_hat_q, H ? &H_p : nullptr);
if (H) {
Matrix H_p;
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q, H_p);
H->resize(2, 3);
H->block<2, 2>(0, 0) << H_p;
H->block<2, 1>(0, 2) << Z_2x1;
return e;
} else {
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q);
return e;
*H << H_p, Z_2x1;
}
}
return e;
}
} // namespace gtsam

58
gtsam/slam/OrientedPlane3Factor.h
View File

@ -16,66 +16,54 @@ namespace gtsam {
* Factor to measure a planar landmark from a given pose
*/
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
protected:
Key poseKey_;
Key landmarkKey_;
Vector measured_coeffs_;
protected:
OrientedPlane3 measured_p_;
typedef NoiseModelFactor2<Pose3, OrientedPlane3> Base;
public:
public:
/// Constructor
OrientedPlane3Factor() {
}
~OrientedPlane3Factor() override {}
/// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel,
const Key& pose, const Key& landmark) :
Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_(
z) {
measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
}
/** Constructor with measured plane (a,b,c,d) coefficients
* @param z measured plane (a,b,c,d) coefficients as 4D vector
* @param noiseModel noiseModel Gaussian noise model
* @param poseKey Key or symbol for unknown pose
* @param landmarkKey Key or symbol for unknown planar landmark
* @return the transformed plane
*/
OrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel,
Key poseKey, Key landmarkKey)
: Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {}
/// print
void print(const std::string& s = "OrientedPlane3Factor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/// evaluateError
Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const override {
OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1,
H2);
Vector err(3);
err << predicted_plane.error(measured_p_);
return (err);
}
;
Vector evaluateError(
const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
};
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> {
protected:
OrientedPlane3 measured_p_; /// measured plane parameters
Key landmarkKey_;
class OrientedPlane3DirectionPrior : public NoiseModelFactor1<OrientedPlane3> {
protected:
OrientedPlane3 measured_p_; /// measured plane parameters
typedef NoiseModelFactor1<OrientedPlane3> Base;
public:
public:
typedef OrientedPlane3DirectionPrior This;
/// Constructor
OrientedPlane3DirectionPrior() {
}
/// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
OrientedPlane3DirectionPrior(Key key, const Vector&z,
const SharedGaussian& noiseModel) :
Base(noiseModel, key), landmarkKey_(key) {
measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
}
OrientedPlane3DirectionPrior(Key key, const Vector4& z,
const SharedGaussian& noiseModel)
: Base(noiseModel, key), measured_p_(z) {}
/// print
void print(const std::string& s = "OrientedPlane3DirectionPrior",

205
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -10,20 +10,24 @@
* -------------------------------------------------------------------------- */
/*
* @file testOrientedPlane3.cpp
* @file testOrientedPlane3Factor.cpp
* @date Dec 19, 2012
* @author Alex Trevor
* @brief Tests the OrientedPlane3Factor class
*/
#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std.hpp>
#include <boost/bind.hpp>
using namespace boost::assign;
using namespace gtsam;
@ -32,46 +36,46 @@ using namespace std;
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
using symbol_shorthand::P; //< Planes
using symbol_shorthand::X; //< Pose3
// *************************************************************************
TEST (OrientedPlane3Factor, lm_translation_error) {
TEST(OrientedPlane3Factor, lm_translation_error) {
// Tests one pose, two measurements of the landmark that differ in range only.
// Normal along -x, 3m away
Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
ISAM2 isam2;
Values new_values;
NonlinearFactorGraph new_graph;
NonlinearFactorGraph graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
Symbol init_sym('x', 0);
// Init pose and prior. Pose Prior is needed since a single plane measurement
// does not fully constrain the pose
Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
Vector sigmas(6);
Vector6 sigmas;
sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
new_graph.addPrior(
init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas));
new_values.insert(init_sym, init_pose);
graph.addPrior(X(0), init_pose, noiseModel::Diagonal::Sigmas(sigmas));
// Add two landmark measurements, differing in range
new_values.insert(lm_sym, test_lm0);
Vector sigmas3(3);
sigmas3 << 0.1, 0.1, 0.1;
Vector test_meas0_mean(4);
test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
OrientedPlane3Factor test_meas0(test_meas0_mean,
noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas0);
Vector test_meas1_mean(4);
test_meas1_mean << -1.0, 0.0, 0.0, 1.0;
OrientedPlane3Factor test_meas1(test_meas1_mean,
noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas1);
Vector3 sigmas3 {0.1, 0.1, 0.1};
Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0};
OrientedPlane3Factor factor0(
measurement0, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0));
graph.add(factor0);
Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0};
OrientedPlane3Factor factor1(
measurement1, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0));
graph.add(factor1);
// Initial Estimate
Values values;
values.insert(X(0), init_pose);
values.insert(P(0), test_lm0);
// Optimize
ISAM2Result result = isam2.update(new_graph, new_values);
ISAM2 isam2;
ISAM2Result result = isam2.update(graph, values);
Values result_values = isam2.calculateEstimate();
OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>(
lm_sym);
OrientedPlane3 optimized_plane_landmark =
result_values.at<OrientedPlane3>(P(0));
// Given two noisy measurements of equal weight, expect result between the two
OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
@ -79,48 +83,80 @@ TEST (OrientedPlane3Factor, lm_translation_error) {
}
// *************************************************************************
// TODO As described in PR #564 after correcting the derivatives in
// OrientedPlane3Factor this test fails. It should be debugged and re-enabled.
/*
TEST (OrientedPlane3Factor, lm_rotation_error) {
// Tests one pose, two measurements of the landmark that differ in angle only.
// Normal along -x, 3m away
Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
OrientedPlane3 test_lm0(-1.0/sqrt(1.01), 0.1/sqrt(1.01), 0.0, 3.0);
ISAM2 isam2;
Values new_values;
NonlinearFactorGraph new_graph;
NonlinearFactorGraph graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
Symbol init_sym('x', 0);
Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
new_graph.addPrior(init_sym, init_pose,
graph.addPrior(X(0), init_pose,
noiseModel::Diagonal::Sigmas(
(Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));
new_values.insert(init_sym, init_pose);
// // Add two landmark measurements, differing in angle
new_values.insert(lm_sym, test_lm0);
Vector test_meas0_mean(4);
test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
OrientedPlane3Factor test_meas0(test_meas0_mean,
noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym);
new_graph.add(test_meas0);
Vector test_meas1_mean(4);
test_meas1_mean << 0.0, -1.0, 0.0, 3.0;
OrientedPlane3Factor test_meas1(test_meas1_mean,
noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym);
new_graph.add(test_meas1);
// Add two landmark measurements, differing in angle
Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0};
OrientedPlane3Factor factor0(measurement0,
noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0));
graph.add(factor0);
Vector4 measurement1 {0.0, -1.0, 0.0, 3.0};
OrientedPlane3Factor factor1(measurement1,
noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0));
graph.add(factor1);
// Initial Estimate
Values values;
values.insert(X(0), init_pose);
values.insert(P(0), test_lm0);
// Optimize
ISAM2Result result = isam2.update(new_graph, new_values);
ISAM2 isam2;
ISAM2Result result = isam2.update(graph, values);
Values result_values = isam2.calculateEstimate();
OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>(
lm_sym);
auto optimized_plane_landmark = result_values.at<OrientedPlane3>(P(0));
// Given two noisy measurements of equal weight, expect result between the two
OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0,
0.0, 3.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
*/
// *************************************************************************
TEST( OrientedPlane3Factor, Derivatives ) {
// Measurement
OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5);
// Linearisation point
OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7);
gtsam::Point3 pointLin = gtsam::Point3(1, 2, -4);
gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI);
Pose3 poseLin(rotationLin, pointLin);
// Factor
Key planeKey(1), poseKey(2);
SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
// Calculate numerical derivatives
boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
&OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2;
factor.evaluateError(poseLin, pLin, actualH1, actualH2);
// Verify we get the expected error
EXPECT(assert_equal(numericalH1, actualH1, 1e-8));
EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
}
// *************************************************************************
TEST( OrientedPlane3DirectionPrior, Constructor ) {
@ -129,7 +165,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
// If pitch and roll are zero for an aerospace frame,
// that means Z is pointing down, i.e., direction of Z = (0,0,-1)
Vector planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin
Vector4 planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin
// Factor
Key key(1);
@ -137,9 +173,9 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
OrientedPlane3DirectionPrior factor(key, planeOrientation, model);
// Create a linearization point at the zero-error point
Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0);
Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0);
Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0);
Vector4 theta1 {0.0, 0.02, -1.2, 10.0};
Vector4 theta2 {0.0, 0.1, -0.8, 10.0};
Vector4 theta3 {0.0, 0.2, -0.9, 10.0};
OrientedPlane3 T1(theta1);
OrientedPlane3 T2(theta2);
@ -170,6 +206,59 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
EXPECT(assert_equal(expectedH3, actualH3, 1e-8));
}
/* ************************************************************************* */
// Simplified version of the test by Marco Camurri to debug issue #561
TEST(OrientedPlane3Factor, Issue561Simplified) {
// Typedefs
using Plane = OrientedPlane3;
NonlinearFactorGraph graph;
// Setup prior factors
// Note: If x0 is too far away from the origin (e.g. x=100) this test can fail.
Pose3 x0(Rot3::identity(), Vector3(10, -1, 1));
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.addPrior<Pose3>(X(0), x0, x0_noise);
// Two horizontal planes with different heights, in the world frame.
const Plane p1(0,0,1,1), p2(0,0,1,2);
auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
graph.addPrior<Plane>(P(1), p1, p1_noise);
graph.addPrior<Plane>(P(2), p2, p2_noise);
// Plane factors
auto p1_measured_from_x0 = p1.transform(x0); // transform p1 to pose x0 as a measurement
auto p2_measured_from_x0 = p2.transform(x0); // transform p2 to pose x0 as a measurement
const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05);
const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05);
graph.emplace_shared<OrientedPlane3Factor>(
p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1));
graph.emplace_shared<OrientedPlane3Factor>(
p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2));
// Initial values
// Just offset the initial pose by 1m. This is what we are trying to optimize.
Values initialEstimate;
Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0)));
initialEstimate.insert(P(1), p1);
initialEstimate.insert(P(2), p2);
initialEstimate.insert(X(0), x0_initial);
// Optimize
try {
GaussNewtonOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
EXPECT(x0.equals(result.at<Pose3>(X(0))));
EXPECT(p1.equals(result.at<Plane>(P(1))));
EXPECT(p2.equals(result.at<Plane>(P(2))));
} catch (const IndeterminantLinearSystemException &e) {
std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl;
EXPECT(false); // fail if this happens
}
}
/* ************************************************************************* */
int main() {
srand(time(nullptr));

16
gtsam_unstable/CMakeLists.txt
View File

@ -107,22 +107,6 @@ install(
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
# Wrap version for gtsam_unstable
if (GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
# Set up codegen
include(GtsamMatlabWrap)
# Generate, build and install toolbox
set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
if(NOT BUILD_SHARED_LIBS)
list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
endif()
# Wrap
wrap_and_install_library(gtsam_unstable.i "gtsam" "" "${mexFlags}")
endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
# Build examples
add_subdirectory(examples)

1
gtsam_unstable/gtsam_unstable.i
View File

@ -163,6 +163,7 @@ class BearingS2 {
void serializable() const; // enabling serialization functionality
};
#include <gtsam_unstable/geometry/SimWall2D.h>
class SimWall2D {
SimWall2D();

64
gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp Normal file
View File

@ -0,0 +1,64 @@
/*
* LocalOrientedPlane3Factor.cpp
*
* Author: David Wisth
* Created on: February 12, 2021
*/
#include "LocalOrientedPlane3Factor.h"
using namespace std;
namespace gtsam {
//***************************************************************************
void LocalOrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << (s == "" ? "" : "\n");
cout << "LocalOrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", "
<< keyFormatter(key2()) << ", " << keyFormatter(key3()) << ")\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi,
const Pose3& wTwa, const OrientedPlane3& a_plane,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
Matrix66 aTai_H_wTwa, aTai_H_wTwi;
Matrix36 predicted_H_aTai;
Matrix33 predicted_H_plane, error_H_predicted;
// Find the relative transform from anchor to sensor frame.
const Pose3 aTai = wTwa.transformPoseTo(wTwi,
H2 ? &aTai_H_wTwa : nullptr,
H1 ? &aTai_H_wTwi : nullptr);
// Transform the plane measurement into sensor frame.
const OrientedPlane3 i_plane = a_plane.transform(aTai,
H2 ? &predicted_H_plane : nullptr,
(H1 || H3) ? &predicted_H_aTai : nullptr);
// Calculate the error between measured and estimated planes in sensor frame.
const Vector3 err = measured_p_.errorVector(i_plane,
boost::none, (H1 || H2 || H3) ? &error_H_predicted : nullptr);
// Apply the chain rule to calculate the derivatives.
if (H1) {
*H1 = error_H_predicted * predicted_H_aTai * aTai_H_wTwi;
}
if (H2) {
*H2 = error_H_predicted * predicted_H_aTai * aTai_H_wTwa;
}
if (H3) {
*H3 = error_H_predicted * predicted_H_plane;
}
return err;
}
} // namespace gtsam

92
gtsam_unstable/slam/LocalOrientedPlane3Factor.h Normal file
View File

@ -0,0 +1,92 @@
/*
* @file LocalOrientedPlane3Factor.h
* @brief LocalOrientedPlane3 Factor class
* @author David Wisth
* @date February 12, 2021
*/
#pragma once
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <string>
namespace gtsam {
/**
* Factor to measure a planar landmark from a given pose, with a given local
* linearization point.
*
* This factor is based on the relative plane factor formulation proposed in:
* Equation 25,
* M. Kaess, "Simultaneous Localization and Mapping with Infinite Planes",
* IEEE International Conference on Robotics and Automation, 2015.
*
*
* The main purpose of this factor is to improve the numerical stability of the
* optimization, especially compared to gtsam::OrientedPlane3Factor. This
* is especially relevant when the sensor is far from the origin (and thus
* the derivatives associated to transforming the plane are large).
*
* x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e.
* a local linearisation point for the plane. The plane is representated and
* optimized in x1 frame in the optimization.
*/
class LocalOrientedPlane3Factor: public NoiseModelFactor3<Pose3, Pose3,
OrientedPlane3> {
protected:
OrientedPlane3 measured_p_;
typedef NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> Base;
public:
/// Constructor
LocalOrientedPlane3Factor() {}
virtual ~LocalOrientedPlane3Factor() {}
/** Constructor with measured plane (a,b,c,d) coefficients
* @param z measured plane (a,b,c,d) coefficients as 4D vector
* @param noiseModel noiseModel Gaussian noise model
* @param poseKey Key or symbol for unknown pose
* @param anchorPoseKey Key or symbol for the plane's linearization point,
(called the "anchor pose").
* @param landmarkKey Key or symbol for unknown planar landmark
*
* Note: The anchorPoseKey can simply be chosen as the first pose a plane
* is observed.
*/
LocalOrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel,
Key poseKey, Key anchorPoseKey, Key landmarkKey)
: Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}
LocalOrientedPlane3Factor(const OrientedPlane3& z,
const SharedGaussian& noiseModel,
Key poseKey, Key anchorPoseKey, Key landmarkKey)
: Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}
/// print
void print(const std::string& s = "LocalOrientedPlane3Factor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/***
* Vector of errors
* @brief Error = measured_plane_.error(a_plane.transform(inv(wTwa) * wTwi))
*
* This is the error of the measured and predicted plane in the current
* sensor frame, i. The plane is represented in the anchor pose, a.
*
* @param wTwi The pose of the sensor in world coordinates
* @param wTwa The pose of the anchor frame in world coordinates
* @param a_plane The estimated plane in anchor frame.
*
* Note: The optimized plane is represented in anchor frame, a, not the
* world frame.
*/
Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa,
const OrientedPlane3& a_plane,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override;
};
} // namespace gtsam

243
gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp Normal file
View File

@ -0,0 +1,243 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* @file testLocalOrientedPlane3Factor.cpp
* @date Feb 12, 2021
* @author David Wisth
* @brief Tests the LocalOrientedPlane3Factor class
*/
#include <gtsam_unstable/slam/LocalOrientedPlane3Factor.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace gtsam;
using namespace std;
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
using symbol_shorthand::P; //< Planes
using symbol_shorthand::X; //< Pose3
// *************************************************************************
TEST(LocalOrientedPlane3Factor, lm_translation_error) {
// Tests one pose, two measurements of the landmark that differ in range only.
// Normal along -x, 3m away
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
NonlinearFactorGraph graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement
// does not fully constrain the pose
Pose3 init_pose = Pose3::identity();
Pose3 anchor_pose = Pose3::identity();
graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001));
// Add two landmark measurements, differing in range
Vector4 measurement0(-1.0, 0.0, 0.0, 3.0);
Vector4 measurement1(-1.0, 0.0, 0.0, 1.0);
LocalOrientedPlane3Factor factor0(
measurement0, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(1), P(0));
LocalOrientedPlane3Factor factor1(
measurement1, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(1), P(0));
graph.add(factor0);
graph.add(factor1);
// Initial Estimate
Values values;
values.insert(X(0), init_pose);
values.insert(X(1), anchor_pose);
values.insert(P(0), test_lm0);
// Optimize
ISAM2 isam2;
isam2.update(graph, values);
Values result_values = isam2.calculateEstimate();
auto optimized_plane_landmark = result_values.at<OrientedPlane3>(P(0));
// Given two noisy measurements of equal weight, expect result between the two
OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
// *************************************************************************
// TODO As described in PR #564 after correcting the derivatives in
// OrientedPlane3Factor this test fails. It should be debugged and re-enabled.
/*
TEST (LocalOrientedPlane3Factor, lm_rotation_error) {
// Tests one pose, two measurements of the landmark that differ in angle only.
// Normal along -x, 3m away
OrientedPlane3 test_lm0(-1.0/sqrt(1.01), -0.1/sqrt(1.01), 0.0, 3.0);
NonlinearFactorGraph graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement
// does not fully constrain the pose
Pose3 init_pose = Pose3::identity();
graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
// Add two landmark measurements, differing in angle
Vector4 measurement0(-1.0, 0.0, 0.0, 3.0);
Vector4 measurement1(0.0, -1.0, 0.0, 3.0);
LocalOrientedPlane3Factor factor0(measurement0,
noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0));
LocalOrientedPlane3Factor factor1(measurement1,
noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0));
graph.add(factor0);
graph.add(factor1);
// Initial Estimate
Values values;
values.insert(X(0), init_pose);
values.insert(P(0), test_lm0);
// Optimize
ISAM2 isam2;
isam2.update(graph, values);
Values result_values = isam2.calculateEstimate();
isam2.getDelta().print();
auto optimized_plane_landmark = result_values.at<OrientedPlane3>(P(0));
values.print();
result_values.print();
// Given two noisy measurements of equal weight, expect result between the two
OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0,
0.0, 3.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
*/
// *************************************************************************
TEST(LocalOrientedPlane3Factor, Derivatives) {
// Measurement
OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5);
// Linearisation point
OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7);
Pose3 poseLin(Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI), Point3(1, 2, -4));
Pose3 anchorPoseLin(Rot3::RzRyRx(-0.1*M_PI, 0.2*M_PI, 1.0), Point3(-5, 0, 1));
// Factor
Key planeKey(1), poseKey(2), anchorPoseKey(3);
SharedGaussian noise = noiseModel::Isotropic::Sigma(3, 0.1);
LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey);
// Calculate numerical derivatives
auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor,
_1, _2, _3, boost::none, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative31<Vector3, Pose3, Pose3,
OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3,
OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH3 = numericalDerivative33<Vector3, Pose3, Pose3,
OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2, actualH3;
factor.evaluateError(poseLin, anchorPoseLin, pLin, actualH1, actualH2,
actualH3);
// Verify we get the expected error
EXPECT(assert_equal(numericalH1, actualH1, 1e-8));
EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
EXPECT(assert_equal(numericalH3, actualH3, 1e-8));
}
/* ************************************************************************* */
// Simplified version of the test by Marco Camurri to debug issue #561
//
// This test provides an example of how LocalOrientedPlane3Factor works.
// x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e.
// a local linearisation point for the plane. The plane is representated and
// optimized in x1 frame in the optimization. This greatly improves numerical
// stability when the pose is far from the origin.
//
TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
// Typedefs
using Plane = OrientedPlane3;
NonlinearFactorGraph graph;
// Setup prior factors
Pose3 x0(Rot3::identity(), Vector3(100, 30, 10)); // the "sensor pose"
Pose3 x1(Rot3::identity(), Vector3(90, 40, 5) ); // the "anchor pose"
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.addPrior<Pose3>(X(0), x0, x0_noise);
graph.addPrior<Pose3>(X(1), x1, x1_noise);
// Two horizontal planes with different heights, in the world frame.
const Plane p1(0, 0, 1, 1), p2(0, 0, 1, 2);
// Transform to x1, the "anchor frame" (i.e. local frame)
auto p1_in_x1 = p1.transform(x1);
auto p2_in_x1 = p2.transform(x1);
auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
graph.addPrior<Plane>(P(1), p1_in_x1, p1_noise);
graph.addPrior<Plane>(P(2), p2_in_x1, p2_noise);
// Add plane factors, with a local linearization point.
// transform p1 to pose x0 as a measurement
auto p1_measured_from_x0 = p1.transform(x0);
// transform p2 to pose x0 as a measurement
auto p2_measured_from_x0 = p2.transform(x0);
const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05);
const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05);
graph.emplace_shared<LocalOrientedPlane3Factor>(
p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), X(1), P(1));
graph.emplace_shared<LocalOrientedPlane3Factor>(
p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), X(1), P(2));
// Initial values
// Just offset the initial pose by 1m. This is what we are trying to optimize.
Values initialEstimate;
Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1, 0, 0)));
initialEstimate.insert(P(1), p1_in_x1);
initialEstimate.insert(P(2), p2_in_x1);
initialEstimate.insert(X(0), x0_initial);
initialEstimate.insert(X(1), x1);
// Optimize
try {
ISAM2 isam2;
isam2.update(graph, initialEstimate);
Values result = isam2.calculateEstimate();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
EXPECT(x0.equals(result.at<Pose3>(X(0))));
EXPECT(p1_in_x1.equals(result.at<Plane>(P(1))));
EXPECT(p2_in_x1.equals(result.at<Plane>(P(2))));
} catch (const IndeterminantLinearSystemException &e) {
cerr << "CAPTURED THE EXCEPTION: "
<< DefaultKeyFormatter(e.nearbyVariable()) << endl;
EXPECT(false); // fail if this happens
}
}
/* ************************************************************************* */
int main() {
srand(time(nullptr));
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
matlab/+gtsam/plotBayesNet.m
View File

@ -5,4 +5,6 @@ function plotBayesNet(bayesNet)
bayesNet.saveGraph('/tmp/bayesNet.dot')
!dot -Tpng -o /tmp/dotImage.png /tmp/bayesNet.dot
dotImage=imread('/tmp/dotImage.png');
imshow(dotImage)
imshow(dotImage)
end

4
matlab/+gtsam/plotBayesTree.m
View File

@ -5,4 +5,6 @@ function plotBayesTree(bayesTree)
bayesTree.saveGraph('/tmp/bayesTree.dot')
!dot -Tpng -o /tmp/dotImage.png /tmp/bayesTree.dot
dotImage=imread('/tmp/dotImage.png');
imshow(dotImage)
imshow(dotImage)
end

148
matlab/CMakeLists.txt
View File

@ -1,44 +1,132 @@
# Install matlab components
include(GtsamMatlabWrap)
# Check if flag is enabled
if(NOT GTSAM_INSTALL_MATLAB_TOOLBOX)
return()
endif()
# Create the matlab toolbox for the gtsam library
# Set the wrapping script variable
set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py")
# Set up cache options
option(GTSAM_MEX_BUILD_STATIC_MODULE
"Build MATLAB wrapper statically (increases build time)" OFF)
set(GTSAM_BUILD_MEX_BINARY_FLAGS
""
CACHE STRING "Extra flags for running Matlab MEX compilation")
set(GTSAM_TOOLBOX_INSTALL_PATH
""
CACHE
PATH
"Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox"
)
if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
endif()
set(WRAP_MEX_BUILD_STATIC_MODULE ${GTSAM_MEX_BUILD_STATIC_MODULE})
set(WRAP_BUILD_MEX_BINARY_FLAGS ${GTSAM_BUILD_MEX_BINARY_FLAGS})
set(WRAP_TOOLBOX_INSTALL_PATH ${GTSAM_TOOLBOX_INSTALL_PATH})
set(WRAP_CUSTOM_MATLAB_PATH ${GTSAM_CUSTOM_MATLAB_PATH})
set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES})
# Fixup the Python paths
if(GTWRAP_DIR)
# packaged
set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR})
else()
set(GTWRAP_PACKAGE_DIR ${GTSAM_SOURCE_DIR}/wrap)
endif()
include(MatlabWrap)
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()
# ##############################################################################
# Generate, build and install toolbox
set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
if(NOT BUILD_SHARED_LIBS)
list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
endif()
# ignoring the non-concrete types (type aliases)
set(ignore
gtsam::Point2
gtsam::Point3)
# Wrap
matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}"
"" "${mexFlags}" "${ignore}")
# Wrap version for gtsam_unstable
if(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
# Generate, build and install toolbox
set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
if(NOT BUILD_SHARED_LIBS)
list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
endif()
# Wrap
matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam" ""
"${mexFlags}")
endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
# Record the root dir for gtsam - needed during external builds, e.g., ROS
set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR})
message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")
# Tests
#message(STATUS "Installing Matlab Toolbox")
# Tests message(STATUS "Installing Matlab Toolbox")
install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig")
install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "README-gtsam-toolbox.txt")
install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/"
"README-gtsam-toolbox.txt")
# Examples
#message(STATUS "Installing Matlab Toolbox Examples")
# Matlab files: *.m and *.fig
#install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" "*.m;*.fig")
# Examples message(STATUS "Installing Matlab Toolbox Examples") Matlab files:
# *.m and *.fig
# install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples"
# "*.m;*.fig")
# Utilities
#message(STATUS "Installing Matlab Toolbox Utilities")
#install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" "*.m")
# Utilities message(STATUS "Installing Matlab Toolbox Utilities")
# install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" "*.m")
#message(STATUS "Installing Matlab Toolbox Example Data")
# Data files: *.graph, *.mat, and *.txt
file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
file(GLOB matlab_examples_data_mat "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat")
file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt})
# message(STATUS "Installing Matlab Toolbox Example Data") Data files: *.graph,
# *.mat, and *.txt
file(GLOB matlab_examples_data_graph
"${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
file(GLOB matlab_examples_data_mat
"${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat")
file(GLOB matlab_examples_data_txt
"${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
set(matlab_examples_data
${matlab_examples_data_graph} ${matlab_examples_data_mat}
${matlab_examples_data_txt})
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
install(FILES ${matlab_examples_data} DESTINATION "${location}/${name}${build_type_tag}/gtsam_examples/Data" CONFIGURATIONS "${build_type}")
endforeach()
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed
# directory
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if
# there is one
get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
install(
FILES ${matlab_examples_data}
DESTINATION "${location}/${name}${build_type_tag}/gtsam_examples/Data"
CONFIGURATIONS "${build_type}")
endforeach()
else()
install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data)
install(FILES ${matlab_examples_data}
DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data)
endif()

2
matlab/gtsam_tests/testThinBayesTree.m
View File

@ -20,4 +20,4 @@
%% Run the tests
import gtsam.*
bayesTree = thinBayesTree(3,2);
EQUALITY('7 = bayesTree.size', 7, bayesTree.size);
EQUALITY('7 = bayesTree.size', 4, bayesTree.size);

10
matlab/gtsam_tests/testTriangulation.m
View File

@ -44,15 +44,15 @@ optimize = true;
rank_tol = 1e-9;
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9);
%% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements = Point2Vector;
measurements.push_back(z1.retract([0.1;0.5]));
measurements.push_back(z2.retract([-0.2;0.3]));
measurements.push_back(z1 + [0.1;0.5]);
measurements.push_back(z2 + [-0.2;0.3]);
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2));
CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-2);
%% two Poses with Bundler Calibration
bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480);
@ -67,4 +67,4 @@ measurements.push_back(z1);
measurements.push_back(z2);
triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize);
CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9);

3
matlab/gtsam_tests/thinBayesTree.m
View File

@ -1,5 +1,6 @@
function bayesTree = thinBayesTree(depth, width)
import gtsam.*
bayesNet = thinTreeBayesNet(depth, width);
bayesTree = GaussianBayesTree(bayesNet);
fg = GaussianFactorGraph(bayesNet);
bayesTree = fg.eliminateMultifrontal();
end

13
matlab/gtsam_tests/thinTreeBayesNet.m
View File

@ -6,8 +6,9 @@ bayesNet = GaussianBayesNet;
tree = thinTree(depth,width);
% Add root to the Bayes net
gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1));
bayesNet.push_front(gc);
model = noiseModel.Isotropic.Sigma(1, 3*rand(1));
gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), model);
bayesNet.push_back(gc);
n=tree.getNumberOfElements();
for i=2:n
@ -17,13 +18,15 @@ for i=2:n
% Create and link the corresponding GaussianConditionals
if tree.getW == 1 || di == 2
% Creation of single-parent GaussianConditional
gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), 5*rand(1));
model = noiseModel.Isotropic.Sigma(1, 5*rand(1));
gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), model);
elseif tree.getW == 2 || di == 3
% GaussianConditionalj associated with the second parent
gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), 5*rand(1));
model = noiseModel.Isotropic.Sigma(1, 5*rand(1));
gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), model);
end
% Add conditional to the Bayes net
bayesNet.push_front(gc);
bayesNet.push_back(gc);
end
end

4
python/CMakeLists.txt
View File

@ -13,6 +13,8 @@ configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in
file(COPY ${PROJECT_SOURCE_DIR}/python/MANIFEST.in
DESTINATION ${GTSAM_PYTHON_BUILD_DIRECTORY})
set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES})
include(PybindWrap)
############################################################
@ -41,6 +43,8 @@ set(ignore
gtsam::BetweenFactorPose2s
gtsam::BetweenFactorPose3s
gtsam::Point2Vector
gtsam::Point3Pairs
gtsam::Pose3Pairs
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::BinaryMeasurementsUnit3

2
python/gtsam/gtsam.tpl
View File

@ -25,7 +25,7 @@
{boost_class_export}
// Holder type for pybind11
{hoder_type}
{holder_type}
// Preamble for STL classes
// TODO(fan): make this automatic

2
python/gtsam/specializations.h
View File

@ -6,6 +6,8 @@ py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "
py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
#endif
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point3Pair> >(m_, "Point3Pairs");
py::bind_vector<std::vector<gtsam::Pose3Pair> >(m_, "Pose3Pairs");
py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");

134
python/gtsam/tests/test_Sim3.py Normal file
View File

@ -0,0 +1,134 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Sim3 unit tests.
Author: John Lambert
"""
# pylint: disable=no-name-in-module
import unittest
import numpy as np
import gtsam
from gtsam import Point3, Pose3, Pose3Pairs, Rot3, Similarity3
from gtsam.utils.test_case import GtsamTestCase
class TestSim3(GtsamTestCase):
"""Test selected Sim3 methods."""
def test_align_poses_along_straight_line(self):
"""Test Align Pose3Pairs method.
Scenario:
3 object poses
same scale (no gauge ambiguity)
world frame has poses rotated about x-axis (90 degree roll)
world and egovehicle frame translated by 15 meters w.r.t. each other
"""
Rx90 = Rot3.Rx(np.deg2rad(90))
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
eTo0 = Pose3(Rot3(), np.array([5, 0, 0]))
eTo1 = Pose3(Rot3(), np.array([10, 0, 0]))
eTo2 = Pose3(Rot3(), np.array([15, 0, 0]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose3(Rx90, np.array([-10, 0, 0]))
wTo1 = Pose3(Rx90, np.array([-5, 0, 0]))
wTo2 = Pose3(Rx90, np.array([0, 0, 0]))
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_along_straight_line_gauge(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
Scenario:
3 object poses
with gauge ambiguity (2x scale)
world frame has poses rotated about z-axis (90 degree yaw)
world and egovehicle frame translated by 11 meters w.r.t. each other
"""
Rz90 = Rot3.Rz(np.deg2rad(90))
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
eTo0 = Pose3(Rot3(), np.array([1, 0, 0]))
eTo1 = Pose3(Rot3(), np.array([2, 0, 0]))
eTo2 = Pose3(Rot3(), np.array([4, 0, 0]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose3(Rz90, np.array([0, 12, 0]))
wTo1 = Pose3(Rz90, np.array([0, 14, 0]))
wTo2 = Pose3(Rz90, np.array([0, 18, 0]))
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_scaled_squares(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
Make sure a big and small square can be aligned.
The u's represent a big square (10x10), and v's represents a small square (4x4).
Scenario:
4 object poses
with gauge ambiguity (2.5x scale)
"""
# 0, 90, 180, and 270 degrees yaw
R0 = Rot3.Rz(np.deg2rad(0))
R90 = Rot3.Rz(np.deg2rad(90))
R180 = Rot3.Rz(np.deg2rad(180))
R270 = Rot3.Rz(np.deg2rad(270))
aTi0 = Pose3(R0, np.array([2, 3, 0]))
aTi1 = Pose3(R90, np.array([12, 3, 0]))
aTi2 = Pose3(R180, np.array([12, 13, 0]))
aTi3 = Pose3(R270, np.array([2, 13, 0]))
aTi_list = [aTi0, aTi1, aTi2, aTi3]
bTi0 = Pose3(R0, np.array([4, 3, 0]))
bTi1 = Pose3(R90, np.array([8, 3, 0]))
bTi2 = Pose3(R180, np.array([8, 7, 0]))
bTi3 = Pose3(R270, np.array([4, 7, 0]))
bTi_list = [bTi0, bTi1, bTi2, bTi3]
ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity3.Align(ab_pairs)
for aTi, bTi in zip(aTi_list, bTi_list):
self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
if __name__ == "__main__":
unittest.main()

46
python/gtsam/tests/test_pickle.py Normal file
View File

@ -0,0 +1,46 @@
"""
GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests to check pickling.
Author: Ayush Baid
"""
from gtsam import Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, SfmTrack, Unit3
from gtsam.utils.test_case import GtsamTestCase
class TestPickle(GtsamTestCase):
"""Tests pickling on some of the classes."""
def test_cal3Bundler_roundtrip(self):
obj = Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70)
self.assertEqualityOnPickleRoundtrip(obj)
def test_pinholeCameraCal3Bundler_roundtrip(self):
obj = PinholeCameraCal3Bundler(
Pose3(Rot3.RzRyRx(0, 0.1, -0.05), Point3(1, 1, 0)),
Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70),
)
self.assertEqualityOnPickleRoundtrip(obj)
def test_rot3_roundtrip(self):
obj = Rot3.RzRyRx(0, 0.05, 0.1)
self.assertEqualityOnPickleRoundtrip(obj)
def test_pose3_roundtrip(self):
obj = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
self.assertEqualityOnPickleRoundtrip(obj)
def test_sfmTrack_roundtrip(self):
obj = SfmTrack(Point3(1, 1, 0))
obj.add_measurement(0, Point2(-1, 5))
obj.add_measurement(1, Point2(6, 2))
self.assertEqualityOnPickleRoundtrip(obj)
def test_unit3_roundtrip(self):
obj = Unit3(Point3(1, 1, 0))
self.assertEqualityOnPickleRoundtrip(obj)

12
python/gtsam/utils/test_case.py
View File

@ -8,6 +8,7 @@ See LICENSE for the license information
TestCase class with GTSAM assert utils.
Author: Frank Dellaert
"""
import pickle
import unittest
@ -29,3 +30,14 @@ class GtsamTestCase(unittest.TestCase):
if not equal:
raise self.failureException(
"Values are not equal:\n{}!={}".format(actual, expected))
def assertEqualityOnPickleRoundtrip(self, obj: object, tol=1e-9) -> None:
""" Performs a round-trip using pickle and asserts equality.
Usage:
self.assertEqualityOnPickleRoundtrip(obj)
Keyword Arguments:
tol {float} -- tolerance passed to 'equals', default 1e-9
"""
roundTripObj = pickle.loads(pickle.dumps(obj))
self.gtsamAssertEquals(roundTripObj, obj)

2
python/gtsam_unstable/gtsam_unstable.tpl
View File

@ -22,7 +22,7 @@
{boost_class_export}
{hoder_type}
{holder_type}
#include "python/gtsam_unstable/preamble.h"

32
tests/testTranslationRecovery.cpp
View File

@ -17,7 +17,6 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/dataset.h>
@ -185,7 +184,7 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
// There is only 1 non-zero translation edge.
EXPECT_LONGS_EQUAL(1, graph.size());
EXPECT_LONGS_EQUAL(1, graph.size());
// Run translation recovery
const auto result = algorithm.run(/*scale=*/3.0);
@ -238,6 +237,35 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3)));
}
TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
Values poses;
poses.insert<Pose3>(0, Pose3(Rot3::RzRyRx(-M_PI / 6, 0, 0), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0)));
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {1, 2}, {2, 0}});
// Check simulated measurements.
for (auto& unitTranslation : relativeTranslations) {
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
// Graph size will be zero as there no 'non-zero distance' edges.
EXPECT_LONGS_EQUAL(0, graph.size());
// Run translation recovery
const auto result = algorithm.run(/*scale=*/4.0);
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(1)));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2)));
}
/* ************************************************************************* */
int main() {
TestResult tr;

46
wrap/CMakeLists.txt
View File

@ -21,39 +21,27 @@ else()
set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake")
endif()
# Install scripts to the standard CMake script directory.
install(FILES cmake/gtwrapConfig.cmake cmake/PybindWrap.cmake
cmake/GtwrapUtils.cmake
# Install CMake scripts to the standard CMake script directory.
install(FILES cmake/gtwrapConfig.cmake cmake/MatlabWrap.cmake
cmake/PybindWrap.cmake cmake/GtwrapUtils.cmake
DESTINATION "${SCRIPT_INSTALL_DIR}/gtwrap")
# Needed for the CMAKE_INSTALL_X variables used below.
include(GNUInstallDirs)
# Install the gtwrap python package as a directory so it can be found by CMake
# for wrapping.
install(DIRECTORY gtwrap DESTINATION "${CMAKE_INSTALL_DATADIR}/gtwrap")
# Install wrapping scripts as binaries to `CMAKE_INSTALL_PREFIX/bin` so they can
# be invoked for wrapping.
# We use DESTINATION (instead of TYPE) so we can support older CMake versions.
# be invoked for wrapping. We use DESTINATION (instead of TYPE) so we can
# support older CMake versions.
install(PROGRAMS scripts/pybind_wrap.py scripts/matlab_wrap.py
DESTINATION ${CMAKE_INSTALL_BINDIR})
# Install pybind11 directory to `CMAKE_INSTALL_PREFIX/lib/pybind11` This will
# allow the gtwrapConfig.cmake file to load it later.
# We use DESTINATION (instead of TYPE) so we can support older CMake versions.
install(DIRECTORY pybind11
DESTINATION ${CMAKE_INSTALL_LIBDIR})
# Install pybind11 directory to `CMAKE_INSTALL_PREFIX/lib/gtwrap/pybind11` This
# will allow the gtwrapConfig.cmake file to load it later.
install(DIRECTORY pybind11 DESTINATION "${CMAKE_INSTALL_LIBDIR}/gtwrap")
# ##############################################################################
# Install the Python package
find_package(
Python ${WRAP_PYTHON_VERSION}
COMPONENTS Interpreter
EXACT)
# Detect virtualenv and set Pip args accordingly
# https://www.scivision.dev/cmake-install-python-package/
if(DEFINED ENV{VIRTUAL_ENV} OR DEFINED ENV{CONDA_PREFIX})
set(_pip_args)
else()
set(_pip_args "--user")
endif()
#TODO add correct flags for virtualenv
# Finally install the gtwrap python package.
execute_process(COMMAND ${Python_EXECUTABLE} -m pip install . ${_pip_args}
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
# Install the matlab.h file to `CMAKE_INSTALL_PREFIX/lib/gtwrap/matlab.h`.
install(FILES matlab.h DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/gtwrap")

167
wrap/DOCS.md Normal file
View File

@ -0,0 +1,167 @@
## Wrap Module Definition
### Important
The python wrapper supports keyword arguments for functions/methods. Hence, the argument names matter. An implementation restriction is that in overloaded methods or functions, arguments of different types *have* to have different names.
### Requirements
- Classes must start with an uppercase letter.
- The wrapper can wrap a typedef, e.g. `typedef TemplatedClass<Arg> EasyName;`.
- Only one Method/Constructor per line, though methods/constructors can extend across multiple lines.
- Methods can return
- Eigen types: `Matrix`, `Vector`.
- C/C++ basic types: `string`, `bool`, `size_t`, `int`, `double`, `char`, `unsigned char`.
- `void`
- Any class with which be copied with `boost::make_shared()`.
- `boost::shared_ptr` of any object type.
- Constructors
- Overloads are supported, but arguments of different types *have* to have different names.
- A class with no constructors can be returned from other functions but not allocated directly in MATLAB.
- Methods
- Constness has no effect.
- Specify by-value (not reference) return types, even if C++ method returns reference.
- Must start with a letter (upper or lowercase).
- Overloads are supported.
- Static methods
- Must start with a letter (upper or lowercase) and use the "static" keyword, e.g. `static void func()`.
- The first letter will be made uppercase in the generated MATLAB interface.
- Overloads are supported, but arguments of different types *have* to have different names.
- Arguments to functions can be any of
- Eigen types: `Matrix`, `Vector`.
- Eigen types and classes as an optionally const reference.
- C/C++ basic types: `string`, `bool`, `size_t`, `size_t`, `double`, `char`, `unsigned char`.
- Any class with which be copied with `boost::make_shared()` (except Eigen).
- `boost::shared_ptr` of any object type (except Eigen).
- Properties or Variables
- You can specify class variables in the interface file as long as they are in the `public` scope, e.g.
```cpp
class Sample {
double seed;
};
```
- Class variables are read-write so they can be updated directly in Python.
- Pointer types
- To declare a pointer type (including shared pointers), simply add an asterisk (i.e. `*`) to the class name.
- E.g. `gtsam::noiseModel::Base*` to define the wrapping for the `Base` noise model shared pointer.
- Comments can use either C++ or C style, with multiple lines.
- Namespace definitions
- Names of namespaces must start with a lowercase letter.
- Start a namespace with `namespace example_ns {`, where `example_ns` is the namespace name.
- End a namespace with exactly `}`
- Namespaces can be nested.
- Namespace usage
- Namespaces can be specified for classes in arguments and return values.
- In each case, the namespace must be fully specified, e.g., `namespace1::namespace2::ClassName`.
- Includes in C++ wrappers
- All includes will be collected and added in a single file.
- All namespaces must have angle brackets, e.g. `#include <path>`.
- No default includes will be added.
- Global/Namespace functions
- Functions specified outside of a class are **global**.
- Can be overloaded with different arguments.
- Can have multiple functions of the same name in different namespaces.
- Using classes defined in other modules
- If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error.
- Virtual inheritance
- Specify fully-qualified base classes, i.e. `virtual class Derived : ns::Base {` where `ns` is the namespace.
- Mark with `virtual` keyword, e.g. `virtual class Base {`, and also `virtual class Derived : ns::Base {`.
- Forward declarations must also be marked virtual, e.g. `virtual class ns::Base;` and
also `virtual class ns::Derived;`.
- Pure virtual (abstract) classes should list no constructors in the interface file.
- Virtual classes must have a `clone()` function in C++ (though it does not have to be included
in the interface file). `clone()` will be called whenever an object copy is needed, instead
of using the copy constructor (which is used for non-virtual objects).
- Signature of clone function - `clone()` will be called virtually, so must appear at least at the top of the inheritance tree
```cpp
virtual boost::shared_ptr<CLASS_NAME> clone() const;
```
- Class Templates
- Basic templates are supported either with an explicit list of types to instantiate,
e.g.
```cpp
template<T = {gtsam::Pose2, gtsam::Rot2, gtsam::Point3}> class Class1 { ... };
```
or with typedefs, e.g.
```cpp
template<T, U> class Class2 { ... };
typedef Class2<Type1, Type2> MyInstantiatedClass;
```
- In the class definition, appearances of the template argument(s) will be replaced with their
instantiated types, e.g. `void setValue(const T& value);`.
- To refer to the instantiation of the template class itself, use `This`, i.e. `static This Create();`.
- To create new instantiations in other modules, you must copy-and-paste the whole class definition
into the new module, but use only your new instantiation types.
- When forward-declaring template instantiations, use the generated/typedefed name, e.g.
```cpp
class gtsam::Class1Pose2;
class gtsam::MyInstantiatedClass;
```
- `Boost.serialization` within the wrapper:
- You need to mark classes as being serializable in the markup file (see `gtsam.i` for examples).
- There are two options currently, depending on the class. To "mark" a class as serializable,
add a function with a particular signature so that `wrap` will catch it.
- Add `void serialize()` to a class to create serialization functions for a class.
Adding this flag subsumes the `serializable()` flag below.
Requirements:
- A default constructor must be publicly accessible.
- Must not be an abstract base class.
- The class must have an actual boost.serialization `serialize()` function.
- Add `void serializable()` to a class if you only want the class to be serialized as a
part of a container (such as `noiseModel`). This version does not require a publicly
accessible default constructor.
- Forward declarations and class definitions for **Pybind**:
- Need to specify the base class (both this forward class and base class are declared in an external Pybind header)
This is so that Pybind can generate proper inheritance.
Example when wrapping a gtsam-based project:
```cpp
// forward declarations
virtual class gtsam::NonlinearFactor
virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor
// class definition
#include <MyFactor.h>
virtual class MyFactor : gtsam::NoiseModelFactor {...};
```
- **DO NOT** re-define an overriden function already declared in the external (forward-declared) base class
- This will cause an ambiguity problem in Pybind header file.
### TODO
- Default values for arguments.
- WORKAROUND: make multiple versions of the same function for different configurations of default arguments.
- Handle `gtsam::Rot3M` conversions to quaternions.
- Parse return of const ref arguments.
- Parse `std::string` variants and convert directly to special string.
- Add enum support.
- Add generalized serialization support via `boost.serialization` with hooks to MATLAB save/load.

97
wrap/README.md
View File

@ -1,15 +1,16 @@
# WRAP
The wrap library wraps the GTSAM library into a Python library or MATLAB toolbox.
It was designed to be more general than just wrapping GTSAM. For notes on creating a wrap interface, see `gtsam.h` for what features can be wrapped into a toolbox, as well as the current state of the toolbox for GTSAM.
## Prerequisites: Pybind11 and pyparsing
## Prerequisites
`Pybind11` and `pyparsing`
1. This library uses `pybind11`, which is included as a subdirectory in GTSAM.
2. The `interface_parser.py` in this library uses `pyparsing` to parse the interface file `gtsam.h`. Please install it first in your current Python environment before attempting the build.
```
```sh
python3 -m pip install pyparsing
```
@ -23,18 +24,18 @@ cmake ..
make install # use sudo if needed
```
Using `wrap` in your project is straightforward from here. In you `CMakeLists.txt` file, you just need to add the following:
Using `wrap` in your project is straightforward from here. In your `CMakeLists.txt` file, you just need to add the following:
```cmake
include(PybindWrap)
find_package(gtwrap)
pybind_wrap(${PROJECT_NAME}_py # target
${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h # interface header file
"${PROJECT_NAME}.cpp" # the generated cpp
"${PROJECT_NAME}" # module_name
"gtsam" # top namespace in the cpp file
"${PROJECT_MODULE_NAME}" # top namespace in the cpp file e.g. gtsam
"${ignore}" # ignore classes
${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl
${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl # the wrapping template file
${PROJECT_NAME} # libs
"${PROJECT_NAME}" # dependencies
ON # use boost
@ -43,36 +44,68 @@ pybind_wrap(${PROJECT_NAME}_py # target
For more information, please follow our [tutorial](https://github.com/borglab/gtsam-project-python).
## GTSAM Python wrapper
## Documentation
Documentation for wrapping C++ code can be found [here](https://github.com/borglab/wrap/blob/master/DOCS.md).
## Python Wrapper
**WARNING: On macOS, you have to statically build GTSAM to use the wrapper.**
1. Set `GTSAM_BUILD_PYTHON=ON` while configuring the build with `cmake`.
1. What you can do in the `build` folder:
1. Just run python then import GTSAM and play around:
```
import gtsam
gtsam.__dir__()
```
1. Just run python then import GTSAM and play around:
```python
import gtsam
gtsam.__dir__()
```
1. Run the unittests:
```sh
python -m unittest discover
```
1. Edit the unittests in `python/gtsam/*.py` and simply rerun the test.
They were symlinked to `<build_folder>/gtsam/*.py` to facilitate fast development.
`python -m unittest gtsam/tests/test_Pose3.py` - NOTE: You might need to re-run `cmake ..` if files are deleted or added.
1. Run the unittests:
```
python -m unittest discover
```
1. Edit the unittests in `python/gtsam/*.py` and simply rerun the test.
They were symlinked to `<build_folder>/gtsam/*.py` to facilitate fast development.
```
python -m unittest gtsam/tests/test_Pose3.py
```
- NOTE: You might need to re-run `cmake ..` if files are deleted or added.
1. Do `make install` and `cd <gtsam_install_folder>/python`. Here, you can:
1. Run the unittests:
```
python setup.py test
```
2. Install `gtsam` to your current Python environment.
```
python setup.py install
```
- NOTE: It's a good idea to create a virtual environment otherwise it will be installed in your system Python's site-packages.
1. Run the unittests:
```sh
python setup.py test
```
2. Install `gtsam` to your current Python environment.
```sh
python setup.py install
```
- NOTE: It's a good idea to create a virtual environment otherwise it will be installed in your system Python's site-packages.
## Matlab Wrapper
In the CMake, simply include the `MatlabWrap.cmake` file.
```cmake
include(MatlabWrap)
```
This cmake file defines functions for generating MATLAB wrappers.
- `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper.
Usage example:
`wrap_and_install_library("lba.h" "" "" "")`
Arguments:
- `interfaceHeader`: The relative or absolute path to the wrapper interface definition file.
- `linkLibraries`: Any _additional_ libraries to link. Your project library
(e.g. `lba`), libraries it depends on, and any necessary
MATLAB libraries will be linked automatically. So normally,
leave this empty.
- `extraIncludeDirs`: Any _additional_ include paths required by dependent
libraries that have not already been added by
include_directories. Again, normally, leave this empty.
- `extraMexFlags`: Any _additional_ flags to pass to the compiler when building
the wrap code. Normally, leave this empty.

477
wrap/cmake/MatlabWrap.cmake Normal file
View File

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

18
wrap/cmake/PybindWrap.cmake
View File

@ -1,5 +1,12 @@
set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION})
if(GTWRAP_PYTHON_PACKAGE_DIR)
# packaged
set(GTWRAP_PACKAGE_DIR "${GTWRAP_PYTHON_PACKAGE_DIR}")
else()
set(GTWRAP_PACKAGE_DIR ${CMAKE_CURRENT_LIST_DIR}/..)
endif()
# User-friendly Pybind11 wrapping and installing function.
# Builds a Pybind11 module from the provided interface_header.
# For example, for the interface header gtsam.h, this will
@ -35,9 +42,16 @@ function(pybind_wrap
else(USE_BOOST)
set(_WRAP_BOOST_ARG "")
endif(USE_BOOST)
if (UNIX)
set(GTWRAP_PATH_SEPARATOR ":")
else()
set(GTWRAP_PATH_SEPARATOR ";")
endif()
add_custom_command(OUTPUT ${generated_cpp}
COMMAND ${PYTHON_EXECUTABLE}
COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
${PYTHON_EXECUTABLE}
${PYBIND_WRAP_SCRIPT}
--src
${interface_header}

18
wrap/cmake/gtwrapConfig.cmake
View File

@ -5,9 +5,13 @@ set(GTWRAP_DIR "${CMAKE_CURRENT_LIST_DIR}")
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}")
if(WIN32 AND NOT CYGWIN)
set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/CMake")
set(GTWRAP_CMAKE_DIR "${GTWRAP_DIR}")
set(GTWRAP_SCRIPT_DIR ${GTWRAP_CMAKE_DIR}/../../../bin)
set(GTWRAP_PYTHON_PACKAGE_DIR ${GTWRAP_CMAKE_DIR}/../../../share/gtwrap)
else()
set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake")
set(GTWRAP_CMAKE_DIR "${GTWRAP_DIR}")
set(GTWRAP_SCRIPT_DIR ${GTWRAP_CMAKE_DIR}/../../../bin)
set(GTWRAP_PYTHON_PACKAGE_DIR ${GTWRAP_CMAKE_DIR}/../../../share/gtwrap)
endif()
# Standard includes
@ -16,12 +20,12 @@ include(CMakePackageConfigHelpers)
include(CMakeDependentOption)
# Load all the CMake scripts from the standard location
include(${SCRIPT_INSTALL_DIR}/gtwrap/PybindWrap.cmake)
include(${SCRIPT_INSTALL_DIR}/gtwrap/GtwrapUtils.cmake)
include(${GTWRAP_CMAKE_DIR}/PybindWrap.cmake)
include(${GTWRAP_CMAKE_DIR}/GtwrapUtils.cmake)
# Set the variables for the wrapping scripts to be used in the build.
set(PYBIND_WRAP_SCRIPT "${CMAKE_INSTALL_FULL_BINDIR}/pybind_wrap.py")
set(MATLAB_WRAP_SCRIPT "${CMAKE_INSTALL_FULL_BINDIR}/matlab_wrap.py")
set(PYBIND_WRAP_SCRIPT "${GTWRAP_SCRIPT_DIR}/pybind_wrap.py")
set(MATLAB_WRAP_SCRIPT "${GTWRAP_SCRIPT_DIR}/matlab_wrap.py")
# Load the pybind11 code from the library installation path
add_subdirectory(${CMAKE_INSTALL_FULL_LIBDIR}/pybind11 pybind11)
add_subdirectory(${GTWRAP_CMAKE_DIR}/../../gtwrap/pybind11 pybind11)

247
wrap/gtwrap/interface_parser.py
View File

@ -6,37 +6,43 @@ All Rights Reserved
See LICENSE for the license information
Parser to get the interface of a C++ source file
Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar and Frank Dellaert
Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
"""
import os
import sys
from pyparsing import (
alphas,
alphanums,
cppStyleComment,
delimitedList,
empty,
nums,
stringEnd,
CharsNotIn,
Forward,
Group,
Keyword,
Literal,
OneOrMore,
Optional,
Or,
ParseException,
ParserElement,
Suppress,
Word,
ZeroOrMore,
)
# pylint: disable=unnecessary-lambda, unused-import, expression-not-assigned, no-else-return, protected-access, too-few-public-methods, too-many-arguments
import sys
import typing
import pyparsing
from pyparsing import (CharsNotIn, Forward, Group, Keyword, Literal, OneOrMore,
Optional, Or, ParseException, ParserElement, Suppress,
Word, ZeroOrMore, alphanums, alphas, cppStyleComment,
delimitedList, empty, nums, stringEnd)
# Fix deepcopy issue with pyparsing
# Can remove once https://github.com/pyparsing/pyparsing/issues/208 is resolved.
if sys.version_info >= (3, 8):
def fixed_get_attr(self, item):
"""
Fix for monkey-patching issue with deepcopy in pyparsing.ParseResults
"""
if item == '__deepcopy__':
raise AttributeError(item)
try:
return self[item]
except KeyError:
return ""
# apply the monkey-patch
pyparsing.ParseResults.__getattr__ = fixed_get_attr
ParserElement.enablePackrat()
# rule for identifiers (e.g. variable names)
IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums)
POINTER, REF = map(Literal, "*&")
LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;")
LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=")
@ -70,9 +76,9 @@ BASIS_TYPES = map(
)
class Typename(object):
class Typename:
"""
Type's name with full namespaces.
Type's name with full namespaces, used in Type class.
"""
namespaces_name_rule = delimitedList(IDENT, "::")
@ -86,34 +92,38 @@ class Typename(object):
)
).setParseAction(lambda t: Typename(t.namespaces_name, t.instantiations))
def __init__(self, namespaces_name, instantiations=[]):
def __init__(self, namespaces_name, instantiations=()):
self.namespaces = namespaces_name[:-1]
self.name = namespaces_name[-1]
if instantiations:
if not isinstance(instantiations, list):
if not isinstance(instantiations, typing.Iterable):
self.instantiations = instantiations.asList()
else:
self.instantiations = instantiations
else:
self.instantiations = []
if self.name in ["Matrix", "Vector"] and not self.namespaces:
self.namespaces = ["gtsam"]
@staticmethod
def from_parse_result(parse_result):
"""Return the typename from the parsed result."""
return parse_result[0]
def __repr__(self):
return self.to_cpp()
def instantiated_name(self):
"""Get the instantiated name of the type."""
res = self.name
for instantiation in self.instantiations:
res += instantiation.instantiated_name()
return res
def to_cpp(self):
"""Generate the C++ code for wrapping."""
idx = 1 if self.namespaces and not self.namespaces[0] else 0
if self.instantiations:
cpp_name = self.name + "<{}>".format(
@ -140,8 +150,11 @@ class Typename(object):
return not res
class Type(object):
class _QualifiedType(object):
class Type:
"""
The type value that is parsed, e.g. void, string, size_t.
"""
class _QualifiedType:
"""
Type with qualifiers.
"""
@ -165,7 +178,7 @@ class Type(object):
self.is_ptr = is_ptr
self.is_ref = is_ref
class _BasisType(object):
class _BasisType:
"""
Basis types don't have qualifiers and only allow copy-by-value.
"""
@ -185,6 +198,7 @@ class Type(object):
@staticmethod
def from_parse_result(t):
"""Return the resulting Type from parsing the source."""
if t.basis:
return Type(
typename=t.basis,
@ -211,6 +225,8 @@ class Type(object):
def to_cpp(self, use_boost):
"""
Generate the C++ code for wrapping.
Treat all pointers as "const shared_ptr<T>&"
Treat Matrix and Vector as "const Matrix&" and "const Vector&" resp.
"""
@ -237,7 +253,15 @@ class Type(object):
)
class Argument(object):
class Argument:
"""
The type and name of a function/method argument.
E.g.
```
void sayHello(/*s is the method argument with type `const string&`*/ const string& s);
```
"""
rule = (Type.rule("ctype") + IDENT("name")).setParseAction(
lambda t: Argument(t.ctype, t.name)
)
@ -250,7 +274,10 @@ class Argument(object):
return '{} {}'.format(self.ctype.__repr__(), self.name)
class ArgumentList(object):
class ArgumentList:
"""
List of Argument objects for all arguments in a function.
"""
rule = Optional(delimitedList(Argument.rule)("args_list")).setParseAction(
lambda t: ArgumentList.from_parse_result(t.args_list)
)
@ -262,6 +289,7 @@ class ArgumentList(object):
@staticmethod
def from_parse_result(parse_result):
"""Return the result of parsing."""
if parse_result:
return ArgumentList(parse_result.asList())
else:
@ -271,13 +299,20 @@ class ArgumentList(object):
return self.args_list.__repr__()
def args_names(self):
"""Return a list of the names of all the arguments."""
return [arg.name for arg in self.args_list]
def to_cpp(self, use_boost):
"""Generate the C++ code for wrapping."""
return [arg.ctype.to_cpp(use_boost) for arg in self.args_list]
class ReturnType(object):
class ReturnType:
"""
Rule to parse the return type.
The return type can either be a single type or a pair such as <type1, type2>.
"""
_pair = (
PAIR.suppress()
+ LOPBRACK
@ -295,6 +330,9 @@ class ReturnType(object):
self.type2 = type2
def is_void(self):
"""
Check if the return type is void.
"""
return self.type1.typename.name == "void" and not self.type2
def __repr__(self):
@ -303,6 +341,7 @@ class ReturnType(object):
)
def to_cpp(self):
"""Generate the C++ code for wrapping."""
if self.type2:
return "std::pair<{type1},{type2}>".format(
type1=self.type1.to_cpp(), type2=self.type2.to_cpp()
@ -311,8 +350,20 @@ class ReturnType(object):
return self.type1.to_cpp()
class Template(object):
class TypenameAndInstantiations(object):
class Template:
"""
Rule to parse templated values in the interface file.
E.g.
template<POSE> // this is the Template.
class Camera { ... };
"""
class TypenameAndInstantiations:
"""
Rule to parse the template parameters.
template<typename POSE> // POSE is the Instantiation.
"""
rule = (
IDENT("typename")
+ Optional(
@ -351,8 +402,21 @@ class Template(object):
self.typenames = [ti.typename for ti in ti_list]
self.instantiations = [ti.instantiations for ti in ti_list]
def __repr__(self):
return "<{0}>".format(", ".join(self.typenames))
class Method(object):
class Method:
"""
Rule to parse a method in a class.
E.g.
```
class Hello {
void sayHello() const;
};
```
"""
rule = (
Optional(Template.rule("template"))
+ ReturnType.rule("return_type")
@ -387,7 +451,17 @@ class Method(object):
)
class StaticMethod(object):
class StaticMethod:
"""
Rule to parse all the static methods in a class.
E.g.
```
class Hello {
static void changeGreeting();
};
```
"""
rule = (
STATIC
+ ReturnType.rule("return_type")
@ -411,10 +485,15 @@ class StaticMethod(object):
return "static {} {}{}".format(self.return_type, self.name, self.args)
def to_cpp(self):
"""Generate the C++ code for wrapping."""
return self.name
class Constructor(object):
class Constructor:
"""
Rule to parse the class constructor.
Can have 0 or more arguments.
"""
rule = (
IDENT("name")
+ LPAREN
@ -433,7 +512,17 @@ class Constructor(object):
return "Constructor: {}".format(self.name)
class Property(object):
class Property:
"""
Rule to parse the variable members of a class.
E.g.
```
class Hello {
string name; // This is a property.
};
````
"""
rule = (Type.rule("ctype") + IDENT("name") + SEMI_COLON).setParseAction(
lambda t: Property(t.ctype, t.name)
)
@ -441,10 +530,6 @@ class Property(object):
def __init__(self, ctype, name, parent=''):
self.ctype = ctype
self.name = name
# Check type constraints: no pointer, no ref.
if self.ctype.is_ptr or self.ctype.is_ref:
raise ValueError("Can't deal with pointer/ref class properties.")
self.parent = parent
def __repr__(self):
@ -452,6 +537,7 @@ class Property(object):
def collect_namespaces(obj):
"""Get the chain of namespaces from the lowest to highest for the given object."""
namespaces = []
ancestor = obj.parent
while ancestor and ancestor.name:
@ -460,8 +546,21 @@ def collect_namespaces(obj):
return [''] + namespaces
class Class(object):
class MethodsAndProperties(object):
class Class:
"""
Rule to parse a class defined in the interface file.
E.g.
```
class Hello {
...
};
```
"""
class MethodsAndProperties:
"""
Rule for all the methods and properties within a class.
"""
rule = ZeroOrMore(
Constructor.rule ^ StaticMethod.rule ^ Method.rule ^ Property.rule
).setParseAction(lambda t: Class.MethodsAndProperties(t.asList()))
@ -549,10 +648,19 @@ class Class(object):
_property.parent = self
def namespaces(self):
"""Get the namespaces which this class is nested under as a list."""
return collect_namespaces(self)
class TypedefTemplateInstantiation(object):
class TypedefTemplateInstantiation:
"""
Rule for parsing typedefs (with templates) within the interface file.
E.g.
```
typedef SuperComplexName<Arg1, Arg2, Arg3> EasierName;
```
"""
rule = (
TYPEDEF + Typename.rule("typename") + IDENT("new_name") + SEMI_COLON
).setParseAction(
@ -567,7 +675,10 @@ class TypedefTemplateInstantiation(object):
self.parent = parent
class Include(object):
class Include:
"""
Rule to parse #include directives.
"""
rule = (
INCLUDE + LOPBRACK + CharsNotIn('>')("header") + ROPBRACK
).setParseAction(lambda t: Include(t.header))
@ -580,7 +691,10 @@ class Include(object):
return "#include <{}>".format(self.header)
class ForwardDeclaration(object):
class ForwardDeclaration:
"""
Rule to parse forward declarations in the interface file.
"""
rule = (
Optional(VIRTUAL("is_virtual"))
+ CLASS
@ -606,7 +720,10 @@ class ForwardDeclaration(object):
)
class GlobalFunction(object):
class GlobalFunction:
"""
Rule to parse functions defined in the global scope.
"""
rule = (
ReturnType.rule("return_type")
+ IDENT("name")
@ -634,10 +751,18 @@ class GlobalFunction(object):
)
def to_cpp(self):
"""Generate the C++ code for wrapping."""
return self.name
def find_sub_namespace(namespace, str_namespaces):
"""
Get the namespaces nested under `namespace`, filtered by a list of namespace strings.
Args:
namespace: The top-level namespace under which to find sub-namespaces.
str_namespaces: The list of namespace strings to filter against.
"""
if not str_namespaces:
return [namespace]
@ -659,7 +784,8 @@ def find_sub_namespace(namespace, str_namespaces):
return res
class Namespace(object):
class Namespace:
"""Rule for parsing a namespace in the interface file."""
rule = Forward()
rule << (
NAMESPACE
@ -687,6 +813,7 @@ class Namespace(object):
@staticmethod
def from_parse_result(t):
"""Return the result of parsing."""
if t.content:
content = t.content.asList()
else:
@ -717,6 +844,7 @@ class Namespace(object):
return res[0]
def top_level(self):
"""Return the top leve namespace."""
if self.name == '' or self.parent == '':
return self
else:
@ -726,15 +854,23 @@ class Namespace(object):
return "Namespace: {}\n\t{}".format(self.name, self.content)
def full_namespaces(self):
"""Get the full namespace list."""
ancestors = collect_namespaces(self)
if self.name:
ancestors.append(self.name)
return ancestors
class Module(object):
class Module:
"""
Module is just a global namespace.
E.g.
```
namespace gtsam {
...
}
```
"""
rule = (
@ -752,5 +888,6 @@ class Module(object):
rule.ignore(cppStyleComment)
@staticmethod
def parseString(str):
return Module.rule.parseString(str)[0]
def parseString(s: str):
"""Parse the source string and apply the rules."""
return Module.rule.parseString(s)[0]

36
wrap/gtwrap/matlab_wrapper.py
View File

@ -49,6 +49,8 @@ class MatlabWrapper(object):
}
"""Methods that should not be wrapped directly"""
whitelist = ['serializable', 'serialize']
"""Methods that should be ignored"""
ignore_methods = ['pickle']
"""Datatypes that do not need to be checked in methods"""
not_check_type = []
"""Data types that are primitive types"""
@ -563,6 +565,8 @@ class MatlabWrapper(object):
for method in methods:
if method.name in self.whitelist:
continue
if method.name in self.ignore_methods:
continue
comment += '%{name}({args})'.format(name=method.name, args=self._wrap_args(method.args))
@ -587,7 +591,7 @@ class MatlabWrapper(object):
file_name = self._wrapper_name() + '.cpp'
wrapper_file = textwrap.dedent('''\
# include <wrap/matlab.h>
# include <gtwrap/matlab.h>
# include <map>
''')
@ -612,6 +616,9 @@ class MatlabWrapper(object):
methods = self._group_methods(methods)
for method in methods:
if method in self.ignore_methods:
continue
if globals:
self._debug("[wrap_methods] wrapping: {}..{}={}".format(method[0].parent.name, method[0].name,
type(method[0].parent.name)))
@ -861,6 +868,8 @@ class MatlabWrapper(object):
method_name = method[0].name
if method_name in self.whitelist and method_name != 'serialize':
continue
if method_name in self.ignore_methods:
continue
if method_name == 'serialize':
serialize[0] = True
@ -932,6 +941,9 @@ class MatlabWrapper(object):
format_name = list(static_method[0].name)
format_name[0] = format_name[0].upper()
if static_method[0].name in self.ignore_methods:
continue
method_text += textwrap.indent(textwrap.dedent('''\
function varargout = {name}(varargin)
'''.format(name=''.join(format_name))),
@ -1010,7 +1022,8 @@ class MatlabWrapper(object):
file_name = self._clean_class_name(instantiated_class)
namespace_file_name = namespace_name + file_name
if instantiated_class.cpp_class() in self.ignore_classes:
uninstantiated_name = "::".join(instantiated_class.namespaces()[1:]) + "::" + instantiated_class.name
if uninstantiated_name in self.ignore_classes:
return None
# Class comment
@ -1463,7 +1476,7 @@ class MatlabWrapper(object):
"""Generate the c++ wrapper."""
# Includes
wrapper_file = textwrap.dedent('''\
#include <wrap/matlab.h>
#include <gtwrap/matlab.h>
#include <map>\n
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
@ -1472,7 +1485,16 @@ class MatlabWrapper(object):
includes_list = sorted(list(self.includes.keys()), key=lambda include: include.header)
wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y), includes_list) + '\n'
# Check the number of includes.
# If no includes, do nothing, if 1 then just append newline.
# if more than one, concatenate them with newlines.
if len(includes_list) == 0:
pass
elif len(includes_list) == 1:
wrapper_file += (str(includes_list[0]) + '\n')
else:
wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y), includes_list)
wrapper_file += '\n'
typedef_instances = '\n'
typedef_collectors = ''
@ -1518,7 +1540,11 @@ class MatlabWrapper(object):
ptr_ctor_frag = ''
for cls in self.classes:
if cls.cpp_class().strip() in self.ignore_classes:
uninstantiated_name = "::".join(cls.namespaces()[1:]) + "::" + cls.name
self._debug("Cls: {} -> {}".format(cls.name, uninstantiated_name))
if uninstantiated_name in self.ignore_classes:
self._debug("Ignoring: {} -> {}".format(cls.name, uninstantiated_name))
continue
def _has_serialization(cls):

133
wrap/gtwrap/pybind_wrapper.py
View File

@ -7,8 +7,11 @@ All Rights Reserved
See LICENSE for the license information
Code generator for wrapping a C++ module with Pybind11
Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar and Frank Dellaert
Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
"""
# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument
import re
import textwrap
@ -16,13 +19,16 @@ import gtwrap.interface_parser as parser
import gtwrap.template_instantiator as instantiator
class PybindWrapper(object):
class PybindWrapper:
"""
Class to generate binding code for Pybind11 specifically.
"""
def __init__(self,
module,
module_name,
top_module_namespaces='',
use_boost=False,
ignore_classes=[],
ignore_classes=(),
module_template=""):
self.module = module
self.module_name = module_name
@ -34,6 +40,7 @@ class PybindWrapper(object):
self.python_keywords = ['print', 'lambda']
def _py_args_names(self, args_list):
"""Set the argument names in Pybind11 format."""
names = args_list.args_names()
if names:
py_args = ['py::arg("{}")'.format(name) for name in names]
@ -42,6 +49,7 @@ class PybindWrapper(object):
return ''
def _method_args_signature_with_names(self, args_list):
"""Define the method signature types with the argument names."""
cpp_types = args_list.to_cpp(self.use_boost)
names = args_list.args_names()
types_names = ["{} {}".format(ctype, name) for ctype, name in zip(cpp_types, names)]
@ -49,6 +57,7 @@ class PybindWrapper(object):
return ','.join(types_names)
def wrap_ctors(self, my_class):
"""Wrap the constructors."""
res = ""
for ctor in my_class.ctors:
res += ('\n' + ' ' * 8 + '.def(py::init<{args_cpp_types}>()'
@ -76,6 +85,21 @@ class PybindWrapper(object):
gtsam::deserialize(serialized, *self);
}}, py::arg("serialized"))
'''.format(class_inst=cpp_class + '*'))
if cpp_method == "pickle":
if not cpp_class in self._serializing_classes:
raise ValueError("Cannot pickle a class which is not serializable")
return textwrap.dedent('''
.def(py::pickle(
[](const {cpp_class} &a){{ // __getstate__
/* Returns a string that encodes the state of the object */
return py::make_tuple(gtsam::serialize(a));
}},
[](py::tuple t){{ // __setstate__
{cpp_class} obj;
gtsam::deserialize(t[0].cast<std::string>(), obj);
return obj;
}}))
'''.format(cpp_class=cpp_class))
is_method = isinstance(method, instantiator.InstantiatedMethod)
is_static = isinstance(method, parser.StaticMethod)
@ -100,8 +124,10 @@ class PybindWrapper(object):
'{py_args_names}){suffix}'.format(
prefix=prefix,
cdef="def_static" if is_static else "def",
py_method=py_method if not py_method in self.python_keywords else py_method + "_",
opt_self="{cpp_class}* self".format(cpp_class=cpp_class) if is_method else "",
py_method=py_method if not py_method in self.python_keywords
else py_method + "_",
opt_self="{cpp_class}* self".format(
cpp_class=cpp_class) if is_method else "",
cpp_class=cpp_class,
cpp_method=cpp_method,
opt_comma=',' if is_method and args_names else '',
@ -137,6 +163,7 @@ class PybindWrapper(object):
return ret
def wrap_methods(self, methods, cpp_class, prefix='\n' + ' ' * 8, suffix=''):
"""Wrap all the methods in the `cpp_class`."""
res = ""
for method in methods:
@ -161,6 +188,7 @@ class PybindWrapper(object):
return res
def wrap_properties(self, properties, cpp_class, prefix='\n' + ' ' * 8):
"""Wrap all the properties in the `cpp_class`."""
res = ""
for prop in properties:
res += ('{prefix}.def_{property}("{property_name}", '
@ -173,50 +201,61 @@ class PybindWrapper(object):
return res
def wrap_instantiated_class(self, instantiated_class):
"""Wrap the class."""
module_var = self._gen_module_var(instantiated_class.namespaces())
cpp_class = instantiated_class.cpp_class()
if cpp_class in self.ignore_classes:
return ""
return ('\n py::class_<{cpp_class}, {class_parent}'
'{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
'{wrapped_ctors}'
'{wrapped_methods}'
'{wrapped_static_methods}'
'{wrapped_properties};\n'.format(
shared_ptr_type=('boost' if self.use_boost else 'std'),
cpp_class=cpp_class,
class_name=instantiated_class.name,
class_parent=str(instantiated_class.parent_class) +
(', ' if instantiated_class.parent_class else ''),
module_var=module_var,
wrapped_ctors=self.wrap_ctors(instantiated_class),
wrapped_methods=self.wrap_methods(instantiated_class.methods, cpp_class),
wrapped_static_methods=self.wrap_methods(instantiated_class.static_methods, cpp_class),
wrapped_properties=self.wrap_properties(instantiated_class.properties, cpp_class),
))
return (
'\n py::class_<{cpp_class}, {class_parent}'
'{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
'{wrapped_ctors}'
'{wrapped_methods}'
'{wrapped_static_methods}'
'{wrapped_properties};\n'.format(
shared_ptr_type=('boost' if self.use_boost else 'std'),
cpp_class=cpp_class,
class_name=instantiated_class.name,
class_parent=str(instantiated_class.parent_class) +
(', ' if instantiated_class.parent_class else ''),
module_var=module_var,
wrapped_ctors=self.wrap_ctors(instantiated_class),
wrapped_methods=self.wrap_methods(instantiated_class.methods,
cpp_class),
wrapped_static_methods=self.wrap_methods(
instantiated_class.static_methods, cpp_class),
wrapped_properties=self.wrap_properties(
instantiated_class.properties, cpp_class),
))
def wrap_stl_class(self, stl_class):
"""Wrap STL containers."""
module_var = self._gen_module_var(stl_class.namespaces())
cpp_class = stl_class.cpp_class()
if cpp_class in self.ignore_classes:
return ""
return ('\n py::class_<{cpp_class}, {class_parent}'
'{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
'{wrapped_ctors}'
'{wrapped_methods}'
'{wrapped_static_methods}'
'{wrapped_properties};\n'.format(
shared_ptr_type=('boost' if self.use_boost else 'std'),
cpp_class=cpp_class,
class_name=stl_class.name,
class_parent=str(stl_class.parent_class) + (', ' if stl_class.parent_class else ''),
module_var=module_var,
wrapped_ctors=self.wrap_ctors(stl_class),
wrapped_methods=self.wrap_methods(stl_class.methods, cpp_class),
wrapped_static_methods=self.wrap_methods(stl_class.static_methods, cpp_class),
wrapped_properties=self.wrap_properties(stl_class.properties, cpp_class),
))
return (
'\n py::class_<{cpp_class}, {class_parent}'
'{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
'{wrapped_ctors}'
'{wrapped_methods}'
'{wrapped_static_methods}'
'{wrapped_properties};\n'.format(
shared_ptr_type=('boost' if self.use_boost else 'std'),
cpp_class=cpp_class,
class_name=stl_class.name,
class_parent=str(stl_class.parent_class) +
(', ' if stl_class.parent_class else ''),
module_var=module_var,
wrapped_ctors=self.wrap_ctors(stl_class),
wrapped_methods=self.wrap_methods(stl_class.methods,
cpp_class),
wrapped_static_methods=self.wrap_methods(
stl_class.static_methods, cpp_class),
wrapped_properties=self.wrap_properties(
stl_class.properties, cpp_class),
))
def _partial_match(self, namespaces1, namespaces2):
for i in range(min(len(namespaces1), len(namespaces2))):
@ -237,6 +276,7 @@ class PybindWrapper(object):
return name
def wrap_namespace(self, namespace):
"""Wrap the complete `namespace`."""
wrapped = ""
includes = ""
@ -283,7 +323,10 @@ class PybindWrapper(object):
wrapped += self.wrap_instantiated_class(element)
# Global functions.
all_funcs = [func for func in namespace.content if isinstance(func, parser.GlobalFunction)]
all_funcs = [
func for func in namespace.content
if isinstance(func, parser.GlobalFunction)
]
wrapped += self.wrap_methods(
all_funcs,
self._add_namespaces('', namespaces)[:-2],
@ -293,6 +336,7 @@ class PybindWrapper(object):
return wrapped, includes
def wrap(self):
"""Wrap the code in the interface file."""
wrapped_namespace, includes = self.wrap_namespace(self.module)
# Export classes for serialization.
@ -308,13 +352,16 @@ class PybindWrapper(object):
)
boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format(new_name=new_name, )
holder_type = "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, " \
"{shared_ptr_type}::shared_ptr<TYPE_PLACEHOLDER_DONOTUSE>);"
include_boost = "#include <boost/shared_ptr.hpp>" if self.use_boost else ""
return self.module_template.format(
include_boost="#include <boost/shared_ptr.hpp>" if self.use_boost else "",
include_boost=include_boost,
module_name=self.module_name,
includes=includes,
hoder_type=
"PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, {shared_ptr_type}::shared_ptr<TYPE_PLACEHOLDER_DONOTUSE>);"
.format(shared_ptr_type=('boost' if self.use_boost else 'std')) if self.use_boost else "",
holder_type=holder_type.format(shared_ptr_type=('boost' if self.use_boost else 'std'))
if self.use_boost else "",
wrapped_namespace=wrapped_namespace,
boost_class_export=boost_class_export,
)

117
wrap/gtwrap/template_instantiator.py
View File

@ -1,15 +1,45 @@
"""Code to help instantiate templated classes, methods and functions."""
# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, unused-variable
import itertools
from copy import deepcopy
from typing import List
import gtwrap.interface_parser as parser
def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, instantiated_class=None):
def instantiate_type(ctype: parser.Type,
template_typenames: List[str],
instantiations: List[parser.Typename],
cpp_typename: parser.Typename,
instantiated_class=None):
"""
Instantiate template typename for @p ctype.
Args:
instiated_class (InstantiatedClass):
@return If ctype's name is in the @p template_typenames, return the
corresponding type to replace in @p instantiations.
If ctype name is `This`, return the new typename @p `cpp_typename`.
Otherwise, return the original ctype.
"""
# make a deep copy so that there is no overwriting of original template params
ctype = deepcopy(ctype)
# Check if the return type has template parameters
if len(ctype.typename.instantiations) > 0:
for idx, instantiation in enumerate(ctype.typename.instantiations):
if instantiation.name in template_typenames:
template_idx = template_typenames.index(instantiation.name)
ctype.typename.instantiations[idx] = instantiations[
template_idx]
return ctype
str_arg_typename = str(ctype.typename)
if str_arg_typename in template_typenames:
idx = template_typenames.index(str_arg_typename)
return parser.Type(
@ -20,7 +50,6 @@ def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, in
is_basis=ctype.is_basis,
)
elif str_arg_typename == 'This':
# import sys
if instantiated_class:
name = instantiated_class.original.name
namespaces_name = instantiated_class.namespaces()
@ -29,8 +58,8 @@ def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, in
# ctype, instantiations, cpp_typename, instantiated_class.instantiations
# ), file=sys.stderr)
cpp_typename = parser.Typename(
namespaces_name, instantiations=[inst for inst in instantiated_class.instantiations]
)
namespaces_name,
instantiations=instantiated_class.instantiations)
return parser.Type(
typename=cpp_typename,
is_const=ctype.is_const,
@ -70,12 +99,18 @@ def instantiate_args_list(args_list, template_typenames, instantiations,
def instantiate_return_type(return_type, template_typenames, instantiations,
cpp_typename, instantiated_class=None):
new_type1 = instantiate_type(
return_type.type1, template_typenames, instantiations, cpp_typename, instantiated_class=instantiated_class)
"""Instantiate the return type."""
new_type1 = instantiate_type(return_type.type1,
template_typenames,
instantiations,
cpp_typename,
instantiated_class=instantiated_class)
if return_type.type2:
new_type2 = instantiate_type(
return_type.type2, template_typenames, instantiations,
cpp_typename, instantiated_class=instantiated_class)
new_type2 = instantiate_type(return_type.type2,
template_typenames,
instantiations,
cpp_typename,
instantiated_class=instantiated_class)
else:
new_type2 = ''
return parser.ReturnType(new_type1, new_type2)
@ -89,9 +124,14 @@ def instantiate_name(original_name, instantiations):
namespaces, but I find that too verbose.
"""
inst_name = ''
instantiated_names = []
for inst in instantiations:
# Ensure the first character of the type is capitalized
name = inst.instantiated_name()
# Using `capitalize` on the complete causes other caps to be lower case
instantiated_names.append(name.replace(name[0], name[0].capitalize()))
return "{}{}".format(original_name, "".join(
[inst.instantiated_name() for inst in instantiations]))
return "{}{}".format(original_name, "".join(instantiated_names))
class InstantiatedMethod(parser.Method):
@ -111,6 +151,7 @@ class InstantiatedMethod(parser.Method):
self.return_type = original.return_type
self.args = original.args
else:
#TODO(Varun) enable multiple templates for methods
if len(self.original.template.typenames) > 1:
raise ValueError("Can only instantiate template method with "
"single template parameter.")
@ -133,11 +174,20 @@ class InstantiatedMethod(parser.Method):
)
self.args = parser.ArgumentList(instantiated_args)
super().__init__(self.template,
self.name,
self.return_type,
self.args,
self.is_const,
parent=self.parent)
def to_cpp(self):
"""Generate the C++ code for wrapping."""
if self.original.template:
return "{}<{}>".format(self.original.name, self.instantiation)
ret = "{}<{}>".format(self.original.name, self.instantiation)
else:
return self.original.name
ret = self.original.name
return ret
def __repr__(self):
return "Instantiated {}".format(
@ -146,7 +196,10 @@ class InstantiatedMethod(parser.Method):
class InstantiatedClass(parser.Class):
def __init__(self, original, instantiations=[], new_name=''):
"""
Instantiate the class defined in the interface file.
"""
def __init__(self, original, instantiations=(), new_name=''):
"""
Template <T, U>
Instantiations: [T1, U1]
@ -190,6 +243,18 @@ class InstantiatedClass(parser.Class):
for inst in method.template.instantiations[0]:
self.methods.append(InstantiatedMethod(method, inst))
super().__init__(
self.template,
self.is_virtual,
self.name,
[self.parent_class],
self.ctors,
self.methods,
self.static_methods,
self.properties,
parent=self.parent,
)
def __repr__(self):
return "{virtual} class {name} [{cpp_class}]: {parent_class}\n"\
"{ctors}\n{static_methods}\n{methods}".format(
@ -204,6 +269,7 @@ class InstantiatedClass(parser.Class):
)
def instantiate_ctors(self):
"""Instantiate the class constructors."""
instantiated_ctors = []
for ctor in self.original.ctors:
instantiated_args = instantiate_args_list(
@ -220,6 +286,7 @@ class InstantiatedClass(parser.Class):
return instantiated_ctors
def instantiate_static_methods(self):
"""Instantiate static methods in the class."""
instantiated_static_methods = []
for static_method in self.original.static_methods:
instantiated_args = instantiate_args_list(
@ -274,6 +341,7 @@ class InstantiatedClass(parser.Class):
return class_instantiated_methods
def instantiate_properties(self):
"""Instantiate the class properties."""
instantiated_properties = instantiate_args_list(
self.original.properties,
self.original.template.typenames,
@ -283,6 +351,7 @@ class InstantiatedClass(parser.Class):
return instantiated_properties
def cpp_class(self):
"""Generate the C++ code for wrapping."""
return self.cpp_typename().to_cpp()
def cpp_typename(self):
@ -303,7 +372,10 @@ class InstantiatedClass(parser.Class):
def instantiate_namespace_inplace(namespace):
"""
@param[in/out] namespace The namespace which content will be replaced with
Instantiate the classes and other elements in the `namespace` content and
assign it back to the namespace content attribute.
@param[in/out] namespace The namespace whose content will be replaced with
the instantiated content.
"""
instantiated_content = []
@ -316,15 +388,14 @@ def instantiate_namespace_inplace(namespace):
instantiated_content.append(
InstantiatedClass(original_class, []))
else:
if (len(original_class.template.typenames) > 1
and original_class.template.instantiations[0]):
raise ValueError(
"Can't instantiate multi-parameter templates here. "
"Please use typedef template instantiation."
)
for inst in original_class.template.instantiations[0]:
# Use itertools to get all possible combinations of instantiations
# Works even if one template does not have an instantiation list
for instantiations in itertools.product(
*original_class.template.instantiations):
instantiated_content.append(
InstantiatedClass(original_class, [inst]))
InstantiatedClass(original_class,
list(instantiations)))
elif isinstance(element, parser.TypedefTemplateInstantiation):
typedef_inst = element
original_class = namespace.top_level().find_class(

2
wrap/pybind_wrapper.tpl.example
View File

@ -11,7 +11,7 @@
{boost_class_export}
{hoder_type}
{holder_type}
#include "python/preamble.h"

8
wrap/scripts/pybind_wrap.py
View File

@ -6,6 +6,8 @@ This script is installed via CMake to the user's binary directory
and invoked during the wrapping by CMake.
"""
# pylint: disable=import-error
import argparse
import gtwrap.interface_parser as parser
@ -68,13 +70,16 @@ def main():
if top_module_namespaces[0]:
top_module_namespaces = [''] + top_module_namespaces
# Read in the complete interface (.i) file
with open(args.src, "r") as f:
content = f.read()
module = parser.Module.parseString(content)
instantiator.instantiate_namespace_inplace(module)
with open(args.template, "r") as f:
template_content = f.read()
wrapper = PybindWrapper(
module=module,
module_name=args.module_name,
@ -84,7 +89,10 @@ def main():
module_template=template_content,
)
# Wrap the code and get back the cpp/cc code.
cc_content = wrapper.wrap()
# Generate the C++ code which Pybind11 will use.
with open(args.out, "w") as f:
f.write(cc_content)

31
wrap/tests/expected-matlab/MultipleTemplatesIntDouble.m Normal file
View File

@ -0,0 +1,31 @@
%class MultipleTemplatesIntDouble, see Doxygen page for details
%at https://gtsam.org/doxygen/
%
classdef MultipleTemplatesIntDouble < handle
properties
ptr_MultipleTemplatesIntDouble = 0
end
methods
function obj = MultipleTemplatesIntDouble(varargin)
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
my_ptr = varargin{2};
geometry_wrapper(89, my_ptr);
else
error('Arguments do not match any overload of MultipleTemplatesIntDouble constructor');
end
obj.ptr_MultipleTemplatesIntDouble = my_ptr;
end
function delete(obj)
geometry_wrapper(90, obj.ptr_MultipleTemplatesIntDouble);
end
function display(obj), obj.print(''); end
%DISPLAY Calls print on the object
function disp(obj), obj.display; end
%DISP Calls print on the object
end
methods(Static = true)
end
end

31
wrap/tests/expected-matlab/MultipleTemplatesIntFloat.m Normal file
View File

@ -0,0 +1,31 @@
%class MultipleTemplatesIntFloat, see Doxygen page for details
%at https://gtsam.org/doxygen/
%
classdef MultipleTemplatesIntFloat < handle
properties
ptr_MultipleTemplatesIntFloat = 0
end
methods
function obj = MultipleTemplatesIntFloat(varargin)
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
my_ptr = varargin{2};
geometry_wrapper(91, my_ptr);
else
error('Arguments do not match any overload of MultipleTemplatesIntFloat constructor');
end
obj.ptr_MultipleTemplatesIntFloat = my_ptr;
end
function delete(obj)
geometry_wrapper(92, obj.ptr_MultipleTemplatesIntFloat);
end
function display(obj), obj.print(''); end
%DISPLAY Calls print on the object
function disp(obj), obj.display; end
%DISP Calls print on the object
end
methods(Static = true)
end
end

6
wrap/tests/expected-matlab/MyFactorPosePoint2.m
View File

@ -12,9 +12,9 @@ classdef MyFactorPosePoint2 < handle
function obj = MyFactorPosePoint2(varargin)
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
my_ptr = varargin{2};
geometry_wrapper(89, my_ptr);
geometry_wrapper(93, my_ptr);
elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base')
my_ptr = geometry_wrapper(90, varargin{1}, varargin{2}, varargin{3}, varargin{4});
my_ptr = geometry_wrapper(94, varargin{1}, varargin{2}, varargin{3}, varargin{4});
else
error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
end
@ -22,7 +22,7 @@ classdef MyFactorPosePoint2 < handle
end
function delete(obj)
geometry_wrapper(91, obj.ptr_MyFactorPosePoint2);
geometry_wrapper(95, obj.ptr_MyFactorPosePoint2);
end
function display(obj), obj.print(''); end

2
wrap/tests/expected-matlab/aGlobalFunction.m
View File

@ -1,6 +1,6 @@
function varargout = aGlobalFunction(varargin)
if length(varargin) == 0
varargout{1} = geometry_wrapper(95, varargin{:});
varargout{1} = geometry_wrapper(99, varargin{:});
else
error('Arguments do not match any overload of function aGlobalFunction');
end

156
wrap/tests/expected-matlab/geometry_wrapper.cpp
View File

@ -1,4 +1,4 @@
#include <wrap/matlab.h>
#include <gtwrap/matlab.h>
#include <map>
#include <boost/archive/text_iarchive.hpp>
@ -11,9 +11,11 @@
typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix;
typedef PrimitiveRef<double> PrimitiveRefdouble;
typedef PrimitiveRef<double> PrimitiveRefDouble;
typedef MyVector<3> MyVector3;
typedef MyVector<12> MyVector12;
typedef MultipleTemplates<int, double> MultipleTemplatesIntDouble;
typedef MultipleTemplates<int, float> MultipleTemplatesIntFloat;
typedef MyFactor<gtsam::Pose2, gtsam::Matrix> MyFactorPosePoint2;
BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2");
@ -31,12 +33,16 @@ typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint
static Collector_MyTemplatePoint2 collector_MyTemplatePoint2;
typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_MyTemplateMatrix;
static Collector_MyTemplateMatrix collector_MyTemplateMatrix;
typedef std::set<boost::shared_ptr<PrimitiveRefdouble>*> Collector_PrimitiveRefdouble;
static Collector_PrimitiveRefdouble collector_PrimitiveRefdouble;
typedef std::set<boost::shared_ptr<PrimitiveRefDouble>*> Collector_PrimitiveRefDouble;
static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble;
typedef std::set<boost::shared_ptr<MyVector3>*> Collector_MyVector3;
static Collector_MyVector3 collector_MyVector3;
typedef std::set<boost::shared_ptr<MyVector12>*> Collector_MyVector12;
static Collector_MyVector12 collector_MyVector12;
typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_MultipleTemplatesIntDouble;
static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
@ -82,10 +88,10 @@ void _deleteAllObjects()
collector_MyTemplateMatrix.erase(iter++);
anyDeleted = true;
} }
{ for(Collector_PrimitiveRefdouble::iterator iter = collector_PrimitiveRefdouble.begin();
iter != collector_PrimitiveRefdouble.end(); ) {
{ for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin();
iter != collector_PrimitiveRefDouble.end(); ) {
delete *iter;
collector_PrimitiveRefdouble.erase(iter++);
collector_PrimitiveRefDouble.erase(iter++);
anyDeleted = true;
} }
{ for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
@ -100,6 +106,18 @@ void _deleteAllObjects()
collector_MyVector12.erase(iter++);
anyDeleted = true;
} }
{ for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin();
iter != collector_MultipleTemplatesIntDouble.end(); ) {
delete *iter;
collector_MultipleTemplatesIntDouble.erase(iter++);
anyDeleted = true;
} }
{ for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin();
iter != collector_MultipleTemplatesIntFloat.end(); ) {
delete *iter;
collector_MultipleTemplatesIntFloat.erase(iter++);
anyDeleted = true;
} }
{ for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
iter != collector_MyFactorPosePoint2.end(); ) {
delete *iter;
@ -912,42 +930,42 @@ void MyTemplateMatrix_Level_78(int nargout, mxArray *out[], int nargin, const mx
out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
}
void PrimitiveRefdouble_collectorInsertAndMakeBase_79(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void PrimitiveRefDouble_collectorInsertAndMakeBase_79(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
collector_PrimitiveRefdouble.insert(self);
collector_PrimitiveRefDouble.insert(self);
}
void PrimitiveRefdouble_constructor_80(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void PrimitiveRefDouble_constructor_80(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
Shared *self = new Shared(new PrimitiveRef<double>());
collector_PrimitiveRefdouble.insert(self);
collector_PrimitiveRefDouble.insert(self);
out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
*reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
}
void PrimitiveRefdouble_deconstructor_81(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void PrimitiveRefDouble_deconstructor_81(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
checkArguments("delete_PrimitiveRefdouble",nargout,nargin,1);
checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1);
Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
Collector_PrimitiveRefdouble::iterator item;
item = collector_PrimitiveRefdouble.find(self);
if(item != collector_PrimitiveRefdouble.end()) {
Collector_PrimitiveRefDouble::iterator item;
item = collector_PrimitiveRefDouble.find(self);
if(item != collector_PrimitiveRefDouble.end()) {
delete self;
collector_PrimitiveRefdouble.erase(item);
collector_PrimitiveRefDouble.erase(item);
}
}
void PrimitiveRefdouble_Brutal_82(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void PrimitiveRefDouble_Brutal_82(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("PrimitiveRefdouble.Brutal",nargout,nargin,1);
checkArguments("PrimitiveRefDouble.Brutal",nargout,nargin,1);
double t = unwrap< double >(in[0]);
out[0] = wrap_shared_ptr(boost::make_shared<PrimitiveRef<double>>(PrimitiveRef<double>::Brutal(t)),"PrimitiveRefdouble", false);
}
@ -1018,7 +1036,51 @@ void MyVector12_deconstructor_88(int nargout, mxArray *out[], int nargin, const
}
}
void MyFactorPosePoint2_collectorInsertAndMakeBase_89(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_89(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
collector_MultipleTemplatesIntDouble.insert(self);
}
void MultipleTemplatesIntDouble_deconstructor_90(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1);
Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
Collector_MultipleTemplatesIntDouble::iterator item;
item = collector_MultipleTemplatesIntDouble.find(self);
if(item != collector_MultipleTemplatesIntDouble.end()) {
delete self;
collector_MultipleTemplatesIntDouble.erase(item);
}
}
void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_91(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
collector_MultipleTemplatesIntFloat.insert(self);
}
void MultipleTemplatesIntFloat_deconstructor_92(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1);
Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
Collector_MultipleTemplatesIntFloat::iterator item;
item = collector_MultipleTemplatesIntFloat.find(self);
if(item != collector_MultipleTemplatesIntFloat.end()) {
delete self;
collector_MultipleTemplatesIntFloat.erase(item);
}
}
void MyFactorPosePoint2_collectorInsertAndMakeBase_93(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@ -1027,7 +1089,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_89(int nargout, mxArray *out[
collector_MyFactorPosePoint2.insert(self);
}
void MyFactorPosePoint2_constructor_90(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void MyFactorPosePoint2_constructor_94(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
mexAtExit(&_deleteAllObjects);
typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@ -1042,7 +1104,7 @@ void MyFactorPosePoint2_constructor_90(int nargout, mxArray *out[], int nargin,
*reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
}
void MyFactorPosePoint2_deconstructor_91(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void MyFactorPosePoint2_deconstructor_95(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1);
@ -1055,7 +1117,7 @@ void MyFactorPosePoint2_deconstructor_91(int nargout, mxArray *out[], int nargin
}
}
void load2D_92(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void load2D_96(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("load2D",nargout,nargin,5);
string filename = unwrap< string >(in[0]);
@ -1067,7 +1129,7 @@ void load2D_92(int nargout, mxArray *out[], int nargin, const mxArray *in[])
out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false);
out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false);
}
void load2D_93(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void load2D_97(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("load2D",nargout,nargin,5);
string filename = unwrap< string >(in[0]);
@ -1079,7 +1141,7 @@ void load2D_93(int nargout, mxArray *out[], int nargin, const mxArray *in[])
out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false);
out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false);
}
void load2D_94(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void load2D_98(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("load2D",nargout,nargin,2);
string filename = unwrap< string >(in[0]);
@ -1088,18 +1150,18 @@ void load2D_94(int nargout, mxArray *out[], int nargin, const mxArray *in[])
out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false);
out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false);
}
void aGlobalFunction_95(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void aGlobalFunction_99(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("aGlobalFunction",nargout,nargin,0);
out[0] = wrap< Vector >(aGlobalFunction());
}
void overloadedGlobalFunction_96(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void overloadedGlobalFunction_100(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("overloadedGlobalFunction",nargout,nargin,1);
int a = unwrap< int >(in[0]);
out[0] = wrap< Vector >(overloadedGlobalFunction(a));
}
void overloadedGlobalFunction_97(int nargout, mxArray *out[], int nargin, const mxArray *in[])
void overloadedGlobalFunction_101(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("overloadedGlobalFunction",nargout,nargin,2);
int a = unwrap< int >(in[0]);
@ -1356,16 +1418,16 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
MyTemplateMatrix_Level_78(nargout, out, nargin-1, in+1);
break;
case 79:
PrimitiveRefdouble_collectorInsertAndMakeBase_79(nargout, out, nargin-1, in+1);
PrimitiveRefDouble_collectorInsertAndMakeBase_79(nargout, out, nargin-1, in+1);
break;
case 80:
PrimitiveRefdouble_constructor_80(nargout, out, nargin-1, in+1);
PrimitiveRefDouble_constructor_80(nargout, out, nargin-1, in+1);
break;
case 81:
PrimitiveRefdouble_deconstructor_81(nargout, out, nargin-1, in+1);
PrimitiveRefDouble_deconstructor_81(nargout, out, nargin-1, in+1);
break;
case 82:
PrimitiveRefdouble_Brutal_82(nargout, out, nargin-1, in+1);
PrimitiveRefDouble_Brutal_82(nargout, out, nargin-1, in+1);
break;
case 83:
MyVector3_collectorInsertAndMakeBase_83(nargout, out, nargin-1, in+1);
@ -1386,31 +1448,43 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
MyVector12_deconstructor_88(nargout, out, nargin-1, in+1);
break;
case 89:
MyFactorPosePoint2_collectorInsertAndMakeBase_89(nargout, out, nargin-1, in+1);
MultipleTemplatesIntDouble_collectorInsertAndMakeBase_89(nargout, out, nargin-1, in+1);
break;
case 90:
MyFactorPosePoint2_constructor_90(nargout, out, nargin-1, in+1);
MultipleTemplatesIntDouble_deconstructor_90(nargout, out, nargin-1, in+1);
break;
case 91:
MyFactorPosePoint2_deconstructor_91(nargout, out, nargin-1, in+1);
MultipleTemplatesIntFloat_collectorInsertAndMakeBase_91(nargout, out, nargin-1, in+1);
break;
case 92:
load2D_92(nargout, out, nargin-1, in+1);
MultipleTemplatesIntFloat_deconstructor_92(nargout, out, nargin-1, in+1);
break;
case 93:
load2D_93(nargout, out, nargin-1, in+1);
MyFactorPosePoint2_collectorInsertAndMakeBase_93(nargout, out, nargin-1, in+1);
break;
case 94:
load2D_94(nargout, out, nargin-1, in+1);
MyFactorPosePoint2_constructor_94(nargout, out, nargin-1, in+1);
break;
case 95:
aGlobalFunction_95(nargout, out, nargin-1, in+1);
MyFactorPosePoint2_deconstructor_95(nargout, out, nargin-1, in+1);
break;
case 96:
overloadedGlobalFunction_96(nargout, out, nargin-1, in+1);
load2D_96(nargout, out, nargin-1, in+1);
break;
case 97:
overloadedGlobalFunction_97(nargout, out, nargin-1, in+1);
load2D_97(nargout, out, nargin-1, in+1);
break;
case 98:
load2D_98(nargout, out, nargin-1, in+1);
break;
case 99:
aGlobalFunction_99(nargout, out, nargin-1, in+1);
break;
case 100:
overloadedGlobalFunction_100(nargout, out, nargin-1, in+1);
break;
case 101:
overloadedGlobalFunction_101(nargout, out, nargin-1, in+1);
break;
}
} catch(const std::exception& e) {

6
wrap/tests/expected-matlab/load2D.m
View File

@ -1,10 +1,10 @@
function varargout = load2D(varargin)
if length(varargin) == 5 && isa(varargin{1},'char') && isa(varargin{2},'Test') && isa(varargin{3},'numeric') && isa(varargin{4},'logical') && isa(varargin{5},'logical')
[ varargout{1} varargout{2} ] = geometry_wrapper(92, varargin{:});
[ varargout{1} varargout{2} ] = geometry_wrapper(96, varargin{:});
elseif length(varargin) == 5 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal') && isa(varargin{3},'numeric') && isa(varargin{4},'logical') && isa(varargin{5},'logical')
[ varargout{1} varargout{2} ] = geometry_wrapper(93, varargin{:});
[ varargout{1} varargout{2} ] = geometry_wrapper(97, varargin{:});
elseif length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal')
[ varargout{1} varargout{2} ] = geometry_wrapper(94, varargin{:});
[ varargout{1} varargout{2} ] = geometry_wrapper(98, varargin{:});
else
error('Arguments do not match any overload of function load2D');
end

4
wrap/tests/expected-matlab/overloadedGlobalFunction.m
View File

@ -1,8 +1,8 @@
function varargout = overloadedGlobalFunction(varargin)
if length(varargin) == 1 && isa(varargin{1},'numeric')
varargout{1} = geometry_wrapper(96, varargin{:});
varargout{1} = geometry_wrapper(100, varargin{:});
elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
varargout{1} = geometry_wrapper(97, varargin{:});
varargout{1} = geometry_wrapper(101, varargin{:});
else
error('Arguments do not match any overload of function overloadedGlobalFunction');
end

31
wrap/tests/expected-python/geometry_pybind.cpp
View File

@ -47,6 +47,17 @@ PYBIND11_MODULE(geometry_py, m_) {
[](gtsam::Point2* self, string serialized){
gtsam::deserialize(serialized, *self);
}, py::arg("serialized"))
.def(py::pickle(
[](const gtsam::Point2 &a){ // __getstate__
/* Returns a string that encodes the state of the object */
return py::make_tuple(gtsam::serialize(a));
},
[](py::tuple t){ // __setstate__
gtsam::Point2 obj;
gtsam::deserialize(t[0].cast<std::string>(), obj);
return obj;
}))
;
py::class_<gtsam::Point3, std::shared_ptr<gtsam::Point3>>(m_gtsam, "Point3")
@ -62,6 +73,17 @@ PYBIND11_MODULE(geometry_py, m_) {
gtsam::deserialize(serialized, *self);
}, py::arg("serialized"))
.def(py::pickle(
[](const gtsam::Point3 &a){ // __getstate__
/* Returns a string that encodes the state of the object */
return py::make_tuple(gtsam::serialize(a));
},
[](py::tuple t){ // __setstate__
gtsam::Point3 obj;
gtsam::deserialize(t[0].cast<std::string>(), obj);
return obj;
}))
.def_static("staticFunction",[](){return gtsam::Point3::staticFunction();})
.def_static("StaticFunctionRet",[]( double z){return gtsam::Point3::StaticFunctionRet(z);}, py::arg("z"));
@ -93,7 +115,8 @@ PYBIND11_MODULE(geometry_py, m_) {
gtsam::RedirectCout redirect;
a.print();
return redirect.str();
});
})
.def_readwrite("model_ptr", &Test::model_ptr);
py::class_<MyBase, std::shared_ptr<MyBase>>(m_, "MyBase");
@ -127,7 +150,7 @@ PYBIND11_MODULE(geometry_py, m_) {
.def("return_ptrs",[](MyTemplate<gtsam::Matrix>* self,const std::shared_ptr<gtsam::Matrix>& p1,const std::shared_ptr<gtsam::Matrix>& p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
.def_static("Level",[](const gtsam::Matrix& K){return MyTemplate<gtsam::Matrix>::Level(K);}, py::arg("K"));
py::class_<PrimitiveRef<double>, std::shared_ptr<PrimitiveRef<double>>>(m_, "PrimitiveRefdouble")
py::class_<PrimitiveRef<double>, std::shared_ptr<PrimitiveRef<double>>>(m_, "PrimitiveRefDouble")
.def(py::init<>())
.def_static("Brutal",[](const double& t){return PrimitiveRef<double>::Brutal(t);}, py::arg("t"));
@ -137,6 +160,10 @@ PYBIND11_MODULE(geometry_py, m_) {
py::class_<MyVector<12>, std::shared_ptr<MyVector<12>>>(m_, "MyVector12")
.def(py::init<>());
py::class_<MultipleTemplates<int, double>, std::shared_ptr<MultipleTemplates<int, double>>>(m_, "MultipleTemplatesIntDouble");
py::class_<MultipleTemplates<int, float>, std::shared_ptr<MultipleTemplates<int, float>>>(m_, "MultipleTemplatesIntFloat");
py::class_<MyFactor<gtsam::Pose2, gtsam::Matrix>, std::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>>(m_, "MyFactorPosePoint2")
.def(py::init< size_t, size_t, double, const std::shared_ptr<gtsam::noiseModel::Base>&>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel"));

13
wrap/tests/geometry.h
View File

@ -22,6 +22,9 @@ class Point2 {
VectorNotEigen vectorConfusion();
void serializable() const; // Sets flag and creates export, but does not make serialization functions
// enable pickling in python
void pickle() const;
};
#include <gtsam/geometry/Point3.h>
@ -35,6 +38,9 @@ class Point3 {
// enabling serialization functionality
void serialize() const; // Just triggers a flag internally and removes actual function
// enable pickling in python
void pickle() const;
};
}
@ -55,6 +61,9 @@ class Test {
// another comment
Test();
// Test a shared ptr property
gtsam::noiseModel::Base* model_ptr;
pair<Vector,Matrix> return_pair (Vector v, Matrix A) const; // intentionally the first method
pair<Vector,Matrix> return_pair (Vector v) const; // overload
@ -154,3 +163,7 @@ class MyVector {
// comments at the end!
// even more comments at the end!
// Class with multiple instantiated templates
template<T = {int}, U = {double, float}>
class MultipleTemplates {};

2
wrap/tests/pybind_wrapper.tpl
View File

@ -11,7 +11,7 @@
{boost_class_export}
{hoder_type}
{holder_type}
using namespace std;

28
wrap/tests/test_matlab_wrapper.py
View File

@ -1,21 +1,26 @@
"""
Unit test for Matlab wrap program
Author: Matthew Sklar
Unit tests for Matlab wrap program
Author: Matthew Sklar, Varun Agrawal
Date: March 2019
"""
# pylint: disable=import-error, wrong-import-position, too-many-branches
import filecmp
import os
import sys
import unittest
import filecmp
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
import gtwrap.template_instantiator as instantiator
import gtwrap.interface_parser as parser
import gtwrap.template_instantiator as instantiator
from gtwrap.matlab_wrapper import MatlabWrapper
class TestWrap(unittest.TestCase):
"""
Test the Matlab wrapper
"""
TEST_DIR = os.path.dirname(os.path.realpath(__file__)) + "/"
MATLAB_TEST_DIR = TEST_DIR + "expected-matlab/"
MATLAB_ACTUAL_DIR = TEST_DIR + "actual-matlab/"
@ -31,11 +36,11 @@ class TestWrap(unittest.TestCase):
"""
if path == '':
path = self.MATLAB_ACTUAL_DIR
for c in cc_content:
if type(c) == list:
if isinstance(c, list):
if len(c) == 0:
continue
import sys
print("c object: {}".format(c[0][0]), file=sys.stderr)
path_to_folder = path + '/' + c[0][0]
@ -46,13 +51,12 @@ class TestWrap(unittest.TestCase):
pass
for sub_content in c:
import sys
print("sub object: {}".format(sub_content[1][0][0]), file=sys.stderr)
self.generate_content(sub_content[1], path_to_folder)
elif type(c[1]) == list:
elif isinstance(c[1], list):
path_to_folder = path + '/' + c[0]
import sys
print("[generate_content_global]: {}".format(path_to_folder), file=sys.stderr)
if not os.path.isdir(path_to_folder):
try:
@ -60,15 +64,14 @@ class TestWrap(unittest.TestCase):
except OSError:
pass
for sub_content in c[1]:
import sys
path_to_file = path_to_folder + '/' + sub_content[0]
print("[generate_global_method]: {}".format(path_to_file), file=sys.stderr)
with open(path_to_file, 'w') as f:
f.write(sub_content[1])
else:
path_to_file = path + '/' + c[0]
import sys
print("[generate_content]: {}".format(path_to_file), file=sys.stderr)
if not os.path.isdir(path_to_file):
try:
@ -80,7 +83,8 @@ class TestWrap(unittest.TestCase):
f.write(c[1])
def test_geometry_matlab(self):
""" Check generation of matlab geometry wrapper.
"""
Check generation of matlab geometry wrapper.
python3 wrap/matlab_wrapper.py --src wrap/tests/geometry.h
--module_name geometry --out wrap/tests/actual-matlab
"""

14
wrap/tests/test_pybind_wrapper.py
View File

@ -1,27 +1,29 @@
"""
Unit test for Pybind wrap program
Author: Matthew Sklar
Author: Matthew Sklar, Varun Agrawal
Date: February 2019
"""
# pylint: disable=import-error, wrong-import-position, too-many-branches
import filecmp
import os
import os.path as path
import sys
import unittest
import filecmp
import os.path as path
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
sys.path.append(os.path.normpath(os.path.abspath(os.path.join(__file__, '../../../build/wrap'))))
from gtwrap.pybind_wrapper import PybindWrapper
import gtwrap.interface_parser as parser
import gtwrap.template_instantiator as instantiator
from gtwrap.pybind_wrapper import PybindWrapper
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
class TestWrap(unittest.TestCase):
"""Tests for Python wrapper based on Pybind11."""
TEST_DIR = os.path.dirname(os.path.realpath(__file__)) + "/"
def test_geometry_python(self):
@ -39,7 +41,7 @@ class TestWrap(unittest.TestCase):
with open(self.TEST_DIR + "pybind_wrapper.tpl") as template_file:
module_template = template_file.read()
# Create Pybind wrapper instance
wrapper = PybindWrapper(
module=module,