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

Merge branch 'develop' into fix/planeFactor

release/4.3a0
Frank Dellaert 2021-01-21 10:09:09 -05:00
parent b9a8101aa4 d267368b28
commit b5789f6b80
176 changed files with 8818 additions and 5432 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3563
.cproject
View File

File diff suppressed because it is too large Load Diff

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

@ -82,4 +82,4 @@ jobs:
- name: Build and Test (Linux)
if: runner.os == 'Linux'
run: |
bash .github/scripts/unix.sh -t
bash .github/scripts/unix.sh -t

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

@ -75,4 +75,4 @@ jobs:
cmake --build build --config ${{ matrix.build_type }} --target wrap
cmake --build build --config ${{ matrix.build_type }} --target check.base
cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable
cmake --build build --config ${{ matrix.build_type }} --target check.linear
cmake --build build --config ${{ matrix.build_type }} --target check.linear

7
CMakeLists.txt
View File

@ -34,7 +34,7 @@ include(GtsamTesting)
include(GtsamPrinting)
# guard against in-source builds
if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
if(${GTSAM_SOURCE_DIR} STREQUAL ${GTSAM_BINARY_DIR})
message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
endif()
@ -62,6 +62,11 @@ 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()

31
INSTALL.md
View File

@ -70,7 +70,34 @@ execute commands as follows for an out-of-source build:
This will build the library and unit tests, run all of the unit tests,
and then install the library itself.
## CMake Configuration Options and Details
# Windows Installation
This section details how to build a GTSAM `.sln` file using Visual Studio.
### Prerequisites
- Visual Studio with C++ CMake tools for Windows
- All the other pre-requisites listed above.
### Steps
1. Open Visual Studio.
2. Select `Open a local folder` and select the GTSAM source directory.
3. Go to `Project -> CMake Settings`.
- (Optional) Set `Configuration name`.
- (Optional) Set `Configuration type`.
- Set the `Toolset` to `msvc_x64_x64`. If you know what toolset you require, then skip this step.
- Update the `Build root` to `${projectDir}\build\${name}`.
- You can optionally create a new configuration for a `Release` build.
- Set the necessary CMake variables for your use case.
- Click on `Show advanced settings`.
- For `CMake generator`, select a version which matches `Visual Studio <Version> <Year> Win64`, e.g. `Visual Studio 16 2019 Win64`.
- Save the settings (Ctrl + S).
4. Click on `Project -> Generate Cache`. This will generate the CMake build files (as seen in the Output window).
5. The last step will generate a `GTSAM.sln` file in the `build` directory. At this point, GTSAM can be used as a regular Visual Studio project.
# CMake Configuration Options and Details
GTSAM has a number of options that can be configured, which is best done with
one of the following:
@ -78,7 +105,7 @@ one of the following:
- ccmake the curses GUI for cmake
- cmake-gui a real GUI for cmake
### Important Options:
## Important Options:
#### CMAKE_BUILD_TYPE
We support several build configurations for GTSAM (case insensitive)

4
cmake/Config.cmake.in
View File

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

6
cmake/GtsamBuildTypes.cmake
View File

@ -204,9 +204,9 @@ endif()
# Make common binary output directory when on Windows
if(WIN32)
set(RUNTIME_OUTPUT_PATH "${CMAKE_BINARY_DIR}/bin")
set(EXECUTABLE_OUTPUT_PATH "${CMAKE_BINARY_DIR}/bin")
set(LIBRARY_OUTPUT_PATH "${CMAKE_BINARY_DIR}/lib")
set(RUNTIME_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
set(EXECUTABLE_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
set(LIBRARY_OUTPUT_PATH "${GTSAM_BINARY_DIR}/lib")
endif()
# Set up build type list for cmake-gui

110
cmake/GtsamMatlabWrap.cmake
View File

@ -1,51 +1,64 @@
# Check / set dependent variables for MATLAB wrapper
if(GTSAM_INSTALL_MATLAB_TOOLBOX)
find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED)
if(NOT Matlab_MEX_COMPILER)
message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.")
endif()
if(GTSAM_BUILD_TYPE_POSTFIXES)
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
endif()
if(NOT BUILD_SHARED_LIBS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
endif()
endif()
# Set up cache options
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")
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).")
message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
endif()
# Try to automatically configure mex path
if(APPLE)
file(GLOB matlab_bin_directories "/Applications/MATLAB*/bin")
set(mex_program_name "mex")
elseif(WIN32)
file(GLOB matlab_bin_directories "C:/Program Files*/MATLAB/*/bin")
set(mex_program_name "mex.bat")
else()
file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
set(mex_program_name "mex")
endif()
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_directories ${GTSAM_CUSTOM_MATLAB_PATH})
endif()
set(matlab_bin_directory ${GTSAM_CUSTOM_MATLAB_PATH})
# Run find_program explicitly putting $PATH after our predefined program
# directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
# on the system path.
list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred
find_program(MEX_COMMAND ${mex_program_name}
PATHS ${matlab_bin_directories} ENV PATH
NO_DEFAULT_PATH)
mark_as_advanced(FORCE MEX_COMMAND)
# Now that we have mex, trace back to find the Matlab installation root
get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
get_filename_component(mex_path "${MEX_COMMAND}" PATH)
if(mex_path MATCHES ".*/win64$")
get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
else()
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
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()
set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
# User-friendly wrapping function. Builds a mex module from the provided
@ -133,7 +146,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
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)
@ -146,7 +159,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
endif()
endif()
endif()
#message("AUTOMATIC DEPENDENCIES: ${automaticDependencies}")
## CHRIS: End temporary fix
@ -200,7 +213,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
endif()
endif()
endforeach()
# Check libraries for conflicting versions built-in to MATLAB
set(dependentLibraries "")
if(NOT "${otherLibraryTargets}" STREQUAL "")
@ -227,12 +240,16 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
set(_ignore gtsam::Point2
gtsam::Point3)
add_custom_command(
# 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}
${CMAKE_SOURCE_DIR}/wrap/matlab_wrapper.py
${MATLAB_WRAP_SCRIPT}
--src ${interfaceHeader}
--module_name ${moduleName}
--out ${generated_files_path}
@ -240,7 +257,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
--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}")
@ -278,8 +295,8 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
endif()
# Hacking around output issue with custom command
# Deletes generated build folder
add_custom_target(wrap_${moduleName}_matlab_distclean
# 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()
@ -330,17 +347,17 @@ function(check_conflicting_libraries_internal libraries)
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})
@ -362,10 +379,10 @@ function(check_conflicting_libraries_internal libraries)
endif()
endif()
endforeach()
# Remove duplicates
list(REMOVE_DUPLICATES linkLibNames)
set(conflictingLibs "")
foreach(lib ${linkLibNames})
list(FIND matlabLibNames "${lib}" libPos)
@ -376,7 +393,7 @@ function(check_conflicting_libraries_internal libraries)
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. "
@ -418,4 +435,3 @@ function(install_matlab_scripts source_directory patterns)
endif()
endfunction()

50
cmake/GtsamTesting.cmake
View File

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

2
cmake/HandleEigen.cmake
View File

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

14
cmake/HandleGeneralOptions.cmake
View File

@ -24,6 +24,7 @@ option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available"
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF)
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
@ -40,16 +41,5 @@ elseif(GTSAM_ROT3_EXPMAP)
set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE)
endif()
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
# Set the default Python version. This is later updated in HandlePython.cmake.
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
# Check / set dependent variables for MATLAB wrapper
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
endif()

50
cmake/HandlePython.cmake
View File

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

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

File diff suppressed because it is too large Load Diff

2
examples/ImuFactorsExample.cpp
View File

@ -125,7 +125,7 @@ int main(int argc, char* argv[]) {
output_filename = var_map["output_filename"].as<string>();
use_isam = var_map["use_isam"].as<bool>();
ISAM2* isam2;
ISAM2* isam2 = 0;
if (use_isam) {
printf("Using ISAM2\n");
ISAM2Params parameters;

4
examples/Pose3Localization.cpp
View File

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

2
examples/Pose3SLAMExample_changeKeys.cpp
View File

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

2
examples/Pose3SLAMExample_g2o.cpp
View File

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

2
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

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

2
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

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

25
examples/ShonanAveragingCLI.cpp
View File

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

2
examples/SolverComparer.cpp
View File

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

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

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

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

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

16
gtsam/CMakeLists.txt
View File

@ -134,15 +134,15 @@ endif()
# of any previously installed GTSAM headers.
target_include_directories(gtsam BEFORE PUBLIC
# main gtsam includes:
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}>
$<INSTALL_INTERFACE:include/>
# config.h
$<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
$<BUILD_INTERFACE:${GTSAM_BINARY_DIR}>
# unit tests:
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/CppUnitLite>
)
# 3rdparty libraries: use the "system" flag so they are included via "-isystem"
# and warnings (and warnings-considered-errors) in those headers are not
# and warnings (and warnings-considered-errors) in those headers are not
# reported as warnings/errors in our targets:
target_include_directories(gtsam SYSTEM BEFORE PUBLIC
# SuiteSparse_config
@ -156,9 +156,9 @@ target_include_directories(gtsam SYSTEM BEFORE PUBLIC
)
if(GTSAM_SUPPORT_NESTED_DISSECTION)
target_include_directories(gtsam BEFORE PUBLIC
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/include>
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/include>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
$<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
$<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
)
endif()
@ -199,7 +199,7 @@ if(WIN32)
else()
if("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
# Suppress all warnings from 3rd party sources.
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w -Wno-everything")
else()
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
endif()

2
gtsam/base/Matrix.cpp
View File

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

2
gtsam/base/Matrix.h
View File

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

8
gtsam/base/Testable.h
View File

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

16
gtsam/base/Vector.cpp
View File

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

8
gtsam/base/Vector.h
View File

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

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

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

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

@ -0,0 +1,75 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Cal3.cpp
* @brief Common code for all calibration models.
* @author Frank Dellaert
*/
#include <gtsam/geometry/Cal3.h>
#include <cmath>
#include <fstream>
#include <iostream>
namespace gtsam {
/* ************************************************************************* */
Cal3::Cal3(double fov, int w, int h)
: s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) {
double a = fov * M_PI / 360.0; // fov/2 in radians
fx_ = double(w) / (2.0 * tan(a));
fy_ = fx_;
}
/* ************************************************************************* */
Cal3::Cal3(const std::string& path) {
const auto buffer = path + std::string("/calibration_info.txt");
std::ifstream infile(buffer, std::ios::in);
if (infile && !infile.eof()) {
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
} else {
throw std::runtime_error("Cal3: Unable to load the calibration");
}
infile.close();
}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3& cal) {
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py();
return os;
}
/* ************************************************************************* */
void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
/* ************************************************************************* */
bool Cal3::equals(const Cal3& K, double tol) const {
return (std::fabs(fx_ - K.fx_) < tol && std::fabs(fy_ - K.fy_) < tol &&
std::fabs(s_ - K.s_) < tol && std::fabs(u0_ - K.u0_) < tol &&
std::fabs(v0_ - K.v0_) < tol);
}
Matrix3 Cal3::inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
Matrix3 K_inverse;
K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
-v0_ / fy_, 0.0, 0.0, 1.0;
return K_inverse;
}
/* ************************************************************************* */
} // \ namespace gtsam

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

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

97
gtsam/geometry/Cal3Bundler.cpp
View File

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

138
gtsam/geometry/Cal3Bundler.h
View File

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

23
gtsam/geometry/Cal3DS2.cpp
View File

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

52
gtsam/geometry/Cal3DS2.h
View File

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

105
gtsam/geometry/Cal3DS2_Base.cpp
View File

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

124
gtsam/geometry/Cal3DS2_Base.h
View File

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

59
gtsam/geometry/Cal3Fisheye.cpp
View File

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

105
gtsam/geometry/Cal3Fisheye.h
View File

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

71
gtsam/geometry/Cal3Unified.cpp
View File

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

85
gtsam/geometry/Cal3Unified.h
View File

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

92
gtsam/geometry/Cal3_S2.cpp
View File

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

194
gtsam/geometry/Cal3_S2.h
View File

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

50
gtsam/geometry/Cal3_S2Stereo.cpp
View File

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

226
gtsam/geometry/Cal3_S2Stereo.h
View File

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

21
gtsam/geometry/Line3.h
View File

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

2
gtsam/geometry/Point3.h
View File

@ -61,7 +61,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
/// mean
template <class CONTAINER>
GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
Point3 mean(const CONTAINER& points) {
if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
Point3 sum(0, 0, 0);
sum = std::accumulate(points.begin(), points.end(), sum);

8
gtsam/geometry/Pose2.cpp
View File

@ -48,7 +48,13 @@ Matrix3 Pose2::matrix() const {
/* ************************************************************************* */
void Pose2::print(const string& s) const {
cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
cout << s << this << endl;
}
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose2& pose) {
os << "(" << pose.x() << ", " << pose.y() << ", " << pose.theta() << ")";
return os;
}
/* ************************************************************************* */

4
gtsam/geometry/Pose2.h
View File

@ -287,6 +287,10 @@ public:
*/
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
/// Output stream operator
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
/// @}
private:

4
gtsam/geometry/Pose3.cpp
View File

@ -106,8 +106,8 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
}
/* ************************************************************************* */
void Pose3::print(const string& s) const {
cout << (s.empty() ? s : s + " ") << *this << endl;
void Pose3::print(const std::string& s) const {
std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
}
/* ************************************************************************* */

19
gtsam/geometry/Pose3.h
View File

@ -112,6 +112,25 @@ public:
return Pose3(R_ * T.R_, t_ + R_ * T.t_);
}
/**
* Interpolate between two poses via individual rotation and translation
* interpolation.
*
* The default "interpolate" method defined in Lie.h minimizes the geodesic
* distance on the manifold, leading to a screw motion interpolation in
* Cartesian space, which might not be what is expected.
* In contrast, this method executes a straight line interpolation for the
* translation, while still using interpolate (aka "slerp") for the rotational
* component. This might be more intuitive in many applications.
*
* @param T End point of interpolation.
* @param t A value in [0, 1].
*/
Pose3 interpolateRt(const Pose3& T, double t) const {
return Pose3(interpolate<Rot3>(R_, T.R_, t),
interpolate<Point3>(t_, T.t_, t));
}
/// @}
/// @name Lie Group
/// @{

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

@ -22,8 +22,6 @@
#include <iostream>
using namespace std;
namespace gtsam {
// Implementation for N>=5 just uses dynamic version
@ -108,7 +106,7 @@ typename SO<N>::VectorN2 SO<N>::vec(
template <int N>
void SO<N>::print(const std::string& s) const {
cout << s << matrix_ << endl;
std::cout << s << matrix_ << std::endl;
}
} // namespace gtsam

2
gtsam/geometry/SimpleCamera.cpp
View File

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

16
gtsam/geometry/SimpleCamera.h
View File

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

98
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -11,11 +11,12 @@
/**
* @file testCal3Bundler.cpp
* @brief Unit tests for transform derivatives
* @brief Unit tests for Bundler calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Bundler.h>
@ -25,30 +26,27 @@ GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
static Point2 p(2,3);
static Point2 p(2, 3);
/* ************************************************************************* */
TEST( Cal3Bundler, vector)
{
TEST(Cal3Bundler, vector) {
Cal3Bundler K;
Vector expected(3);
expected << 1, 0, 0;
CHECK(assert_equal(expected,K.vector()));
CHECK(assert_equal(expected, K.vector()));
}
/* ************************************************************************* */
TEST( Cal3Bundler, uncalibrate)
{
Vector v = K.vector() ;
double r = p.x()*p.x() + p.y()*p.y() ;
double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
Point2 expected (1000+g*p.x(), 2000+g*p.y()) ;
TEST(Cal3Bundler, uncalibrate) {
Vector v = K.vector();
double r = p.x() * p.x() + p.y() * p.y();
double g = v[0] * (1 + v[1] * r + v[2] * r * r);
Point2 expected(1000 + g * p.x(), 2000 + g * p.y());
Point2 actual = K.uncalibrate(p);
CHECK(assert_equal(expected,actual));
CHECK(assert_equal(expected, actual));
}
TEST( Cal3Bundler, calibrate )
{
TEST(Cal3Bundler, calibrate) {
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
@ -56,26 +54,28 @@ TEST( Cal3Bundler, calibrate )
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) {
return k.uncalibrate(pt);
}
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
/* ************************************************************************* */
TEST( Cal3Bundler, Duncalibrate)
{
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(2182, 3773);
CHECK(assert_equal(expected,actual,1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1,Dcal,1e-7));
CHECK(assert_equal(numerical2,Dp,1e-7));
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3Bundler, Dcalibrate)
{
TEST(Cal3Bundler, Duncalibrate) {
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(2182, 3773);
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1, Dcal, 1e-7));
CHECK(assert_equal(numerical2, Dp, 1e-7));
}
/* ************************************************************************* */
TEST(Cal3Bundler, Dcalibrate) {
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
@ -83,27 +83,41 @@ TEST( Cal3Bundler, Dcalibrate)
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5));
CHECK(assert_equal(numerical2,Dp,1e-5));
CHECK(assert_equal(numerical1, Dcal, 1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST( Cal3Bundler, assert_equal)
{
CHECK(assert_equal(K,K,1e-7));
}
TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
/* ************************************************************************* */
TEST( Cal3Bundler, retract)
{
TEST(Cal3Bundler, retract) {
Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
Vector d(3);
EXPECT_LONGS_EQUAL(3, expected.dim());
EXPECT_LONGS_EQUAL(Cal3Bundler::Dim(), 3);
EXPECT_LONGS_EQUAL(expected.dim(), 3);
Vector3 d;
d << 10, 1e-3, 1e-3;
Cal3Bundler actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
TEST(Cal3_S2, Print) {
Cal3Bundler cal(1, 2, 3, 4, 5);
std::stringstream os;
os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
<< ", px: " << cal.px() << ", py: " << cal.py();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

36
gtsam/geometry/tests/testCal3DFisheye.cpp
View File

@ -10,12 +10,13 @@
* -------------------------------------------------------------------------- */
/**
* @file testCal3DFisheye.cpp
* @file testCal3Fisheye.cpp
* @brief Unit tests for fisheye calibration class
* @author ghaggin
*/
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Point3.h>
@ -41,7 +42,11 @@ TEST(Cal3Fisheye, retract) {
Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
K.k4() + 9);
Vector d(9);
EXPECT_LONGS_EQUAL(Cal3Fisheye::Dim(), 9);
EXPECT_LONGS_EQUAL(expected.dim(), 9);
Vector9 d;
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Cal3Fisheye actual = K.retract(d);
CHECK(assert_equal(expected, actual, 1e-7));
@ -181,6 +186,33 @@ TEST(Cal3Fisheye, calibrate3) {
CHECK(assert_equal(xi_hat, xi));
}
Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Dcalibrate) {
Point2 p(0.5, 0.5);
Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp;
K.calibrate(pi, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
CHECK(assert_equal(numerical1, Dcal, 1e-5));
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Print) {
Cal3Fisheye cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
<< ", k2: " << cal.k2() << ", k3: " << cal.k3() << ", k4: " << cal.k4();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;

108
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -11,12 +11,12 @@
/**
* @file testCal3DS2.cpp
* @brief Unit tests for transform derivatives
* @brief Unit tests for Cal3DS2 calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3DS2.h>
@ -25,73 +25,103 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2)
static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
static Point2 p(2,3);
static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
4.0 * 1e-3);
static Point2 p(2, 3);
/* ************************************************************************* */
TEST( Cal3DS2, uncalibrate)
{
Vector k = K.k() ;
double r = p.x()*p.x() + p.y()*p.y() ;
double g = 1+k[0]*r+k[1]*r*r ;
double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ;
double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0).finished();
Vector v_i = K.K() * v_hat ;
Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
TEST(Cal3DS2, Uncalibrate) {
Vector k = K.k();
double r = p.x() * p.x() + p.y() * p.y();
double g = 1 + k[0] * r + k[1] * r * r;
double tx = 2 * k[2] * p.x() * p.y() + k[3] * (r + 2 * p.x() * p.x());
double ty = k[2] * (r + 2 * p.y() * p.y()) + 2 * k[3] * p.x() * p.y();
Vector v_hat = (Vector(3) << g * p.x() + tx, g * p.y() + ty, 1.0).finished();
Vector v_i = K.K() * v_hat;
Point2 p_i(v_i(0) / v_i(2), v_i(1) / v_i(2));
Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i));
CHECK(assert_equal(q, p_i));
}
TEST( Cal3DS2, calibrate )
{
TEST(Cal3DS2, Calibrate) {
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
}
Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) {
return k.uncalibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3DS2, Duncalibrate1)
{
TEST(Cal3DS2, Duncalibrate1) {
Matrix computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5));
CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_calibration(p);
CHECK(assert_equal(numerical,separate,1e-5));
CHECK(assert_equal(numerical, separate, 1e-5));
}
/* ************************************************************************* */
TEST( Cal3DS2, Duncalibrate2)
{
Matrix computed; K.uncalibrate(p, boost::none, computed);
TEST(Cal3DS2, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5));
CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_intrinsic(p);
CHECK(assert_equal(numerical,separate,1e-5));
CHECK(assert_equal(numerical, separate, 1e-5));
}
Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3DS2, assert_equal)
{
CHECK(assert_equal(K,K,1e-5));
TEST(Cal3DS2, Dcalibrate) {
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Matrix Dcal, Dp;
K.calibrate(pi, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi, 1e-7);
CHECK(assert_equal(numerical1, Dcal, 1e-5));
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi, 1e-7);
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST( Cal3DS2, retract)
{
TEST(Cal3DS2, Equal) { CHECK(assert_equal(K, K, 1e-5)); }
/* ************************************************************************* */
TEST(Cal3DS2, Retract) {
Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
Vector d(9);
d << 1,2,3,4,5,6,7,8,9;
2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9);
EXPECT_LONGS_EQUAL(expected.dim(), 9);
Vector9 d;
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Cal3DS2 actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
TEST(Cal3DS2, Print) {
Cal3DS2 cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
<< ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

97
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -10,17 +10,18 @@
* -------------------------------------------------------------------------- */
/**
* @file testCal3Unify.cpp
* @brief Unit tests for transform derivatives
* @file testCal3Unified.cpp
* @brief Unit tests for Cal3Unified calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/Values.h>
using namespace gtsam;
@ -35,74 +36,87 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox
*/
static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1);
static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
4.0 * 1e-3, 0.1);
static Point2 p(0.5, 0.7);
/* ************************************************************************* */
TEST( Cal3Unified, uncalibrate)
{
Point2 p_i(364.7791831734982, 305.6677211952602) ;
TEST(Cal3Unified, Uncalibrate) {
Point2 p_i(364.7791831734982, 305.6677211952602);
Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i));
CHECK(assert_equal(q, p_i));
}
/* ************************************************************************* */
TEST( Cal3Unified, spaceNplane)
{
TEST(Cal3Unified, SpaceNplane) {
Point2 q = K.spaceToNPlane(p);
CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
CHECK(assert_equal(p, K.nPlaneToSpace(q)));
}
/* ************************************************************************* */
TEST( Cal3Unified, calibrate)
{
TEST(Cal3Unified, Calibrate) {
Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi);
CHECK( traits<Point2>::Equals(p, pn_hat, 1e-8));
CHECK(traits<Point2>::Equals(p, pn_hat, 1e-8));
}
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) {
return k.uncalibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3Unified, Duncalibrate1)
{
TEST(Cal3Unified, Duncalibrate1) {
Matrix computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-6));
CHECK(assert_equal(numerical, computed, 1e-6));
}
/* ************************************************************************* */
TEST( Cal3Unified, Duncalibrate2)
{
TEST(Cal3Unified, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-6));
CHECK(assert_equal(numerical, computed, 1e-6));
}
Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3Unified, assert_equal)
{
CHECK(assert_equal(K,K,1e-9));
TEST(Cal3Unified, Dcalibrate) {
Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp;
K.calibrate(pi, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
CHECK(assert_equal(numerical1, Dcal, 1e-5));
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST( Cal3Unified, retract)
{
Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
Vector d(10);
TEST(Cal3Unified, Equal) { CHECK(assert_equal(K, K, 1e-9)); }
/* ************************************************************************* */
TEST(Cal3Unified, Retract) {
Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7,
2.0 * 1e-3 + 8, 3.0 * 1e-3 + 9, 4.0 * 1e-3 + 10,
0.1 + 1);
EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10);
EXPECT_LONGS_EQUAL(expected.dim(), 10);
Vector10 d;
d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
Cal3Unified actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-9));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
CHECK(assert_equal(expected, actual, 1e-9));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-9));
}
/* ************************************************************************* */
TEST( Cal3Unified, DerivedValue)
{
TEST(Cal3Unified, DerivedValue) {
Values values;
Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
Key key = 1;
@ -110,9 +124,24 @@ TEST( Cal3Unified, DerivedValue)
Cal3Unified calafter = values.at<Cal3Unified>(key);
CHECK(assert_equal(cal,calafter,1e-9));
CHECK(assert_equal(cal, calafter, 1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
TEST(Cal3Unified, Print) {
Cal3Unified cal(0, 1, 2, 3, 4, 5, 6, 7, 8, 9);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
<< ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2()
<< ", xi: " << cal.xi();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -11,7 +11,7 @@
/**
* @file testCal3_S2.cpp
* @brief Unit tests for transform derivatives
* @brief Unit tests for basic Cal3_S2 calibration model.
*/
#include <CppUnitLite/TestHarness.h>
@ -31,90 +31,94 @@ static Point2 p_uv(1320.3, 1740);
static Point2 p_xy(2, 3);
/* ************************************************************************* */
TEST( Cal3_S2, easy_constructor)
{
TEST(Cal3_S2, Constructor) {
Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
double fov = 60; // degrees
size_t w=640,h=480;
Cal3_S2 actual(fov,w,h);
double fov = 60; // degrees
size_t w = 640, h = 480;
Cal3_S2 actual(fov, w, h);
CHECK(assert_equal(expected,actual,1e-3));
CHECK(assert_equal(expected, actual, 1e-3));
}
/* ************************************************************************* */
TEST( Cal3_S2, calibrate)
{
Point2 intrinsic(2,3);
TEST(Cal3_S2, Calibrate) {
Point2 intrinsic(2, 3);
Point2 expectedimage(1320.3, 1740);
Point2 imagecoordinates = K.uncalibrate(intrinsic);
CHECK(assert_equal(expectedimage,imagecoordinates));
CHECK(assert_equal(intrinsic,K.calibrate(imagecoordinates)));
CHECK(assert_equal(expectedimage, imagecoordinates));
CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
}
/* ************************************************************************* */
TEST( Cal3_S2, calibrate_homogeneous) {
TEST(Cal3_S2, CalibrateHomogeneous) {
Vector3 intrinsic(2, 3, 1);
Vector3 image(1320.3, 1740, 1);
CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image)));
CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
TEST( Cal3_S2, Duncalibrate1)
{
Matrix25 computed; K.uncalibrate(p, computed, boost::none);
Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
return k.uncalibrate(pt);
}
TEST(Cal3_S2, Duncalibrate1) {
Matrix25 computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
}
/* ************************************************************************* */
TEST( Cal3_S2, Duncalibrate2)
{
Matrix computed; K.uncalibrate(p, boost::none, computed);
TEST(Cal3_S2, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-9));
CHECK(assert_equal(numerical, computed, 1e-9));
}
Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); }
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate1)
{
Matrix computed;
Point2 expected = K.calibrate(p_uv, computed, boost::none);
Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate2)
{
Matrix computed;
Point2 expected = K.calibrate(p_uv, boost::none, computed);
Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
TEST(Cal3_S2, Dcalibrate1) {
Matrix computed;
Point2 expected = K.calibrate(p_uv, computed, boost::none);
Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
}
/* ************************************************************************* */
TEST( Cal3_S2, assert_equal)
{
CHECK(assert_equal(K,K,1e-9));
TEST(Cal3_S2, Dcalibrate2) {
Matrix computed;
Point2 expected = K.calibrate(p_uv, boost::none, computed);
Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
}
/* ************************************************************************* */
TEST(Cal3_S2, Equal) {
CHECK(assert_equal(K, K, 1e-9));
Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
CHECK(assert_equal(K,K1,1e-9));
CHECK(assert_equal(K, K1, 1e-9));
}
/* ************************************************************************* */
TEST( Cal3_S2, retract)
{
Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5);
Vector d(5);
d << 1,2,3,4,5;
TEST(Cal3_S2, Retract) {
Cal3_S2 expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5);
EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5);
EXPECT_LONGS_EQUAL(expected.dim(), 5);
Vector5 d;
d << 1, 2, 3, 4, 5;
Cal3_S2 actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
@ -122,18 +126,17 @@ TEST(Cal3_S2, between) {
Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9);
Matrix H1, H2;
EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
EXPECT(assert_equal(Cal3_S2(0, 1, 2, 3, 4), k1.between(k2, H1, H2)));
EXPECT(assert_equal(-I_5x5, H1));
EXPECT(assert_equal(I_5x5, H2));
}
/* ************************************************************************* */
TEST(Cal3_S2, Print) {
Cal3_S2 cal(5, 5, 5, 5, 5);
std::stringstream os;
os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
<< ", py:" << cal.py() << "}";
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py();
EXPECT(assert_stdout_equal(os.str(), cal));
}
@ -144,4 +147,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

129
gtsam/geometry/tests/testCal3_S2Stereo.cpp Normal file
View File

@ -0,0 +1,129 @@
/* ----------------------------------------------------------------------------
* 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 testCal3_S2Stereo.cpp
* @brief Unit tests for stereo-rig calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2Stereo)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2Stereo)
static Cal3_S2Stereo K(500, 500, 0.1, 640 / 2, 480 / 2, 1);
static Point2 p(1, -2);
static Point2 p_uv(1320.3, 1740);
static Point2 p_xy(2, 3);
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Constructor) {
Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
double fov = 60; // degrees
size_t w = 640, h = 480;
Cal3_S2Stereo actual(fov, w, h, 3);
CHECK(assert_equal(expected, actual, 1e-3));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Calibrate) {
Point2 intrinsic(2, 3);
Point2 expectedimage(1320.3, 1740);
Point2 imagecoordinates = K.uncalibrate(intrinsic);
CHECK(assert_equal(expectedimage, imagecoordinates));
CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, CalibrateHomogeneous) {
Vector3 intrinsic(2, 3, 1);
Vector3 image(1320.3, 1740, 1);
CHECK(assert_equal(intrinsic, K.calibrate(image)));
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
return k.uncalibrate(pt);
}
TEST(Cal3_S2Stereo, Duncalibrate) {
Matrix26 Dcal;
Matrix22 Dp;
K.uncalibrate(p, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical1, Dcal, 1e-8));
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical2, Dp, 1e-9));
}
Point2 calibrate_(const Cal3_S2Stereo& K, const Point2& pt) {
return K.calibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Dcalibrate) {
Matrix26 Dcal;
Matrix22 Dp;
Point2 expected = K.calibrate(p_uv, Dcal, Dp);
CHECK(assert_equal(expected, p_xy, 1e-8));
Matrix numerical1 = numericalDerivative21(calibrate_, K, p_uv);
CHECK(assert_equal(numerical1, Dcal, 1e-8));
Matrix numerical2 = numericalDerivative22(calibrate_, K, p_uv);
CHECK(assert_equal(numerical2, Dp, 1e-8));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Equal) {
CHECK(assert_equal(K, K, 1e-9));
Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1);
CHECK(assert_equal(K, K1, 1e-9));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Retract) {
Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
7);
EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6);
EXPECT_LONGS_EQUAL(expected.dim(), 6);
Vector6 d;
d << 1, 2, 3, 4, 5, 6;
Cal3_S2Stereo actual = K.retract(d);
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Print) {
Cal3_S2Stereo cal(5, 5, 5, 5, 5, 2);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py()
<< ", b: " << cal.baseline();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -14,6 +14,16 @@ GTSAM_CONCEPT_MANIFOLD_INST(Line3)
static const Line3 l(Rot3(), 1, 1);
// Testing getters
TEST(Line3, getMethods) {
const double a = 5, b = 10;
const Rot3 R = Rot3::Expmap(Vector3(0.1, 0.2, 0.3));
const Line3 line(R, a, b);
EXPECT_DOUBLES_EQUAL(a, line.a(), 1e-8);
EXPECT_DOUBLES_EQUAL(b, line.b(), 1e-8);
EXPECT(assert_equal(R, line.R(), 1e-8));
}
// Testing equals function of Line3
TEST(Line3, equals) {
Line3 l_same = l;

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

@ -1016,6 +1016,33 @@ TEST(Pose3, TransformCovariance6) {
TEST(Pose3, interpolate) {
EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0)));
EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0)));
// Trivial example: start at origin and move to (1, 0, 0) while rotating pi/2
// about z-axis.
Pose3 start;
Pose3 end(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
// This interpolation is easy to calculate by hand.
double t = 0.5;
Pose3 expected0(Rot3::Rz(M_PI_4), Point3(0.5, 0, 0));
EXPECT(assert_equal(expected0, start.interpolateRt(end, t)));
// Example from Peter Corke
// https://robotacademy.net.au/lesson/interpolating-pose-in-3d/
t = 0.0759; // corresponds to the 10th element when calling `ctraj` in
// the video
Pose3 O;
Pose3 F(Rot3::Roll(0.6).compose(Rot3::Pitch(0.8)).compose(Rot3::Yaw(1.4)),
Point3(1, 2, 3));
// The expected answer matches the result presented in the video.
Pose3 expected1(interpolate(O.rotation(), F.rotation(), t),
interpolate(O.translation(), F.translation(), t));
EXPECT(assert_equal(expected1, O.interpolateRt(F, t)));
// Non-trivial interpolation, translation value taken from output.
Pose3 expected2(interpolate(T2.rotation(), T3.rotation(), t),
interpolate(T2.translation(), T3.translation(), t));
EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t)));
}
/* ************************************************************************* */

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

@ -807,15 +807,15 @@ TEST(Rot3, RQ_derivative) {
test_xyz.push_back(VecAndErr{{0, 0, 0}, error});
test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error});
test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error});
test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, error});
test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, 1e-8});
test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error});
test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error});
test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error});
test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error});
// Test close to singularity
test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-8});
test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-8});
test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-7});
test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-7});
test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4});
test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4});
@ -825,7 +825,7 @@ TEST(Rot3, RQ_derivative) {
const auto R = Rot3::RzRyRx(xyz).matrix();
const auto num = numericalDerivative11(RQ_proxy, R);
Matrix39 calc;
RQ(R, calc).second;
auto dummy = RQ(R, calc).second;
const auto err = vec_err.second;
CHECK(assert_equal(num, calc, err));

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

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

8
gtsam/geometry/triangulation.h
View File

@ -18,8 +18,10 @@
#pragma once
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -494,5 +496,9 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
}
}
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
} // \namespace gtsam

136
gtsam/gtsam.i
View File

@ -329,7 +329,7 @@ virtual class Value {
};
#include <gtsam/base/GenericValue.h>
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
@ -852,8 +852,7 @@ class Cal3_S2 {
gtsam::Point2 principalPoint() const;
Vector vector() const;
Matrix K() const;
Matrix matrix() const;
Matrix matrix_inverse() const;
Matrix inverse() const;
// enabling serialization functionality
void serialize() const;
@ -881,7 +880,7 @@ virtual class Cal3DS2_Base {
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
// enabling serialization functionality
void serialize() const;
@ -1059,56 +1058,24 @@ class PinholeCamera {
void serialize() const;
};
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h>
virtual class SimpleCamera {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
const gtsam::Point3& upVector);
// Testable
void print(string s) const;
bool equals(const gtsam::SimpleCamera& camera, double tol) const;
// Standard Interface
gtsam::Pose3 pose() const;
gtsam::Cal3_S2 calibration() const;
// Manifold
gtsam::SimpleCamera retract(Vector d) const;
Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
size_t dim() const;
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
double range(const gtsam::Point3& point);
double range(const gtsam::Pose3& pose);
// enabling serialization functionality
void serialize() const;
};
gtsam::SimpleCamera simpleCamera(const Matrix& P);
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
template<T>
class CameraSet {
CameraSet();
// structure specific methods
T at(size_t i) const;
void push_back(const T& cam);
};
#include <gtsam/geometry/StereoCamera.h>
class StereoCamera {
// Standard Constructors and Named Constructors
@ -1140,7 +1107,7 @@ class StereoCamera {
#include <gtsam/geometry/triangulation.h>
// Templates appear not yet supported for free functions
// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
@ -1150,7 +1117,13 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
const gtsam::Point2Vector& measurements, double rank_tol,
bool optimize);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
const gtsam::Point2Vector& measurements, double rank_tol,
bool optimize);
//*************************************************************************
// Symbolic
//*************************************************************************
@ -1963,6 +1936,21 @@ class KalmanFilter {
//*************************************************************************
#include <gtsam/inference/Symbol.h>
class Symbol {
Symbol();
Symbol(char c, uint64_t j);
Symbol(size_t key);
size_t key() const;
void print(const string& s) const;
bool equals(const gtsam::Symbol& expected, double tol) const;
char chr() const;
uint64_t index() const;
string string() const;
};
size_t symbol(char chr, size_t index);
char symbolChr(size_t key);
size_t symbolIndex(size_t key);
@ -2069,7 +2057,7 @@ class NonlinearFactorGraph {
gtsam::KeySet keys() const;
gtsam::KeyVector keyVector() const;
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
// NonlinearFactorGraph
@ -2083,6 +2071,7 @@ class NonlinearFactorGraph {
// enabling serialization functionality
void serialize() const;
void saveGraph(const string& s) const;
};
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -2493,7 +2482,7 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
@ -2527,12 +2516,11 @@ class NonlinearISAM {
//*************************************************************************
// Nonlinear factor types
//*************************************************************************
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/nonlinear/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2556,7 +2544,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2,
gtsam::PinholeCameraCal3_S2,
gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation
@ -2611,6 +2599,7 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
};
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
typedef gtsam::BearingFactor<gtsam::Pose3, gtsam::Point3, gtsam::Unit3> BearingFactor3D;
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2> BearingFactorPose2;
#include <gtsam/geometry/BearingRange.h>
@ -2634,6 +2623,8 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
const BEARING& measuredBearing, const RANGE& measuredRange,
const gtsam::noiseModel::Base* noiseModel);
BearingRange<POSE, POINT, BEARING, RANGE> measured() const;
// enabling serialization functionality
void serialize() const;
};
@ -2675,11 +2666,10 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
gtsam::Point2 measured() const;
};
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
template<CALIBRATION = {gtsam::Cal3_S2}>
template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey);
gtsam::Point2 measured() const;
@ -2780,6 +2770,12 @@ class SfmTrack {
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;
void add_measurement(size_t idx, const gtsam::Point2& m);
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmTrack& expected, double tol) const;
};
class SfmData {
@ -2790,6 +2786,12 @@ class SfmData {
gtsam::SfmTrack track(size_t idx) const;
void add_track(const gtsam::SfmTrack& t) ;
void add_camera(const gtsam::SfmCamera& cam);
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmData& expected, double tol) const;
};
gtsam::SfmData readBal(string filename);
@ -2933,9 +2935,14 @@ class ShonanAveragingParameters2 {
void setAnchorWeight(double value);
double getAnchorWeight() const;
void setKarcherWeight(double value);
double getKarcherWeight();
double getKarcherWeight() const;
void setGaugesWeight(double value);
double getGaugesWeight();
double getGaugesWeight() const;
void setUseHuber(bool value);
bool getUseHuber() const;
void setCertifyOptimality(bool value);
bool getCertifyOptimality() const;
void print() const;
};
class ShonanAveragingParameters3 {
@ -2949,9 +2956,14 @@ class ShonanAveragingParameters3 {
void setAnchorWeight(double value);
double getAnchorWeight() const;
void setKarcherWeight(double value);
double getKarcherWeight();
double getKarcherWeight() const;
void setGaugesWeight(double value);
double getGaugesWeight();
double getGaugesWeight() const;
void setUseHuber(bool value);
bool getUseHuber() const;
void setCertifyOptimality(bool value);
bool getCertifyOptimality() const;
void print() const;
};
class ShonanAveraging2 {

8
gtsam/inference/Symbol.h
View File

@ -80,6 +80,9 @@ public:
/** Create a string from the key */
operator std::string() const;
/// Return string representation of the key
std::string string() const { return std::string(*this); };
/** Comparison for use in maps */
bool operator<(const Symbol& comp) const {
return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
@ -167,10 +170,11 @@ inline Key Z(std::uint64_t j) { return Symbol('z', j); }
/** Generates symbol shorthands with alternative names different than the
* one-letter predefined ones. */
class SymbolGenerator {
const char c_;
const unsigned char c_;
public:
SymbolGenerator(const char c) : c_(c) {}
constexpr SymbolGenerator(const unsigned char c) : c_(c) {}
Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); }
constexpr unsigned char chr() const { return c_; }
};
/// traits

6
gtsam/inference/tests/testKey.cpp
View File

@ -59,6 +59,12 @@ TEST(Key, SymbolGenerator) {
EXPECT(assert_equal(a1, ddz1));
}
/* ************************************************************************* */
TEST(Key, SymbolGeneratorConstexpr) {
constexpr auto Z = gtsam::SymbolGenerator('x');
EXPECT(assert_equal(Z.chr(), 'x'));
}
/* ************************************************************************* */
template<int KeySize>
Key KeyTestValue();

176
gtsam/linear/AcceleratedPowerMethod.h Normal file
View File

@ -0,0 +1,176 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file AcceleratedPowerMethod.h
* @date Sept 2020
* @author Jing Wu
* @brief accelerated power method for fast eigenvalue and eigenvector
* computation
*/
#pragma once
#include <gtsam/linear/PowerMethod.h>
namespace gtsam {
using Sparse = Eigen::SparseMatrix<double>;
/**
* \brief Compute maximum Eigenpair with accelerated power method
*
* References :
* 1) G. Golub and C. V. Loan, Matrix Computations, 3rd ed. Baltimore, Johns
* Hopkins University Press, 1996, pp.405-411
* 2) Rosen, D. and Carlone, L., 2017, September. Computational
* enhancements for certifiably correct SLAM. In Proceedings of the
* International Conference on Intelligent Robots and Systems.
* 3) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How,
* 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv
* 4) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated
* stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp.
* 58–67
*
* It performs the following iteration: \f$ x_{k+1} = A * x_k - \beta *
* x_{k-1} \f$ where A is the aim matrix we want to get eigenpair of, x is the
* Ritz vector
*
* Template argument Operator just needs multiplication operator
*
*/
template <class Operator>
class AcceleratedPowerMethod : public PowerMethod<Operator> {
double beta_ = 0; // a Polyak momentum term
Vector previousVector_; // store previous vector
public:
/**
* Constructor from aim matrix A (given as Matrix or Sparse), optional intial
* vector as ritzVector
*/
explicit AcceleratedPowerMethod(
const Operator &A, const boost::optional<Vector> initial = boost::none,
double initialBeta = 0.0)
: PowerMethod<Operator>(A, initial) {
// initialize Ritz eigen vector and previous vector
this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_);
this->ritzVector_.normalize();
previousVector_ = Vector::Zero(this->dim_);
// initialize beta_
beta_ = initialBeta;
}
/**
* Run accelerated power iteration to get ritzVector with beta and previous
* two ritzVector x0 and x00, and return y = (A * x0 - \beta * x00) / || A * x0
* - \beta * x00 ||
*/
Vector acceleratedPowerIteration (const Vector &x1, const Vector &x0,
const double beta) const {
Vector y = this->A_ * x1 - beta * x0;
y.normalize();
return y;
}
/**
* Run accelerated power iteration to get ritzVector with beta and previous
* two ritzVector x0 and x00, and return y = (A * x0 - \beta * x00) / || A * x0
* - \beta * x00 ||
*/
Vector acceleratedPowerIteration () const {
Vector y = acceleratedPowerIteration(this->ritzVector_, previousVector_, beta_);
return y;
}
/**
* Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3), T
* is the iteration time to find beta with largest Rayleigh quotient
*/
double estimateBeta(const size_t T = 10) const {
// set initial estimation of maxBeta
Vector initVector = this->ritzVector_;
const double up = initVector.dot( this->A_ * initVector );
const double down = initVector.dot(initVector);
const double mu = up / down;
double maxBeta = mu * mu / 4;
size_t maxIndex;
std::vector<double> betas;
Matrix R = Matrix::Zero(this->dim_, 10);
// run T times of iteration to find the beta that has the largest Rayleigh quotient
for (size_t t = 0; t < T; t++) {
// after each t iteration, reset the betas with the current maxBeta
betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, 1.01 * maxBeta,
1.5 * maxBeta};
// iterate through every beta value
for (size_t k = 0; k < betas.size(); ++k) {
// initialize x0 and x00 in each iteration of each beta
Vector x0 = initVector;
Vector x00 = Vector::Zero(this->dim_);
// run 10 steps of accelerated power iteration with this beta
for (size_t j = 1; j < 10; j++) {
if (j < 2) {
R.col(0) = acceleratedPowerIteration(x0, x00, betas[k]);
R.col(1) = acceleratedPowerIteration(R.col(0), x0, betas[k]);
} else {
R.col(j) = acceleratedPowerIteration(R.col(j - 1), R.col(j - 2),
betas[k]);
}
}
// compute the Rayleigh quotient for the randomly sampled vector after
// 10 steps of power accelerated iteration
const Vector x = R.col(9);
const double up = x.dot(this->A_ * x);
const double down = x.dot(x);
const double mu = up / down;
// store the momentum with largest Rayleigh quotient and its according index of beta_
if (mu * mu / 4 > maxBeta) {
// save the max beta index
maxIndex = k;
maxBeta = mu * mu / 4;
}
}
}
// set beta_ to momentum with largest Rayleigh quotient
return betas[maxIndex];
}
/**
* Start the accelerated iteration, after performing the
* accelerated iteration, calculate the ritz error, repeat this
* operation until the ritz error converge. If converged return true, else
* false.
*/
bool compute(size_t maxIterations, double tol) {
// Starting
bool isConverged = false;
for (size_t i = 0; i < maxIterations && !isConverged; i++) {
++(this->nrIterations_);
Vector tmp = this->ritzVector_;
// update the ritzVector after accelerated power iteration
this->ritzVector_ = acceleratedPowerIteration();
// update the previousVector with ritzVector
previousVector_ = tmp;
// update the ritzValue
this->ritzValue_ = this->ritzVector_.dot(this->A_ * this->ritzVector_);
isConverged = this->converged(tol);
}
return isConverged;
}
};
} // namespace gtsam

152
gtsam/linear/PowerMethod.h Normal file
View File

@ -0,0 +1,152 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PowerMethod.h
* @date Sept 2020
* @author Jing Wu
* @brief Power method for fast eigenvalue and eigenvector
* computation
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <random>
#include <vector>
namespace gtsam {
using Sparse = Eigen::SparseMatrix<double>;
/**
* \brief Compute maximum Eigenpair with power method
*
* References :
* 1) G. Golub and C. V. Loan, Matrix Computations, 3rd ed. Baltimore, Johns
* Hopkins University Press, 1996, pp.405-411
* 2) Rosen, D. and Carlone, L., 2017, September. Computational
* enhancements for certifiably correct SLAM. In Proceedings of the
* International Conference on Intelligent Robots and Systems.
* 3) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How,
* 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv
* 4) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated
* stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp.
* 58–67
*
* It performs the following iteration: \f$ x_{k+1} = A * x_k \f$
* where A is the aim matrix we want to get eigenpair of, x is the
* Ritz vector
*
* Template argument Operator just needs multiplication operator
*
*/
template <class Operator>
class PowerMethod {
protected:
/**
* Const reference to an externally-held matrix whose minimum-eigenvalue we
* want to compute
*/
const Operator &A_;
const int dim_; // dimension of Matrix A
size_t nrIterations_; // number of iterations
double ritzValue_; // Ritz eigenvalue
Vector ritzVector_; // Ritz eigenvector
public:
/// @name Standard Constructors
/// @{
/// Construct from the aim matrix and intial ritz vector
explicit PowerMethod(const Operator &A,
const boost::optional<Vector> initial = boost::none)
: A_(A), dim_(A.rows()), nrIterations_(0) {
Vector x0;
x0 = initial ? initial.get() : Vector::Random(dim_);
x0.normalize();
// initialize Ritz eigen value
ritzValue_ = 0.0;
// initialize Ritz eigen vector
ritzVector_ = powerIteration(x0);
}
/**
* Run power iteration to get ritzVector with previous ritzVector x, and
* return A * x / || A * x ||
*/
Vector powerIteration(const Vector &x) const {
Vector y = A_ * x;
y.normalize();
return y;
}
/**
* Run power iteration to get ritzVector with previous ritzVector x, and
* return A * x / || A * x ||
*/
Vector powerIteration() const { return powerIteration(ritzVector_); }
/**
* After Perform power iteration on a single Ritz value, check if the Ritz
* residual for the current Ritz pair is less than the required convergence
* tol, return true if yes, else false
*/
bool converged(double tol) const {
const Vector x = ritzVector_;
// store the Ritz eigen value
const double ritzValue = x.dot(A_ * x);
const double error = (A_ * x - ritzValue * x).norm();
return error < tol;
}
/// Return the number of iterations
size_t nrIterations() const { return nrIterations_; }
/**
* Start the power/accelerated iteration, after performing the
* power/accelerated iteration, calculate the ritz error, repeat this
* operation until the ritz error converge. If converged return true, else
* false.
*/
bool compute(size_t maxIterations, double tol) {
// Starting
bool isConverged = false;
for (size_t i = 0; i < maxIterations && !isConverged; i++) {
++nrIterations_;
// update the ritzVector after power iteration
ritzVector_ = powerIteration();
// update the ritzValue
ritzValue_ = ritzVector_.dot(A_ * ritzVector_);
isConverged = converged(tol);
}
return isConverged;
}
/// Return the eigenvalue
double eigenvalue() const { return ritzValue_; }
/// Return the eigenvector
Vector eigenvector() const { return ritzVector_; }
};
} // namespace gtsam

119
gtsam/linear/SparseEigen.h Normal file
View File

@ -0,0 +1,119 @@
/* ----------------------------------------------------------------------------
* 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 SparseEigen.h
*
* @brief Utilities for creating Eigen sparse matrices (gtsam::SparseEigen)
*
* @date Aug 2019
* @author Mandy Xie
* @author Fan Jiang
* @author Gerry Chen
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <Eigen/Sparse>
namespace gtsam {
typedef Eigen::SparseMatrix<double> SparseEigen;
/// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph
SparseEigen sparseJacobianEigen(
const GaussianFactorGraph &gfg, const Ordering &ordering) {
// TODO(gerry): eliminate copy/pasta by making GaussianFactorGraph version
// more general, or by creating an Eigen::Triplet compatible wrapper for
// boost::tuple return type
// First find dimensions of each variable
std::map<Key, size_t> dims;
for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
if (!static_cast<bool>(factor)) continue;
for (auto it = factor->begin(); it != factor->end(); ++it) {
dims[*it] = factor->getDim(it);
}
}
// Compute first scalar column of each variable
size_t currentColIndex = 0;
std::map<Key, size_t> columnIndices;
for (const auto key : ordering) {
columnIndices[key] = currentColIndex;
currentColIndex += dims[key];
}
// Iterate over all factors, adding sparse scalar entries
std::vector<Eigen::Triplet<double>> entries;
entries.reserve(60 * gfg.size());
size_t row = 0;
for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
if (!static_cast<bool>(factor)) continue;
// Convert to JacobianFactor if necessary
JacobianFactor::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (!jacobianFactor) {
HessianFactor::shared_ptr hessian(
boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian)
jacobianFactor.reset(new JacobianFactor(*hessian));
else
throw std::invalid_argument(
"GaussianFactorGraph contains a factor that is neither a "
"JacobianFactor nor a HessianFactor.");
}
// Whiten the factor and add entries for it
// iterate over all variables in the factor
const JacobianFactor whitened(jacobianFactor->whiten());
for (JacobianFactor::const_iterator key = whitened.begin();
key < whitened.end(); ++key) {
JacobianFactor::constABlock whitenedA = whitened.getA(key);
// find first column index for this key
size_t column_start = columnIndices[*key];
for (size_t i = 0; i < (size_t)whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t)whitenedA.cols(); j++) {
double s = whitenedA(i, j);
if (std::abs(s) > 1e-12)
entries.emplace_back(row + i, column_start + j, s);
}
}
JacobianFactor::constBVector whitenedb(whitened.getb());
size_t bcolumn = currentColIndex;
for (size_t i = 0; i < (size_t)whitenedb.size(); i++) {
double s = whitenedb(i);
if (std::abs(s) > 1e-12) entries.emplace_back(row + i, bcolumn, s);
}
// Increment row index
row += jacobianFactor->rows();
}
// ...and make a sparse matrix with it.
SparseEigen Ab(row, currentColIndex + 1);
Ab.setFromTriplets(entries.begin(), entries.end());
return Ab;
}
SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
}
} // namespace gtsam

2
gtsam/linear/SubgraphBuilder.cpp
View File

@ -383,7 +383,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
if (tree.size() != n - 1) {
throw std::runtime_error(
"SubgraphBuilder::operator() failure: tree.size() != n-1");
"SubgraphBuilder::operator() failure: tree.size() != n-1, might be caused by disconnected graph");
}
// Downweight the tree edges to zero.

4
gtsam/linear/VectorValues.cpp
View File

@ -161,7 +161,7 @@ namespace gtsam {
bool VectorValues::equals(const VectorValues& x, double tol) const {
if(this->size() != x.size())
return false;
for(const auto& values: boost::combine(*this, x)) {
for(const auto values: boost::combine(*this, x)) {
if(values.get<0>().first != values.get<1>().first ||
!equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
return false;
@ -233,7 +233,7 @@ namespace gtsam {
double result = 0.0;
typedef boost::tuple<value_type, value_type> ValuePair;
using boost::adaptors::map_values;
for(const ValuePair& values: boost::combine(*this, v)) {
for(const ValuePair values: boost::combine(*this, v)) {
assert_throw(values.get<0>().first == values.get<1>().first,
invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),

67
gtsam/linear/tests/powerMethodExample.h Normal file
View File

@ -0,0 +1,67 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* powerMethodExample.h
*
* @file powerMethodExample.h
* @date Nov 2020
* @author Jing Wu
* @brief Create sparse and dense factor graph for
* PowerMethod/AcceleratedPowerMethod
*/
#include <gtsam/inference/Symbol.h>
#include <iostream>
namespace gtsam {
namespace linear {
namespace test {
namespace example {
/* ************************************************************************* */
inline GaussianFactorGraph createSparseGraph() {
using symbol_shorthand::X;
// Let's make a scalar synchronization graph with 4 nodes
GaussianFactorGraph fg;
auto model = noiseModel::Unit::Create(1);
for (size_t j = 0; j < 3; j++) {
fg.add(X(j), -I_1x1, X(j + 1), I_1x1, Vector1::Zero(), model);
}
fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row
return fg;
}
/* ************************************************************************* */
inline GaussianFactorGraph createDenseGraph() {
using symbol_shorthand::X;
// Let's make a scalar synchronization graph with 10 nodes
GaussianFactorGraph fg;
auto model = noiseModel::Unit::Create(1);
// Iterate over nodes
for (size_t j = 0; j < 10; j++) {
// Each node has an edge with all the others
for (size_t i = 1; i < 10; i++)
fg.add(X(j), -I_1x1, X((j + i) % 10), I_1x1, Vector1::Zero(), model);
}
return fg;
}
/* ************************************************************************* */
} // namespace example
} // namespace test
} // namespace linear
} // namespace gtsam

140
gtsam/linear/tests/testAcceleratedPowerMethod.cpp Normal file
View File

@ -0,0 +1,140 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* testPowerMethod.cpp
*
* @file testAcceleratedPowerMethod.cpp
* @date Sept 2020
* @author Jing Wu
* @brief Check eigenvalue and eigenvector computed by accelerated power method
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/AcceleratedPowerMethod.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/tests/powerMethodExample.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <random>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(AcceleratedPowerMethod, acceleratedPowerIteration) {
// test power iteration, beta is set to 0
Sparse A(6, 6);
A.coeffRef(0, 0) = 6;
A.coeffRef(1, 1) = 5;
A.coeffRef(2, 2) = 4;
A.coeffRef(3, 3) = 3;
A.coeffRef(4, 4) = 2;
A.coeffRef(5, 5) = 1;
Vector initial = (Vector(6) << 0.24434602, 0.22829942, 0.70094486, 0.15463092, 0.55871359,
0.2465342).finished();
const double ev1 = 6.0;
// test accelerated power iteration
AcceleratedPowerMethod<Sparse> apf(A, initial);
apf.compute(100, 1e-5);
EXPECT_LONGS_EQUAL(6, apf.eigenvector().rows());
Vector6 actual1 = apf.eigenvector();
const double ritzValue = actual1.dot(A * actual1);
const double ritzResidual = (A * actual1 - ritzValue * actual1).norm();
EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-5);
}
/* ************************************************************************* */
TEST(AcceleratedPowerMethod, useFactorGraphSparse) {
// Let's make a scalar synchronization graph with 4 nodes
GaussianFactorGraph fg = gtsam::linear::test::example::createSparseGraph();
// Get eigenvalues and eigenvectors with Eigen
auto L = fg.hessian();
Eigen::EigenSolver<Matrix> solver(L.first);
// find the index of the max eigenvalue
size_t maxIdx = 0;
for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
maxIdx = i;
}
// Store the max eigenvalue and its according eigenvector
const auto ev1 = solver.eigenvalues()(maxIdx).real();
Vector disturb = Vector4::Random();
disturb.normalize();
Vector initial = L.first.row(0);
double magnitude = initial.norm();
initial += 0.03 * magnitude * disturb;
AcceleratedPowerMethod<Matrix> apf(L.first, initial);
apf.compute(100, 1e-5);
// Check if the eigenvalue is the maximum eigen value
EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8);
// Check if the according ritz residual converged to the threshold
Vector actual1 = apf.eigenvector();
const double ritzValue = actual1.dot(L.first * actual1);
const double ritzResidual = (L.first * actual1 - ritzValue * actual1).norm();
EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
}
/* ************************************************************************* */
TEST(AcceleratedPowerMethod, useFactorGraphDense) {
// Let's make a scalar synchronization graph with 10 nodes
GaussianFactorGraph fg = gtsam::linear::test::example::createDenseGraph();
// Get eigenvalues and eigenvectors with Eigen
auto L = fg.hessian();
Eigen::EigenSolver<Matrix> solver(L.first);
// find the index of the max eigenvalue
size_t maxIdx = 0;
for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
maxIdx = i;
}
// Store the max eigenvalue and its according eigenvector
const auto ev1 = solver.eigenvalues()(maxIdx).real();
Vector disturb = Vector10::Random();
disturb.normalize();
Vector initial = L.first.row(0);
double magnitude = initial.norm();
initial += 0.03 * magnitude * disturb;
AcceleratedPowerMethod<Matrix> apf(L.first, initial);
apf.compute(100, 1e-5);
// Check if the eigenvalue is the maximum eigen value
EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8);
// Check if the according ritz residual converged to the threshold
Vector actual1 = apf.eigenvector();
const double ritzValue = actual1.dot(L.first * actual1);
const double ritzResidual = (L.first * actual1 - ritzValue * actual1).norm();
EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

44
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -36,9 +36,18 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
// static SharedDiagonal
// sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
// constraintModel = noiseModel::Constrained::All(2);
typedef boost::tuple<size_t, size_t, double> BoostTriplet;
bool triplet_equal(BoostTriplet a, BoostTriplet b) {
if (a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>() &&
a.get<2>() == b.get<2>()) return true;
cout << "not equal:" << endl;
cout << "\texpected: "
"(" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl;
cout << "\tactual: "
"(" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl;
return false;
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, initialization) {
@ -74,8 +83,8 @@ TEST(GaussianFactorGraph, sparseJacobian) {
// 9 10 0 11 12 13
// 0 0 0 14 15 16
// Expected - NOTE that we transpose this!
Matrix expectedT = (Matrix(16, 3) <<
// Expected
Matrix expected = (Matrix(16, 3) <<
1., 1., 2.,
1., 2., 4.,
1., 3., 6.,
@ -93,17 +102,32 @@ TEST(GaussianFactorGraph, sparseJacobian) {
3., 6.,26.,
4., 6.,32.).finished();
Matrix expected = expectedT.transpose();
// expected: in matlab format - NOTE the transpose!)
Matrix expectedMatlab = expected.transpose();
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model);
gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1,
(Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model);
const Key x123 = 0, x45 = 1;
gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
Vector2(4, 8), model);
gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
Vector2(13, 16), model);
Matrix actual = gfg.sparseJacobian_();
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(expectedMatlab, actual));
// BoostTriplets
auto boostActual = gfg.sparseJacobian();
// check the triplets size...
EXPECT_LONGS_EQUAL(16, boostActual.size());
// check content
for (int i = 0; i < 16; i++) {
EXPECT(triplet_equal(
BoostTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
boostActual.at(i)));
}
}
/* ************************************************************************* */

124
gtsam/linear/tests/testPowerMethod.cpp Normal file
View File

@ -0,0 +1,124 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* testPowerMethod.cpp
*
* @file testPowerMethod.cpp
* @date Sept 2020
* @author Jing Wu
* @brief Check eigenvalue and eigenvector computed by power method
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PowerMethod.h>
#include <gtsam/linear/tests/powerMethodExample.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <random>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(PowerMethod, powerIteration) {
// test power iteration, beta is set to 0
Sparse A(6, 6);
A.coeffRef(0, 0) = 6;
A.coeffRef(1, 1) = 5;
A.coeffRef(2, 2) = 4;
A.coeffRef(3, 3) = 3;
A.coeffRef(4, 4) = 2;
A.coeffRef(5, 5) = 1;
Vector initial = (Vector(6) << 0.24434602, 0.22829942, 0.70094486, 0.15463092, 0.55871359,
0.2465342).finished();
PowerMethod<Sparse> pf(A, initial);
pf.compute(100, 1e-5);
EXPECT_LONGS_EQUAL(6, pf.eigenvector().rows());
Vector6 actual1 = pf.eigenvector();
const double ritzValue = actual1.dot(A * actual1);
const double ritzResidual = (A * actual1 - ritzValue * actual1).norm();
EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
const double ev1 = 6.0;
EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-5);
}
/* ************************************************************************* */
TEST(PowerMethod, useFactorGraphSparse) {
// Let's make a scalar synchronization graph with 4 nodes
GaussianFactorGraph fg = gtsam::linear::test::example::createSparseGraph();
// Get eigenvalues and eigenvectors with Eigen
auto L = fg.hessian();
Eigen::EigenSolver<Matrix> solver(L.first);
// find the index of the max eigenvalue
size_t maxIdx = 0;
for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
maxIdx = i;
}
// Store the max eigenvalue and its according eigenvector
const auto ev1 = solver.eigenvalues()(maxIdx).real();
Vector initial = Vector4::Random();
PowerMethod<Matrix> pf(L.first, initial);
pf.compute(100, 1e-5);
EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8);
auto actual2 = pf.eigenvector();
const double ritzValue = actual2.dot(L.first * actual2);
const double ritzResidual = (L.first * actual2 - ritzValue * actual2).norm();
EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
}
/* ************************************************************************* */
TEST(PowerMethod, useFactorGraphDense) {
// Let's make a scalar synchronization graph with 10 nodes
GaussianFactorGraph fg = gtsam::linear::test::example::createDenseGraph();
// Get eigenvalues and eigenvectors with Eigen
auto L = fg.hessian();
Eigen::EigenSolver<Matrix> solver(L.first);
// find the index of the max eigenvalue
size_t maxIdx = 0;
for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
maxIdx = i;
}
// Store the max eigenvalue and its according eigenvector
const auto ev1 = solver.eigenvalues()(maxIdx).real();
Vector initial = Vector10::Random();
PowerMethod<Matrix> pf(L.first, initial);
pf.compute(100, 1e-5);
EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8);
auto actual2 = pf.eigenvector();
const double ritzValue = actual2.dot(L.first * actual2);
const double ritzResidual = (L.first * actual2 - ritzValue * actual2).norm();
EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

72
gtsam/linear/tests/testSparseEigen.cpp Normal file
View File

@ -0,0 +1,72 @@
/* ----------------------------------------------------------------------------
* 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 testSparseMatrix.cpp
* @author Mandy Xie
* @author Fan Jiang
* @author Gerry Chen
* @author Frank Dellaert
* @date Jan, 2021
*/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SparseEigen.h>
#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(SparseEigen, sparseJacobianEigen) {
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
const Key x123 = 0, x45 = 1;
gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
Vector2(4, 8), model);
gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
Vector2(13, 16), model);
// Sparse Matrix
auto sparseResult = sparseJacobianEigen(gfg);
EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
EXPECT(assert_equal(4, sparseResult.rows()));
EXPECT(assert_equal(6, sparseResult.cols()));
EXPECT(assert_equal(gfg.augmentedJacobian(), Matrix(sparseResult)));
// Call sparseJacobian with optional ordering...
auto ordering = Ordering(list_of(x45)(x123));
// Eigen Sparse with optional ordering
EXPECT(assert_equal(gfg.augmentedJacobian(ordering),
Matrix(sparseJacobianEigen(gfg, ordering))));
// Check matrix dimensions when zero rows / cols
gfg.add(x123, Matrix23::Zero(), Vector2::Zero(), model); // zero row
gfg.add(2, Matrix21::Zero(), Vector2::Zero(), model); // zero col
sparseResult = sparseJacobianEigen(gfg);
EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
EXPECT(assert_equal(8, sparseResult.rows()));
EXPECT(assert_equal(7, sparseResult.cols()));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
gtsam/navigation/ImuBias.h
View File

@ -29,8 +29,8 @@ namespace imuBias {
class GTSAM_EXPORT ConstantBias {
private:
Vector3 biasAcc_;
Vector3 biasGyro_;
Vector3 biasAcc_; ///< The units for stddev are σ = m/s² or m √Hz/s²
Vector3 biasGyro_; ///< The units for stddev are σ = rad/s or rad √Hz/s
public:
/// dimension of the variable - used to autodetect sizes

4
gtsam/navigation/PreintegratedRotation.h
View File

@ -29,7 +29,9 @@ namespace gtsam {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct GTSAM_EXPORT PreintegratedRotationParams {
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
/// Continuous-time "Covariance" of gyroscope measurements
/// The units for stddev are σ = rad/s/√Hz
Matrix3 gyroscopeCovariance;
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame

4
gtsam/navigation/PreintegrationParams.h
View File

@ -24,7 +24,9 @@ namespace gtsam {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
/// Continuous-time "Covariance" of accelerometer
/// The units for stddev are σ = m/s²/√Hz
Matrix3 accelerometerCovariance;
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 n_gravity; ///< Gravity vector in nav frame

116
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -87,8 +87,8 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
}
Vector evaluateError(const T &params,
boost::optional<Matrix &> H = boost::none) const override {
Vector evaluateError(const T &params, boost::optional<Matrix &> H =
boost::none) const override {
R x = func_(params, H);
Vector error = traits<R>::Local(measured_, x);
return error;
@ -96,8 +96,9 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
/// @name Testable
/// @{
void print(const std::string &s = "",
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
void print(
const std::string &s = "",
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
<< keyFormatter(this->key()) << ")" << std::endl;
@ -144,4 +145,111 @@ FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
return FunctorizedFactor<R, T>(key, z, model, func);
}
/**
* Factor which evaluates provided binary functor and uses the result to compute
* error with respect to the provided measurement.
*
* Template parameters are
* @param R: The return type of the functor after evaluation.
* @param T1: The first argument type for the functor.
* @param T2: The second argument type for the functor.
*/
template <typename R, typename T1, typename T2>
class GTSAM_EXPORT FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
private:
using Base = NoiseModelFactor2<T1, T2>;
R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
boost::optional<Matrix &>)>;
FunctionType func_; ///< functor instance
public:
/** default constructor - only use for serialization */
FunctorizedFactor2() {}
/** Construct with given x and the parameters of the basis
*
* @param key: Factor key
* @param z: Measurement object of same type as that returned by functor
* @param model: Noise model
* @param func: The instance of the functor object
*/
FunctorizedFactor2(Key key1, Key key2, const R &z,
const SharedNoiseModel &model, const FunctionType func)
: Base(model, key1, key2),
measured_(z),
noiseModel_(model),
func_(func) {}
virtual ~FunctorizedFactor2() {}
/// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
}
Vector evaluateError(
const T1 &params1, const T2 &params2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const override {
R x = func_(params1, params2, H1, H2);
Vector error = traits<R>::Local(measured_, x);
return error;
}
/// @name Testable
/// @{
void print(
const std::string &s = "",
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
<< keyFormatter(this->key1()) << ", "
<< keyFormatter(this->key2()) << ")" << std::endl;
traits<R>::Print(measured_, " measurement: ");
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl;
}
bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
const FunctorizedFactor2<R, T1, T2> *e =
dynamic_cast<const FunctorizedFactor2<R, T1, T2> *>(&other);
return e && Base::equals(other, tol) &&
traits<R>::Equals(this->measured_, e->measured_, tol);
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_);
ar &BOOST_SERIALIZATION_NVP(func_);
}
};
/// traits
template <typename R, typename T1, typename T2>
struct traits<FunctorizedFactor2<R, T1, T2>>
: public Testable<FunctorizedFactor2<R, T1, T2>> {};
/**
* Helper function to create a functorized factor.
*
* Uses function template deduction to identify return type and functor type, so
* template list only needs the functor argument type.
*/
template <typename T1, typename T2, typename R, typename FUNC>
FunctorizedFactor2<R, T1, T2> MakeFunctorizedFactor2(
Key key1, Key key2, const R &z, const SharedNoiseModel &model,
const FUNC func) {
return FunctorizedFactor2<R, T1, T2>(key1, key2, z, model, func);
}
} // namespace gtsam

2
gtsam/nonlinear/GaussNewtonOptimizer.h
View File

@ -28,6 +28,8 @@ class GaussNewtonOptimizer;
* NonlinearOptimizationParams.
*/
class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams {
public:
using OptimizerType = GaussNewtonOptimizer;
};
/**

338
gtsam/nonlinear/GncOptimizer.h Normal file
View File

@ -0,0 +1,338 @@
/* ----------------------------------------------------------------------------
* 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 GncOptimizer.h
* @brief The GncOptimizer class
* @author Jingnan Shi
* @author Luca Carlone
* @author Frank Dellaert
*
* Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception:
* From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf)
*
* See also:
* Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications",
* arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020.
*/
#pragma once
#include <gtsam/nonlinear/GncParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam {
/* ************************************************************************* */
template<class GncParameters>
class GncOptimizer {
public:
/// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
typedef typename GncParameters::OptimizerType BaseOptimizer;
private:
NonlinearFactorGraph nfg_; ///< Original factor graph to be solved by GNC.
Values state_; ///< Initial values to be used at each iteration by GNC.
GncParameters params_; ///< GNC parameters.
Vector weights_; ///< Weights associated to each factor in GNC (this could be a local variable in optimize, but it is useful to make it accessible from outside).
public:
/// Constructor.
GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
const GncParameters& params = GncParameters())
: state_(initialValues),
params_(params) {
// make sure all noiseModels are Gaussian or convert to Gaussian
nfg_.resize(graph.size());
for (size_t i = 0; i < graph.size(); i++) {
if (graph[i]) {
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
NoiseModelFactor>(graph[i]);
auto robust = boost::dynamic_pointer_cast<
noiseModel::Robust>(factor->noiseModel());
// if the factor has a robust loss, we remove the robust loss
nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
}
}
}
/// Access a copy of the internal factor graph.
const NonlinearFactorGraph& getFactors() const { return nfg_; }
/// Access a copy of the internal values.
const Values& getState() const { return state_; }
/// Access a copy of the parameters.
const GncParameters& getParams() const { return params_;}
/// Access a copy of the GNC weights.
const Vector& getWeights() const { return weights_;}
/// Compute optimal solution using graduated non-convexity.
Values optimize() {
// start by assuming all measurements are inliers
weights_ = Vector::Ones(nfg_.size());
BaseOptimizer baseOptimizer(nfg_, state_);
Values result = baseOptimizer.optimize();
double mu = initializeMu();
double prev_cost = nfg_.error(result);
double cost = 0.0; // this will be updated in the main loop
// handle the degenerate case that corresponds to small
// maximum residual errors at initialization
// For GM: if residual error is small, mu -> 0
// For TLS: if residual error is small, mu -> -1
if (mu <= 0) {
if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
std::cout << "GNC Optimizer stopped because maximum residual at "
"initialization is small."
<< std::endl;
}
if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
result.print("result\n");
std::cout << "mu: " << mu << std::endl;
}
return result;
}
size_t iter;
for (iter = 0; iter < params_.maxIterations; iter++) {
// display info
if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
std::cout << "iter: " << iter << std::endl;
result.print("result\n");
std::cout << "mu: " << mu << std::endl;
std::cout << "weights: " << weights_ << std::endl;
}
// weights update
weights_ = calculateWeights(result, mu);
// variable/values update
NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_);
BaseOptimizer baseOptimizer_iter(graph_iter, state_);
result = baseOptimizer_iter.optimize();
// stopping condition
cost = graph_iter.error(result);
if (checkConvergence(mu, weights_, cost, prev_cost)) {
break;
}
// update mu
mu = updateMu(mu);
// get ready for next iteration
prev_cost = cost;
// display info
if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
std::cout << "previous cost: " << prev_cost << std::endl;
std::cout << "current cost: " << cost << std::endl;
}
}
// display info
if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
std::cout << "final iterations: " << iter << std::endl;
std::cout << "final mu: " << mu << std::endl;
std::cout << "final weights: " << weights_ << std::endl;
std::cout << "previous cost: " << prev_cost << std::endl;
std::cout << "current cost: " << cost << std::endl;
}
return result;
}
/// Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
double initializeMu() const {
// compute largest error across all factors
double rmax_sq = 0.0;
for (size_t i = 0; i < nfg_.size(); i++) {
if (nfg_[i]) {
rmax_sq = std::max(rmax_sq, nfg_[i]->error(state_));
}
}
// set initial mu
switch (params_.lossType) {
case GncLossType::GM:
// surrogate cost is convex for large mu
return 2 * rmax_sq / params_.barcSq; // initial mu
case GncLossType::TLS:
/* initialize mu to the value specified in Remark 5 in GNC paper.
surrogate cost is convex for mu close to zero
degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop)
according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual
however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop
*/
return
(2 * rmax_sq - params_.barcSq) > 0 ?
params_.barcSq / (2 * rmax_sq - params_.barcSq) : -1;
default:
throw std::runtime_error(
"GncOptimizer::initializeMu: called with unknown loss type.");
}
}
/// Update the gnc parameter mu to gradually increase nonconvexity.
double updateMu(const double mu) const {
switch (params_.lossType) {
case GncLossType::GM:
// reduce mu, but saturate at 1 (original cost is recovered for mu -> 1)
return std::max(1.0, mu / params_.muStep);
case GncLossType::TLS:
// increases mu at each iteration (original cost is recovered for mu -> inf)
return mu * params_.muStep;
default:
throw std::runtime_error(
"GncOptimizer::updateMu: called with unknown loss type.");
}
}
/// Check if we have reached the value of mu for which the surrogate loss matches the original loss.
bool checkMuConvergence(const double mu) const {
bool muConverged = false;
switch (params_.lossType) {
case GncLossType::GM:
muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function
break;
case GncLossType::TLS:
muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity)
break;
default:
throw std::runtime_error(
"GncOptimizer::checkMuConvergence: called with unknown loss type.");
}
if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
std::cout << "muConverged = true " << std::endl;
return muConverged;
}
/// Check convergence of relative cost differences.
bool checkCostConvergence(const double cost, const double prev_cost) const {
bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7)
< params_.relativeCostTol;
if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
std::cout << "checkCostConvergence = true " << std::endl;
return costConverged;
}
/// Check convergence of weights to binary values.
bool checkWeightsConvergence(const Vector& weights) const {
bool weightsConverged = false;
switch (params_.lossType) {
case GncLossType::GM:
weightsConverged = false; // for GM, there is no clear binary convergence for the weights
break;
case GncLossType::TLS:
weightsConverged = true;
for (size_t i = 0; i < weights.size(); i++) {
if (std::fabs(weights[i] - std::round(weights[i]))
> params_.weightsTol) {
weightsConverged = false;
break;
}
}
break;
default:
throw std::runtime_error(
"GncOptimizer::checkWeightsConvergence: called with unknown loss type.");
}
if (weightsConverged
&& params_.verbosity >= GncParameters::Verbosity::SUMMARY)
std::cout << "weightsConverged = true " << std::endl;
return weightsConverged;
}
/// Check for convergence between consecutive GNC iterations.
bool checkConvergence(const double mu, const Vector& weights,
const double cost, const double prev_cost) const {
return checkCostConvergence(cost, prev_cost)
|| checkWeightsConvergence(weights) || checkMuConvergence(mu);
}
/// Create a graph where each factor is weighted by the gnc weights.
NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const {
// make sure all noiseModels are Gaussian or convert to Gaussian
NonlinearFactorGraph newGraph;
newGraph.resize(nfg_.size());
for (size_t i = 0; i < nfg_.size(); i++) {
if (nfg_[i]) {
auto factor = boost::dynamic_pointer_cast<
NoiseModelFactor>(nfg_[i]);
auto noiseModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(
factor->noiseModel());
if (noiseModel) {
Matrix newInfo = weights[i] * noiseModel->information();
auto newNoiseModel = noiseModel::Gaussian::Information(newInfo);
newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
} else {
throw std::runtime_error(
"GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model.");
}
}
}
return newGraph;
}
/// Calculate gnc weights.
Vector calculateWeights(const Values& currentEstimate, const double mu) {
Vector weights = Vector::Ones(nfg_.size());
// do not update the weights that the user has decided are known inliers
std::vector<size_t> allWeights;
for (size_t k = 0; k < nfg_.size(); k++) {
allWeights.push_back(k);
}
std::vector<size_t> unknownWeights;
std::set_difference(allWeights.begin(), allWeights.end(),
params_.knownInliers.begin(),
params_.knownInliers.end(),
std::inserter(unknownWeights, unknownWeights.begin()));
// update weights of known inlier/outlier measurements
switch (params_.lossType) {
case GncLossType::GM: { // use eq (12) in GNC paper
for (size_t k : unknownWeights) {
if (nfg_[k]) {
double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
weights[k] = std::pow(
(mu * params_.barcSq) / (u2_k + mu * params_.barcSq), 2);
}
}
return weights;
}
case GncLossType::TLS: { // use eq (14) in GNC paper
double upperbound = (mu + 1) / mu * params_.barcSq;
double lowerbound = mu / (mu + 1) * params_.barcSq;
for (size_t k : unknownWeights) {
if (nfg_[k]) {
double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
if (u2_k >= upperbound) {
weights[k] = 0;
} else if (u2_k <= lowerbound) {
weights[k] = 1;
} else {
weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k)
- mu;
}
}
}
return weights;
}
default:
throw std::runtime_error(
"GncOptimizer::calculateWeights: called with unknown loss type.");
}
}
};
}

164
gtsam/nonlinear/GncParams.h Normal file
View File

@ -0,0 +1,164 @@
/* ----------------------------------------------------------------------------
* 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 GncOptimizer.h
* @brief The GncOptimizer class
* @author Jingnan Shi
* @author Luca Carlone
* @author Frank Dellaert
*
* Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception:
* From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf)
*
* See also:
* Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications",
* arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020.
*/
#pragma once
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
namespace gtsam {
/* ************************************************************************* */
/// Choice of robust loss function for GNC.
enum GncLossType {
GM /*Geman McClure*/,
TLS /*Truncated least squares*/
};
template<class BaseOptimizerParameters>
class GncParams {
public:
/// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
/// Verbosity levels
enum Verbosity {
SILENT = 0,
SUMMARY,
VALUES
};
/// Constructor.
GncParams(const BaseOptimizerParameters& baseOptimizerParams)
: baseOptimizerParams(baseOptimizerParams) {
}
/// Default constructor.
GncParams()
: baseOptimizerParams() {
}
/// GNC parameters.
BaseOptimizerParameters baseOptimizerParams; ///< Optimization parameters used to solve the weighted least squares problem at each GNC iteration
/// any other specific GNC parameters:
GncLossType lossType = TLS; ///< Default loss
size_t maxIterations = 100; ///< Maximum number of iterations
double barcSq = 1.0; ///< A factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance
double muStep = 1.4; ///< Multiplicative factor to reduce/increase the mu in gnc
double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating
double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
Verbosity verbosity = SILENT; ///< Verbosity level
std::vector<size_t> knownInliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are inliers
/// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
void setLossType(const GncLossType type) {
lossType = type;
}
/// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended).
void setMaxIterations(const size_t maxIter) {
std::cout
<< "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
<< std::endl;
maxIterations = maxIter;
}
/** Set the maximum weighted residual error for an inlier. For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega,
* the inlier threshold is the largest value of f(x) for the corresponding measurement to be considered an inlier.
* In other words, an inlier at x is such that 0.5 * || r(x) ||^2_Omega <= barcSq.
* Assuming a isotropic measurement covariance sigma^2 * Identity, the cost becomes: 0.5 * 1/sigma^2 || r(x) ||^2 <= barcSq.
* Hence || r(x) ||^2 <= 2 * barcSq * sigma^2.
* */
void setInlierCostThreshold(const double inth) {
barcSq = inth;
}
/// Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep.
void setMuStep(const double step) {
muStep = step;
}
/// Set the maximum relative difference in mu values to stop iterating.
void setRelativeCostTol(double value) {
relativeCostTol = value;
}
/// Set the maximum difference between the weights and their rounding in {0,1} to stop iterating.
void setWeightsTol(double value) {
weightsTol = value;
}
/// Set the verbosity level.
void setVerbosityGNC(const Verbosity value) {
verbosity = value;
}
/** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector
* corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
* and you provide knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
* This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and
* only apply GNC to prune outliers from the loop closures.
* */
void setKnownInliers(const std::vector<size_t>& knownIn) {
for (size_t i = 0; i < knownIn.size(); i++)
knownInliers.push_back(knownIn[i]);
}
/// Equals.
bool equals(const GncParams& other, double tol = 1e-9) const {
return baseOptimizerParams.equals(other.baseOptimizerParams)
&& lossType == other.lossType && maxIterations == other.maxIterations
&& std::fabs(barcSq - other.barcSq) <= tol
&& std::fabs(muStep - other.muStep) <= tol
&& verbosity == other.verbosity && knownInliers == other.knownInliers;
}
/// Print.
void print(const std::string& str) const {
std::cout << str << "\n";
switch (lossType) {
case GM:
std::cout << "lossType: Geman McClure" << "\n";
break;
case TLS:
std::cout << "lossType: Truncated Least-squares" << "\n";
break;
default:
throw std::runtime_error("GncParams::print: unknown loss type.");
}
std::cout << "maxIterations: " << maxIterations << "\n";
std::cout << "barcSq: " << barcSq << "\n";
std::cout << "muStep: " << muStep << "\n";
std::cout << "relativeCostTol: " << relativeCostTol << "\n";
std::cout << "weightsTol: " << weightsTol << "\n";
std::cout << "verbosity: " << verbosity << "\n";
for (size_t i = 0; i < knownInliers.size(); i++)
std::cout << "knownInliers: " << knownInliers[i] << "\n";
baseOptimizerParams.print(str);
}
};
}

3
gtsam/nonlinear/LevenbergMarquardtParams.h
View File

@ -25,6 +25,8 @@
namespace gtsam {
class LevenbergMarquardtOptimizer;
/** Parameters for Levenberg-Marquardt optimization. Note that this parameters
* class inherits from NonlinearOptimizerParams, which specifies the parameters
* common to all nonlinear optimization algorithms. This class also contains
@ -40,6 +42,7 @@ public:
static VerbosityLM verbosityLMTranslator(const std::string &s);
static std::string verbosityLMTranslator(VerbosityLM value);
using OptimizerType = LevenbergMarquardtOptimizer;
public:

4
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

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

8
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -76,6 +76,14 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
&& noiseModel_->equals(*e->noiseModel_, tol)));
}
/* ************************************************************************* */
NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel(
const SharedNoiseModel newNoise) const {
NoiseModelFactor::shared_ptr new_factor = boost::dynamic_pointer_cast<NoiseModelFactor>(clone());
new_factor->noiseModel_ = newNoise;
return new_factor;
}
/* ************************************************************************* */
static void check(const SharedNoiseModel& noiseModel, size_t m) {
if (noiseModel && m != noiseModel->dim())

6
gtsam/nonlinear/NonlinearFactor.h
View File

@ -244,6 +244,12 @@ public:
*/
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
/**
* Creates a shared_ptr clone of the
* factor with a new noise model
*/
shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
private:
/** Serialization function */
friend class boost::serialization::access;

13
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -34,6 +34,7 @@
#endif
#include <cmath>
#include <fstream>
#include <limits>
using namespace std;
@ -256,6 +257,16 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
stm << "}\n";
}
/* ************************************************************************* */
void NonlinearFactorGraph::saveGraph(
const std::string& file, const Values& values,
const GraphvizFormatting& graphvizFormatting,
const KeyFormatter& keyFormatter) const {
std::ofstream of(file);
saveGraph(of, values, graphvizFormatting, keyFormatter);
of.close();
}
/* ************************************************************************* */
double NonlinearFactorGraph::error(const Values& values) const {
gttic(NonlinearFactorGraph_error);
@ -365,7 +376,7 @@ static Scatter scatterFromValues(const Values& values) {
scatter.reserve(values.size());
// use "natural" ordering with keys taken from the initial values
for (const auto& key_value : values) {
for (const auto key_value : values) {
scatter.add(key_value.key, key_value.value.dim());
}

14
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -11,7 +11,7 @@
/**
* @file NonlinearFactorGraph.h
* @brief Factor Graph Constsiting of non-linear factors
* @brief Factor Graph consisting of non-linear factors
* @author Frank Dellaert
* @author Carlos Nieto
* @author Christian Potthast
@ -111,11 +111,21 @@ namespace gtsam {
/** Test equality */
bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
/** Write the graph in GraphViz format for visualization */
/// Write the graph in GraphViz format for visualization
void saveGraph(std::ostream& stm, const Values& values = Values(),
const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/**
* Write the graph in GraphViz format to file for visualization.
*
* This is a wrapper friendly version since wrapped languages don't have
* access to C++ streams.
*/
void saveGraph(const std::string& file, const Values& values = Values(),
const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */
double error(const Values& values) const;

6
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

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

2
gtsam/nonlinear/NonlinearOptimizer.h
View File

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

71
gtsam/nonlinear/NonlinearOptimizerParams.h
View File

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

19
gtsam/nonlinear/Values-inl.h
View File

@ -217,7 +217,7 @@ namespace gtsam {
/** Constructor from a Filtered view copies out all values */
template<class ValueType>
Values::Values(const Values::Filtered<ValueType>& view) {
for(const typename Filtered<ValueType>::KeyValuePair& key_value: view) {
for(const auto key_value: view) {
Key key = key_value.key;
insert(key, static_cast<const ValueType&>(key_value.value));
}
@ -226,7 +226,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class ValueType>
Values::Values(const Values::ConstFiltered<ValueType>& view) {
for(const typename ConstFiltered<ValueType>::KeyValuePair& key_value: view) {
for(const auto key_value: view) {
Key key = key_value.key;
insert(key, static_cast<const ValueType&>(key_value.value));
}
@ -338,19 +338,18 @@ namespace gtsam {
} // internal
/* ************************************************************************* */
template<typename ValueType>
ValueType Values::at(Key j) const {
template <typename ValueType>
const ValueType Values::at(Key j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
// Throw exception if it does not exist
if(item == values_.end())
throw ValuesKeyDoesNotExist("at", j);
if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
// Check the type and throw exception if incorrect
// h() split in two lines to avoid internal compiler error (MSVC2017)
auto h = internal::handle<ValueType>();
return h(j,item->second);
// Check the type and throw exception if incorrect
// h() split in two lines to avoid internal compiler error (MSVC2017)
auto h = internal::handle<ValueType>();
return h(j, item->second);
}
/* ************************************************************************* */

4
gtsam/nonlinear/Values.cpp
View File

@ -206,7 +206,7 @@ namespace gtsam {
/* ************************************************************************* */
size_t Values::dim() const {
size_t result = 0;
for(const ConstKeyValuePair& key_value: *this) {
for(const auto key_value: *this) {
result += key_value.value.dim();
}
return result;
@ -215,7 +215,7 @@ namespace gtsam {
/* ************************************************************************* */
VectorValues Values::zeroVectors() const {
VectorValues result;
for(const ConstKeyValuePair& key_value: *this)
for(const auto key_value: *this)
result.insert(key_value.key, Vector::Zero(key_value.value.dim()));
return result;
}

4
gtsam/nonlinear/Values.h
View File

@ -187,8 +187,8 @@ namespace gtsam {
* Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa.
* @return The stored value
*/
template<typename ValueType>
ValueType at(Key j) const;
template <typename ValueType>
const ValueType at(Key j) const;
/// version for double
double atDouble(size_t key) const { return at<double>(key);}

2
gtsam/nonlinear/internal/LevenbergMarquardtState.h
View File

@ -126,7 +126,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState {
noiseModelCache.resize(0);
// for each of the variables, add a prior
damped.reserve(damped.size() + values.size());
for (const auto& key_value : values) {
for (const auto key_value : values) {
const Key key = key_value.key;
const size_t dim = key_value.value.dim();
const CachedModel* item = getCachedModel(dim);

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

@ -27,8 +27,15 @@
using namespace std;
using namespace gtsam;
// Key for FunctorizedFactor
Key key = Symbol('X', 0);
// Keys for FunctorizedFactor2
Key keyA = Symbol('A', 0);
Key keyx = Symbol('x', 0);
auto model = noiseModel::Isotropic::Sigma(9, 1);
auto model2 = noiseModel::Isotropic::Sigma(3, 1);
/// Functor that takes a matrix and multiplies every element by m
class MultiplyFunctor {
@ -44,6 +51,21 @@ class MultiplyFunctor {
}
};
/// Functor that performs Ax where A is a matrix and x is a vector.
class ProjectionFunctor {
public:
Vector operator()(const Matrix &A, const Vector &x,
OptionalJacobian<-1, -1> H1 = boost::none,
OptionalJacobian<-1, -1> H2 = boost::none) const {
if (H1) {
H1->resize(x.size(), A.size());
*H1 << I_3x3, I_3x3, I_3x3;
}
if (H2) *H2 = A;
return A * x;
}
};
/* ************************************************************************* */
// Test identity operation for FunctorizedFactor.
TEST(FunctorizedFactor, Identity) {
@ -88,7 +110,7 @@ TEST(FunctorizedFactor, Equality) {
EXPECT(factor1.equals(factor2));
}
/* *************************************************************************** */
/* ************************************************************************* */
// Test Jacobians of FunctorizedFactor.
TEST(FunctorizedFactor, Jacobians) {
Matrix X = Matrix::Identity(3, 3);
@ -168,6 +190,83 @@ TEST(FunctorizedFactor, Lambda) {
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
// Test identity operation for FunctorizedFactor2.
TEST(FunctorizedFactor, Identity2) {
// x = Ax since A is I_3x3
Matrix A = Matrix::Identity(3, 3);
Vector x = Vector::Ones(3);
auto functor = ProjectionFunctor();
auto factor =
MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2, functor);
Vector error = factor.evaluateError(A, x);
EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
}
/* ************************************************************************* */
// Test Jacobians of FunctorizedFactor2.
TEST(FunctorizedFactor, Jacobians2) {
Matrix A = Matrix::Identity(3, 3);
Vector x = Vector::Ones(3);
Matrix actualH1, actualH2;
auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2,
ProjectionFunctor());
Values values;
values.insert<Matrix>(keyA, A);
values.insert<Vector>(keyx, x);
// Check Jacobians
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
// Test FunctorizedFactor2 using a std::function type.
TEST(FunctorizedFactor, Functional2) {
Matrix A = Matrix::Identity(3, 3);
Vector3 x(1, 2, 3);
Vector measurement = A * x;
std::function<Matrix(Matrix, Matrix, boost::optional<Matrix &>,
boost::optional<Matrix &>)>
functional = ProjectionFunctor();
auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement,
model2, functional);
Vector error = factor.evaluateError(A, x);
EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
}
/* ************************************************************************* */
// Test FunctorizedFactor2 with a lambda function.
TEST(FunctorizedFactor, Lambda2) {
Matrix A = Matrix::Identity(3, 3);
Vector3 x = Vector3(1, 2, 3);
Matrix measurement = A * x;
auto lambda = [](const Matrix &A, const Vector &x,
OptionalJacobian<-1, -1> H1 = boost::none,
OptionalJacobian<-1, -1> H2 = boost::none) {
if (H1) {
H1->resize(x.size(), A.size());
*H1 << I_3x3, I_3x3, I_3x3;
}
if (H2) *H2 = A;
return A * x;
};
// FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement, model2, lambda);
Vector error = factor.evaluateError(A, x);
EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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