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

Merge branch 'fix/planeFactor' into fix/oriented-plane3-factor-jacobian

release/4.3a0
Frank Dellaert 2021-01-21 10:50:18 -05:00 committed by GitHub
parent 534fa6bb31 6b11c9fe83
commit fd8575bd13
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
127 changed files with 6979 additions and 4208 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

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

@ -83,9 +83,3 @@ jobs:
if: runner.os == 'Linux'
run: |
bash .github/scripts/unix.sh -t
- name: Upload build directory
uses: actions/upload-artifact@v2
if: matrix.build_type == 'Release'
with:
name: gtsam-${{ matrix.name }}-${{ matrix.build_type }}
path: ${{ github.workspace }}/build/

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

@ -51,9 +51,3 @@ jobs:
if: runner.os == 'macOS'
run: |
bash .github/scripts/unix.sh -t
- name: Upload build directory
uses: actions/upload-artifact@v2
if: matrix.build_type == 'Release'
with:
name: gtsam-${{ matrix.name }}-${{ matrix.build_type }}
path: ${{ github.workspace }}/build/

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

@ -76,9 +76,3 @@ jobs:
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
- name: Upload build directory
uses: actions/upload-artifact@v2
if: matrix.build_type == 'Release'
with:
name: gtsam-${{ matrix.name }}-${{ matrix.build_type }}
path: ${{ github.workspace }}/build/

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

33
cmake/GtsamMatlabWrap.cmake
View File

@ -146,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)
@ -159,7 +159,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
endif()
endif()
endif()
#message("AUTOMATIC DEPENDENCIES: ${automaticDependencies}")
## CHRIS: End temporary fix
@ -213,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 "")
@ -240,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}
@ -253,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}")
@ -291,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()
@ -343,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})
@ -375,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)
@ -389,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. "
@ -431,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:

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

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

14
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()

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;
}
/* ************************************************************************* */

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 */

3
gtsam/geometry/Cal3.h
View File

@ -99,6 +99,9 @@ class GTSAM_EXPORT Cal3 {
*/
Cal3(double fov, int w, int h);
/// Virtual destructor
virtual ~Cal3() {}
/// @}
/// @name Advanced Constructors
/// @{

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

10
gtsam/geometry/OrientedPlane3.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
#include <gtsam/base/numericalDerivative.h>
@ -29,7 +30,7 @@ namespace gtsam {
/* ************************************************************************* */
void OrientedPlane3::print(const string& s) const {
Vector4 coeffs = planeCoefficients();
cout << s << " : " << coeffs << endl;
cout << s << " : " << coeffs.transpose() << endl;
}
/* ************************************************************************* */
@ -43,15 +44,14 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
double pred_d = n_.unitVector().dot(xr.translation()) + d_;
if (Hr) {
*Hr = Matrix::Zero(3,6);
Hr->setZero();
Hr->block<2, 3>(0, 0) = D_rotated_plane;
Hr->block<1, 3>(2, 3) = unit_vec;
}
if (Hp) {
Vector2 hpp = n_.basis().transpose() * xr.translation();
*Hp = Z_3x3;
Hp->setZero();
Hp->block<2, 2>(0, 0) = D_rotated_pose;
Hp->block<1, 2>(2, 0) = hpp;
Hp->block<1, 2>(2, 0) = n_.basis().transpose() * xr.translation();
(*Hp)(2, 2) = 1;
}

8
gtsam/geometry/OrientedPlane3.h
View File

@ -24,6 +24,9 @@
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>
#include <algorithm>
#include <string>
namespace gtsam {
/**
@ -58,9 +61,8 @@ public:
}
/// Construct from a vector of plane coefficients
OrientedPlane3(const Vector4& vec) :
n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
}
explicit OrientedPlane3(const Vector4& vec)
: n_(vec(0), vec(1), vec(2)), d_(vec(3)) {}
/// Construct from four numbers of plane coeffcients (a, b, c, d)
OrientedPlane3(double a, double b, double c, double d) {

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

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)));
}
/* ************************************************************************* */

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

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

53
gtsam/gtsam.i
View File

@ -1936,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);
@ -2056,6 +2071,7 @@ class NonlinearFactorGraph {
// enabling serialization functionality
void serialize() const;
void saveGraph(const string& s) const;
};
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -2504,7 +2520,7 @@ class NonlinearISAM {
#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::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;
@ -2583,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>
@ -2606,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;
};
@ -2650,7 +2669,7 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> Gene
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;
@ -2751,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 {
@ -2761,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);
@ -2904,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 {
@ -2920,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 {

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

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

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);
}
/* ************************************************************************* */

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:

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;

11
gtsam/nonlinear/NonlinearOptimizerParams.h
View File

@ -113,6 +113,17 @@ public:
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);

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

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

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;

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

@ -175,10 +175,11 @@ TEST(Values, basic_functions)
{
Values values;
const Values& values_c = values;
values.insert(2, Vector3());
values.insert(4, Vector3());
values.insert(6, Matrix23());
values.insert(8, Matrix23());
Matrix23 M1 = Matrix23::Zero(), M2 = Matrix23::Zero();
values.insert(2, Vector3(0, 0, 0));
values.insert(4, Vector3(0, 0, 0));
values.insert(6, M1);
values.insert(8, M2);
// find
EXPECT_LONGS_EQUAL(4, values.find(4)->key);
@ -335,7 +336,7 @@ TEST(Values, filter) {
int i = 0;
Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
EXPECT_LONGS_EQUAL(2, (long)filtered.size());
for(const Values::Filtered<>::KeyValuePair& key_value: filtered) {
for(const auto key_value: filtered) {
if(i == 0) {
LONGS_EQUAL(2, (long)key_value.key);
try {key_value.value.cast<Pose2>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");}
@ -370,7 +371,7 @@ TEST(Values, filter) {
i = 0;
Values::ConstFiltered<Pose3> pose_filtered = values.filter<Pose3>();
EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size());
for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: pose_filtered) {
for(const auto key_value: pose_filtered) {
if(i == 0) {
EXPECT_LONGS_EQUAL(1, (long)key_value.key);
EXPECT(assert_equal(pose1, key_value.value));
@ -408,7 +409,7 @@ TEST(Values, Symbol_filter) {
values.insert(Symbol('y', 3), pose3);
int i = 0;
for(const Values::Filtered<Value>::KeyValuePair& key_value: values.filter(Symbol::ChrTest('y'))) {
for(const auto key_value: values.filter(Symbol::ChrTest('y'))) {
if(i == 0) {
LONGS_EQUAL(Symbol('y', 1), (long)key_value.key);
EXPECT(assert_equal(pose1, key_value.value.cast<Pose3>()));

7
gtsam/sfm/BinaryMeasurement.h
View File

@ -45,10 +45,11 @@ private:
T measured_; ///< The measurement
SharedNoiseModel noiseModel_; ///< Noise model
public:
public:
BinaryMeasurement(Key key1, Key key2, const T &measured,
const SharedNoiseModel &model = nullptr)
: Factor(std::vector<Key>({key1, key2})), measured_(measured),
: Factor(std::vector<Key>({key1, key2})),
measured_(measured),
noiseModel_(model) {}
/// @name Standard Interface
@ -80,4 +81,4 @@ public:
}
/// @}
};
} // namespace gtsam
} // namespace gtsam

208
gtsam/sfm/ShonanAveraging.cpp
View File

@ -17,6 +17,7 @@
*/
#include <SymEigsSolver.h>
#include <cmath>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -53,7 +54,9 @@ ShonanAveragingParameters<d>::ShonanAveragingParameters(
optimalityThreshold(optimalityThreshold),
alpha(alpha),
beta(beta),
gamma(gamma) {
gamma(gamma),
useHuber(false),
certifyOptimality(true) {
// By default, we will do conjugate gradient
lm.linearSolverType = LevenbergMarquardtParams::Iterative;
@ -140,25 +143,23 @@ template <size_t d>
NonlinearFactorGraph ShonanAveraging<d>::buildGraphAt(size_t p) const {
NonlinearFactorGraph graph;
auto G = boost::make_shared<Matrix>(SO<-1>::VectorizedGenerators(p));
for (const auto &measurement : measurements_) {
const auto &keys = measurement.keys();
const auto &Rij = measurement.measured();
const auto &model = measurement.noiseModel();
graph.emplace_shared<ShonanFactor<d>>(keys[0], keys[1], Rij, p, model, G);
}
// Possibly add Karcher prior
if (parameters_.beta > 0) {
const size_t dim = SOn::Dimension(p);
graph.emplace_shared<KarcherMeanFactor<SOn>>(graph.keys(), dim);
}
// Possibly add gauge factors - they are probably useless as gradient is zero
if (parameters_.gamma > 0 && p > d + 1) {
for (auto key : graph.keys())
graph.emplace_shared<ShonanGaugeFactor>(key, p, d, parameters_.gamma);
}
return graph;
}
@ -186,7 +187,6 @@ ShonanAveraging<d>::createOptimizerAt(size_t p, const Values &initial) const {
graph.emplace_shared<PriorFactor<SOn>>(i, SOn::Lift(p, value.matrix()),
model);
}
// Optimize
return boost::make_shared<LevenbergMarquardtOptimizer>(graph, initial,
parameters_.lm);
@ -334,15 +334,33 @@ double ShonanAveraging<d>::cost(const Values &values) const {
/* ************************************************************************* */
// Get kappa from noise model
template <typename T>
static double Kappa(const BinaryMeasurement<T> &measurement) {
template <typename T, size_t d>
static double Kappa(const BinaryMeasurement<T> &measurement,
const ShonanAveragingParameters<d> &parameters) {
const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
measurement.noiseModel());
if (!isotropic) {
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic.");
double sigma;
if (isotropic) {
sigma = isotropic->sigma();
} else {
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
measurement.noiseModel());
// Check if noise model is robust
if (robust) {
// If robust, check if optimality certificate is expected
if (parameters.getCertifyOptimality()) {
throw std::invalid_argument(
"Certification of optimality does not work with robust cost.");
} else {
// Optimality certificate not required, so setting default sigma
sigma = 1;
}
} else {
throw std::invalid_argument(
"Shonan averaging noise models must be isotropic (but robust losses "
"are allowed).");
}
}
const double sigma = isotropic->sigma();
return 1.0 / (sigma * sigma);
}
@ -362,7 +380,7 @@ Sparse ShonanAveraging<d>::buildD() const {
const auto &keys = measurement.keys();
// Get kappa from noise model
double kappa = Kappa<Rot>(measurement);
double kappa = Kappa<Rot, d>(measurement, parameters_);
const size_t di = d * keys[0], dj = d * keys[1];
for (size_t k = 0; k < d; k++) {
@ -400,7 +418,7 @@ Sparse ShonanAveraging<d>::buildQ() const {
const auto Rij = measurement.measured().matrix();
// Get kappa from noise model
double kappa = Kappa<Rot>(measurement);
double kappa = Kappa<Rot, d>(measurement, parameters_);
const size_t di = d * keys[0], dj = d * keys[1];
for (size_t r = 0; r < d; r++) {
@ -470,8 +488,88 @@ Sparse ShonanAveraging<d>::computeA(const Values &values) const {
return Lambda - Q_;
}
/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
auto Lambda = computeLambda(S);
return Lambda - Q_;
}
/* ************************************************************************* */
// Perturb the initial initialVector by adding a spherically-uniformly
// distributed random vector with 0.03*||initialVector||_2 magnitude to
// initialVector
// ref : Part III. C, Rosen, D. and Carlone, L., 2017, September. Computational
// enhancements for certifiably correct SLAM. In Proceedings of the
// International Conference on Intelligent Robots and Systems.
static Vector perturb(const Vector &initialVector) {
// generate a 0.03*||x_0||_2 as stated in David's paper
int n = initialVector.rows();
Vector disturb = Vector::Random(n);
disturb.normalize();
double magnitude = initialVector.norm();
Vector perturbedVector = initialVector + 0.03 * magnitude * disturb;
perturbedVector.normalize();
return perturbedVector;
}
/* ************************************************************************* */
/// MINIMUM EIGENVALUE COMPUTATIONS
// Alg.6 from paper Distributed Certifiably Correct Pose-Graph Optimization,
// it takes in the certificate matrix A as input, the maxIterations and the
// minEigenvalueNonnegativityTolerance is set to 1000 and 10e-4 ad default,
// there are two parts
// in this algorithm:
// (1) compute the maximum eigenpair (\lamda_dom, \vect{v}_dom) of A by power
// method. if \lamda_dom is less than zero, then return the eigenpair. (2)
// compute the maximum eigenpair (\theta, \vect{v}) of C = \lamda_dom * I - A by
// accelerated power method. Then return (\lamda_dom - \theta, \vect{v}).
static bool PowerMinimumEigenValue(
const Sparse &A, const Matrix &S, double &minEigenValue,
Vector *minEigenVector = 0, size_t *numIterations = 0,
size_t maxIterations = 1000,
double minEigenvalueNonnegativityTolerance = 10e-4) {
// a. Compute dominant eigenpair of S using power method
PowerMethod<Sparse> pmOperator(A);
const bool pmConverged = pmOperator.compute(
maxIterations, 1e-5);
// Check convergence and bail out if necessary
if (!pmConverged) return false;
const double pmEigenValue = pmOperator.eigenvalue();
if (pmEigenValue < 0) {
// The largest-magnitude eigenvalue is negative, and therefore also the
// minimum eigenvalue, so just return this solution
minEigenValue = pmEigenValue;
if (minEigenVector) {
*minEigenVector = pmOperator.eigenvector();
minEigenVector->normalize(); // Ensure that this is a unit vector
}
return true;
}
const Sparse C = pmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A;
const boost::optional<Vector> initial = perturb(S.row(0));
AcceleratedPowerMethod<Sparse> apmShiftedOperator(C, initial);
const bool minConverged = apmShiftedOperator.compute(
maxIterations, minEigenvalueNonnegativityTolerance / pmEigenValue);
if (!minConverged) return false;
minEigenValue = pmEigenValue - apmShiftedOperator.eigenvalue();
if (minEigenVector) {
*minEigenVector = apmShiftedOperator.eigenvector();
minEigenVector->normalize(); // Ensure that this is a unit vector
}
if (numIterations) *numIterations = apmShiftedOperator.nrIterations();
return true;
}
/** This is a lightweight struct used in conjunction with Spectra to compute
* the minimum eigenvalue and eigenvector of a sparse matrix A; it has a single
@ -617,13 +715,6 @@ static bool SparseMinimumEigenValue(
return true;
}
/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
auto Lambda = computeLambda(S);
return Lambda - Q_;
}
/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
@ -641,6 +732,23 @@ double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
return minEigenValue;
}
/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::computeMinEigenValueAP(const Values &values,
Vector *minEigenVector) const {
assert(values.size() == nrUnknowns());
const Matrix S = StiefelElementMatrix(values);
auto A = computeA(S);
double minEigenValue = 0;
bool success = PowerMinimumEigenValue(A, S, minEigenValue, minEigenVector);
if (!success) {
throw std::runtime_error(
"PowerMinimumEigenValue failed to compute minimum eigenvalue.");
}
return minEigenValue;
}
/* ************************************************************************* */
template <size_t d>
std::pair<double, Vector> ShonanAveraging<d>::computeMinEigenVector(
@ -793,21 +901,30 @@ std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
for (size_t p = pMin; p <= pMax; p++) {
// Optimize until convergence at this level
Qstar = tryOptimizingAt(p, initialSOp);
// Check certificate of global optimzality
Vector minEigenVector;
double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
if (minEigenValue > parameters_.optimalityThreshold) {
// If at global optimum, round and return solution
if (parameters_.getUseHuber() || !parameters_.getCertifyOptimality()) {
// in this case, there is no optimality certification
if (pMin != pMax) {
throw std::runtime_error(
"When using robust norm, Shonan only tests a single rank. Set pMin = pMax");
}
const Values SO3Values = roundSolution(Qstar);
return std::make_pair(SO3Values, minEigenValue);
}
return std::make_pair(SO3Values, 0);
} else {
// Check certificate of global optimality
Vector minEigenVector;
double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
if (minEigenValue > parameters_.optimalityThreshold) {
// If at global optimum, round and return solution
const Values SO3Values = roundSolution(Qstar);
return std::make_pair(SO3Values, minEigenValue);
}
// Not at global optimimum yet, so check whether we will go to next level
if (p != pMax) {
// Calculate initial estimate for next level by following minEigenVector
initialSOp =
initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue);
// Not at global optimimum yet, so check whether we will go to next level
if (p != pMax) {
// Calculate initial estimate for next level by following minEigenVector
initialSOp =
initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue);
}
}
}
throw std::runtime_error("Shonan::run did not converge for given pMax");
@ -819,9 +936,13 @@ template class ShonanAveraging<2>;
ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<2>(measurements, parameters) {}
: ShonanAveraging<2>(maybeRobust(measurements, parameters.getUseHuber()),
parameters) {}
ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
: ShonanAveraging<2>(parseMeasurements<Rot2>(g2oFile), parameters) {}
: ShonanAveraging<2>(maybeRobust(parseMeasurements<Rot2>(g2oFile),
parameters.getUseHuber()),
parameters) {}
/* ************************************************************************* */
// Explicit instantiation for d=3
@ -829,10 +950,13 @@ template class ShonanAveraging<3>;
ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters)
: ShonanAveraging<3>(measurements, parameters) {}
: ShonanAveraging<3>(maybeRobust(measurements, parameters.getUseHuber()),
parameters) {}
ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
: ShonanAveraging<3>(parseMeasurements<Rot3>(g2oFile), parameters) {}
: ShonanAveraging<3>(maybeRobust(parseMeasurements<Rot3>(g2oFile),
parameters.getUseHuber()),
parameters) {}
// TODO(frank): Deprecate after we land pybind wrapper
@ -847,9 +971,9 @@ static BinaryMeasurement<Rot3> convert(
"parseMeasurements<Rot3> can only convert Pose3 measurements "
"with Gaussian noise models.");
const Matrix6 M = gaussian->covariance();
return BinaryMeasurement<Rot3>(
f->key1(), f->key2(), f->measured().rotation(),
noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3));
return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
model);
}
static ShonanAveraging3::Measurements extractRot3Measurements(
@ -862,7 +986,9 @@ static ShonanAveraging3::Measurements extractRot3Measurements(
ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
const Parameters &parameters)
: ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {}
: ShonanAveraging<3>(maybeRobust(extractRot3Measurements(factors),
parameters.getUseHuber()),
parameters) {}
/* ************************************************************************* */
} // namespace gtsam

105
gtsam/sfm/ShonanAveraging.h
View File

@ -26,6 +26,8 @@
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/linear/PowerMethod.h>
#include <gtsam/linear/AcceleratedPowerMethod.h>
#include <gtsam/slam/dataset.h>
#include <Eigen/Sparse>
@ -46,13 +48,17 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
using Anchor = std::pair<size_t, Rot>;
// Paremeters themselves:
LevenbergMarquardtParams lm; // LM parameters
double optimalityThreshold; // threshold used in checkOptimality
Anchor anchor; // pose to use as anchor if not Karcher
double alpha; // weight of anchor-based prior (default 0)
double beta; // weight of Karcher-based prior (default 1)
double gamma; // weight of gauge-fixing factors (default 0)
// Parameters themselves:
LevenbergMarquardtParams lm; ///< LM parameters
double optimalityThreshold; ///< threshold used in checkOptimality
Anchor anchor; ///< pose to use as anchor if not Karcher
double alpha; ///< weight of anchor-based prior (default 0)
double beta; ///< weight of Karcher-based prior (default 1)
double gamma; ///< weight of gauge-fixing factors (default 0)
/// if enabled, the Huber loss is used (default false)
bool useHuber;
/// if enabled solution optimality is certified (default true)
bool certifyOptimality;
ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
LevenbergMarquardtParams::CeresDefaults(),
@ -67,16 +73,31 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
double getOptimalityThreshold() const { return optimalityThreshold; }
void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
std::pair<size_t, Rot> getAnchor() { return anchor; }
std::pair<size_t, Rot> getAnchor() const { return anchor; }
void setAnchorWeight(double value) { alpha = value; }
double getAnchorWeight() { return alpha; }
double getAnchorWeight() const { return alpha; }
void setKarcherWeight(double value) { beta = value; }
double getKarcherWeight() { return beta; }
double getKarcherWeight() const { return beta; }
void setGaugesWeight(double value) { gamma = value; }
double getGaugesWeight() { return gamma; }
double getGaugesWeight() const { return gamma; }
void setUseHuber(bool value) { useHuber = value; }
bool getUseHuber() const { return useHuber; }
void setCertifyOptimality(bool value) { certifyOptimality = value; }
bool getCertifyOptimality() const { return certifyOptimality; }
/// Print the parameters and flags used for rotation averaging.
void print() const {
std::cout << " ShonanAveragingParameters: " << std::endl;
std::cout << " alpha: " << alpha << std::endl;
std::cout << " beta: " << beta << std::endl;
std::cout << " gamma: " << gamma << std::endl;
std::cout << " useHuber: " << useHuber << std::endl;
}
};
using ShonanAveragingParameters2 = ShonanAveragingParameters<2>;
@ -107,7 +128,6 @@ class GTSAM_EXPORT ShonanAveraging {
using Rot = typename Parameters::Rot;
// We store SO(d) BetweenFactors to get noise model
// TODO(frank): use BinaryMeasurement?
using Measurements = std::vector<BinaryMeasurement<Rot>>;
private:
@ -151,6 +171,36 @@ class GTSAM_EXPORT ShonanAveraging {
return measurements_[k];
}
/**
* Update factors to use robust Huber loss.
*
* @param measurements Vector of BinaryMeasurements.
* @param k Huber noise model threshold.
*/
Measurements makeNoiseModelRobust(const Measurements &measurements,
double k = 1.345) const {
Measurements robustMeasurements;
for (auto &measurement : measurements) {
auto model = measurement.noiseModel();
const auto &robust =
boost::dynamic_pointer_cast<noiseModel::Robust>(model);
SharedNoiseModel robust_model;
// Check if the noise model is already robust
if (robust) {
robust_model = model;
} else {
// make robust
robust_model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(k), model);
}
BinaryMeasurement<Rot> meas(measurement.key1(), measurement.key2(),
measurement.measured(), robust_model);
robustMeasurements.push_back(meas);
}
return robustMeasurements;
}
/// k^th measurement, as a Rot.
const Rot &measured(size_t k) const { return measurements_[k].measured(); }
@ -202,6 +252,13 @@ class GTSAM_EXPORT ShonanAveraging {
double computeMinEigenValue(const Values &values,
Vector *minEigenVector = nullptr) const;
/**
* Compute minimum eigenvalue with accelerated power method.
* @param values: should be of type SOn
*/
double computeMinEigenValueAP(const Values &values,
Vector *minEigenVector = nullptr) const;
/// Project pxdN Stiefel manifold matrix S to Rot3^N
Values roundSolutionS(const Matrix &S) const;
@ -345,24 +402,40 @@ class GTSAM_EXPORT ShonanAveraging {
std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
size_t pMax = 10) const;
/// @}
/**
* Helper function to convert measurements to robust noise model
* if flag is set.
*
* @tparam T the type of measurement, e.g. Rot3.
* @param measurements vector of BinaryMeasurements of type T.
* @param useRobustModel flag indicating whether use robust noise model
* instead.
*/
template <typename T>
inline std::vector<BinaryMeasurement<T>> maybeRobust(
const std::vector<BinaryMeasurement<T>> &measurements,
bool useRobustModel = false) const {
return useRobustModel ? makeNoiseModelRobust(measurements) : measurements;
}
};
// Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a
// convenience interface with file access.
class ShonanAveraging2 : public ShonanAveraging<2> {
class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> {
public:
ShonanAveraging2(const Measurements &measurements,
const Parameters &parameters = Parameters());
explicit ShonanAveraging2(string g2oFile,
explicit ShonanAveraging2(std::string g2oFile,
const Parameters &parameters = Parameters());
};
class ShonanAveraging3 : public ShonanAveraging<3> {
class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> {
public:
ShonanAveraging3(const Measurements &measurements,
const Parameters &parameters = Parameters());
explicit ShonanAveraging3(string g2oFile,
explicit ShonanAveraging3(std::string g2oFile,
const Parameters &parameters = Parameters());
// TODO(frank): Deprecate after we land pybind wrapper

17
gtsam/sfm/tests/testBinaryMeasurement.cpp
View File

@ -36,6 +36,7 @@ static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05));
const Unit3 unit3Measured(Vector3(1, 1, 1));
const Rot3 rot3Measured;
/* ************************************************************************* */
TEST(BinaryMeasurement, Unit3) {
BinaryMeasurement<Unit3> unit3Measurement(kKey1, kKey2, unit3Measured,
unit3_model);
@ -48,6 +49,7 @@ TEST(BinaryMeasurement, Unit3) {
EXPECT(unit3Measurement.equals(unit3MeasurementCopy));
}
/* ************************************************************************* */
TEST(BinaryMeasurement, Rot3) {
// testing the accessors
BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
@ -62,6 +64,21 @@ TEST(BinaryMeasurement, Rot3) {
EXPECT(rot3Measurement.equals(rot3MeasurementCopy));
}
/* ************************************************************************* */
TEST(BinaryMeasurement, Rot3MakeRobust) {
auto huber_model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), rot3_model);
BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
huber_model);
EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
EXPECT(rot3Measurement.measured().equals(rot3Measured));
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
rot3Measurement.noiseModel());
EXPECT(robust);
}
/* ************************************************************************* */
int main() {
TestResult tr;

42
gtsam/sfm/tests/testShonanAveraging.cpp
View File

@ -17,6 +17,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>
@ -187,9 +188,14 @@ TEST(ShonanAveraging3, CheckWithEigen) {
for (int i = 1; i < lambdas.size(); i++)
minEigenValue = min(lambdas(i), minEigenValue);
// Compute Eigenvalue with Accelerated Power method
double lambdaAP = kShonan.computeMinEigenValueAP(Qstar3);
// Actual check
EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11);
EXPECT_DOUBLES_EQUAL(0, minEigenValue, 1e-11);
EXPECT_DOUBLES_EQUAL(0, lambdaAP, 1e-11);
// Construct test descent direction (as minEigenVector is not predictable
// across platforms, being one from a basically flat 3d- subspace)
@ -342,6 +348,42 @@ TEST(ShonanAveraging2, noisyToyGraph) {
EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
}
/* ************************************************************************* */
TEST(ShonanAveraging2, noisyToyGraphWithHuber) {
// Load 2D toy example
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
string g2oFile = findExampleDataFile("noisyToyGraph.txt");
ShonanAveraging2::Parameters parameters(lmParams);
auto measurements = parseMeasurements<Rot2>(g2oFile);
parameters.setUseHuber(true);
parameters.setCertifyOptimality(false);
string parameters_print =
" ShonanAveragingParameters: \n alpha: 0\n beta: 1\n gamma: 0\n "
"useHuber: 1\n";
assert_print_equal(parameters_print, parameters);
ShonanAveraging2 shonan(measurements, parameters);
EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
// Check graph building
NonlinearFactorGraph graph = shonan.buildGraphAt(2);
EXPECT_LONGS_EQUAL(6, graph.size());
// test that each factor is actually robust
for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
boost::dynamic_pointer_cast<NoiseModelFactor>(graph[i])->noiseModel());
EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber)
}
// test result
auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
auto result = shonan.run(initial, 2,2);
EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
}
/* ************************************************************************* */
// Test alpha/beta/gamma prior weighting.
TEST(ShonanAveraging3, PriorWeights) {

25
gtsam/slam/FrobeniusFactor.cpp
View File

@ -23,17 +23,25 @@ using namespace std;
namespace gtsam {
//******************************************************************************
boost::shared_ptr<noiseModel::Isotropic>
SharedNoiseModel
ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
double sigma = 1.0;
if (model != nullptr) {
auto sigmas = model->sigmas();
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
Vector sigmas;
if (robust) {
sigmas = robust->noise()->sigmas();
} else {
sigmas = model->sigmas();
}
size_t n = sigmas.size();
if (n == 1) {
sigma = sigmas(0); // Rot2
goto exit;
}
if (n == 3 || n == 6) {
else if (n == 3 || n == 6) {
sigma = sigmas(2); // Pose2, Rot3, or Pose3
if (sigmas(0) != sigma || sigmas(1) != sigma) {
if (!defaultToUnit) {
@ -46,8 +54,15 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
throw std::runtime_error("Can only convert Pose2/Pose3 noise models");
}
}
exit:
return noiseModel::Isotropic::Sigma(d, sigma);
exit:
auto isoModel = noiseModel::Isotropic::Sigma(d, sigma);
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
if (robust) {
return noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), isoModel);
} else {
return isoModel;
}
}
//******************************************************************************

19
gtsam/slam/FrobeniusFactor.h
View File

@ -26,15 +26,20 @@
namespace gtsam {
/**
* When creating (any) FrobeniusFactor we can convert a Rot/Pose
* BetweenFactor noise model into a n-dimensional isotropic noise
* model used to weight the Frobenius norm. If the noise model passed is
* null we return a n-dimensional isotropic noise model with sigma=1.0. If
* not, we we check if the d-dimensional noise model on rotations is
* When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor
* noise model into a n-dimensional isotropic noise
* model used to weight the Frobenius norm.
* If the noise model passed is null we return a n-dimensional isotropic noise
* model with sigma=1.0.
* If not, we we check if the d-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an
* error. If defaultToUnit == false throws an exception on unexepcted input.
* error.
* If the noise model is a robust error model, we use the sigmas of the
* underlying noise model.
*
* If defaultToUnit == false throws an exception on unexepcted input.
*/
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic>
GTSAM_EXPORT SharedNoiseModel
ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
bool defaultToUnit = true);

17
gtsam/slam/GeneralSFMFactor.h
View File

@ -86,12 +86,15 @@ public:
* @param cameraKey is the index of the camera
* @param landmarkKey is the index of the landmark
*/
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
Base(model, cameraKey, landmarkKey), measured_(measured) {}
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model,
Key cameraKey, Key landmarkKey)
: Base(model, cameraKey, landmarkKey), measured_(measured) {}
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
GeneralSFMFactor() : measured_(0.0, 0.0) {} ///< default constructor
///< constructor that takes a Point2
GeneralSFMFactor(const Point2& p) : measured_(p) {}
///< constructor that takes doubles x,y to make a Point2
GeneralSFMFactor(double x, double y) : measured_(x, y) {}
virtual ~GeneralSFMFactor() {} ///< destructor
@ -127,7 +130,7 @@ public:
catch( CheiralityException& e) {
if (H1) *H1 = JacobianC::Zero();
if (H2) *H2 = JacobianL::Zero();
// TODO warn if verbose output asked for
//TODO Print the exception via logging
return Z_2x1;
}
}
@ -149,7 +152,7 @@ public:
H1.setZero();
H2.setZero();
b.setZero();
// TODO warn if verbose output asked for
//TODO Print the exception via logging
}
// Whiten the system if needed

4
gtsam/slam/InitializePose.h
View File

@ -62,7 +62,7 @@ static Values computePoses(const Values& initialRot,
// Upgrade rotations to full poses
Values initialPose;
for (const auto& key_value : initialRot) {
for (const auto key_value : initialRot) {
Key key = key_value.key;
const auto& rot = initialRot.at<typename Pose::Rotation>(key);
Pose initializedPose = Pose(rot, origin);
@ -86,7 +86,7 @@ static Values computePoses(const Values& initialRot,
// put into Values structure
Values estimate;
for (const auto& key_value : GNresult) {
for (const auto key_value : GNresult) {
Key key = key_value.key;
if (key != kAnchorKey) {
const Pose& pose = GNresult.at<Pose>(key);

8
gtsam/slam/InitializePose3.cpp
View File

@ -124,7 +124,7 @@ Values InitializePose3::computeOrientationsGradient(
// this works on the inverse rotations, according to Tron&Vidal,2011
Values inverseRot;
inverseRot.insert(initialize::kAnchorKey, Rot3());
for(const auto& key_value: givenGuess) {
for(const auto key_value: givenGuess) {
Key key = key_value.key;
const Pose3& pose = givenGuess.at<Pose3>(key);
inverseRot.insert(key, pose.rotation().inverse());
@ -139,7 +139,7 @@ Values InitializePose3::computeOrientationsGradient(
// calculate max node degree & allocate gradient
size_t maxNodeDeg = 0;
VectorValues grad;
for(const auto& key_value: inverseRot) {
for(const auto key_value: inverseRot) {
Key key = key_value.key;
grad.insert(key,Z_3x1);
size_t currNodeDeg = (adjEdgesMap.at(key)).size();
@ -162,7 +162,7 @@ Values InitializePose3::computeOrientationsGradient(
//////////////////////////////////////////////////////////////////////////
// compute the gradient at each node
maxGrad = 0;
for (const auto& key_value : inverseRot) {
for (const auto key_value : inverseRot) {
Key key = key_value.key;
Vector gradKey = Z_3x1;
// collect the gradient for each edge incident on key
@ -203,7 +203,7 @@ Values InitializePose3::computeOrientationsGradient(
// Return correct rotations
const Rot3& Rref = inverseRot.at<Rot3>(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior
Values estimateRot;
for(const auto& key_value: inverseRot) {
for(const auto key_value: inverseRot) {
Key key = key_value.key;
if (key != initialize::kAnchorKey) {
const Rot3& R = inverseRot.at<Rot3>(key);

4
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -14,7 +14,7 @@ namespace gtsam {
//***************************************************************************
void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "OrientedPlane3Factor Factor on " << landmarkKey_ << "\n";
cout << "OrientedPlane3Factor Factor on " << keyFormatter(landmarkKey_) << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
@ -44,7 +44,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
//***************************************************************************
void OrientedPlane3DirectionPrior::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "Prior Factor on " << landmarkKey_ << "\n";
cout << "Prior Factor on " << keyFormatter(landmarkKey_) << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}

6
gtsam/slam/SmartFactorBase.h
View File

@ -13,6 +13,7 @@
* @file SmartFactorBase.h
* @brief Base class to create smart factors on poses or cameras
* @author Luca Carlone
* @author Antoni Rosinol
* @author Zsolt Kira
* @author Frank Dellaert
* @author Chris Beall
@ -131,9 +132,10 @@ protected:
/**
* Add a bunch of measurements, together with the camera keys
*/
void add(ZVector& measurements, KeyVector& cameraKeys) {
void add(const ZVector& measurements, const KeyVector& cameraKeys) {
assert(measurements.size() == cameraKeys.size());
for (size_t i = 0; i < measurements.size(); i++) {
this->add(measurements.at(i), cameraKeys.at(i));
this->add(measurements[i], cameraKeys[i]);
}
}

2
gtsam/slam/dataset.cpp
View File

@ -586,7 +586,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config,
fstream stream(filename.c_str(), fstream::out);
// save poses
for (const Values::ConstKeyValuePair key_value : config) {
for (const auto key_value : config) {
const Pose2 &pose = key_value.value.cast<Pose2>();
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
<< " " << pose.theta() << endl;

130
gtsam/slam/dataset.h
View File

@ -29,6 +29,8 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/types.h>
#include <boost/smart_ptr/shared_ptr.hpp>
@ -216,13 +218,17 @@ typedef std::pair<size_t, size_t> SiftIndex;
/// Define the structure for the 3D points
struct SfmTrack {
SfmTrack(): p(0,0,0) {}
SfmTrack(const gtsam::Point3& pt) : p(pt) {}
SfmTrack(float r = 0, float g = 0, float b = 0): p(0,0,0), r(r), g(g), b(b) {}
SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, float b = 0) : p(pt), r(r), g(g), b(b) {}
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point
std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
std::vector<SiftIndex> siftIndices;
/// Get RGB values describing 3d point
const Point3 rgb() const { return Point3(r, g, b); }
/// Total number of measurements in this track
size_t number_measurements() const {
return measurements.size();
@ -243,6 +249,73 @@ struct SfmTrack {
void add_measurement(size_t idx, const gtsam::Point2& m) {
measurements.emplace_back(idx, m);
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(p);
ar & BOOST_SERIALIZATION_NVP(r);
ar & BOOST_SERIALIZATION_NVP(g);
ar & BOOST_SERIALIZATION_NVP(b);
ar & BOOST_SERIALIZATION_NVP(measurements);
ar & BOOST_SERIALIZATION_NVP(siftIndices);
}
/// assert equality up to a tolerance
bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const {
// check the 3D point
if (!p.isApprox(sfmTrack.p)) {
return false;
}
// check the RGB values
if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) {
return false;
}
// compare size of vectors for measurements and siftIndices
if (number_measurements() != sfmTrack.number_measurements() ||
siftIndices.size() != sfmTrack.siftIndices.size()) {
return false;
}
// compare measurements (order sensitive)
for (size_t idx = 0; idx < number_measurements(); ++idx) {
SfmMeasurement measurement = measurements[idx];
SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];
if (measurement.first != otherMeasurement.first ||
!measurement.second.isApprox(otherMeasurement.second)) {
return false;
}
}
// compare sift indices (order sensitive)
for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
SiftIndex index = siftIndices[idx];
SiftIndex otherIndex = sfmTrack.siftIndices[idx];
if (index.first != otherIndex.first ||
index.second != otherIndex.second) {
return false;
}
}
return true;
}
/// print
void print(const std::string& s = "") const {
std::cout << "Track with " << measurements.size();
std::cout << " measurements of point " << p << std::endl;
}
};
/* ************************************************************************* */
/// traits
template<>
struct traits<SfmTrack> : public Testable<SfmTrack> {
};
@ -269,13 +342,62 @@ struct SfmData {
return tracks[idx];
}
/// Add a track to SfmData
void add_track(const SfmTrack& t) {
void add_track(const SfmTrack& t) {
tracks.push_back(t);
}
/// Add a camera to SfmData
void add_camera(const SfmCamera& cam){
void add_camera(const SfmCamera& cam) {
cameras.push_back(cam);
}
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(cameras);
ar & BOOST_SERIALIZATION_NVP(tracks);
}
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals(const SfmData &sfmData, double tol = 1e-9) const {
// check number of cameras and tracks
if (number_cameras() != sfmData.number_cameras() ||
number_tracks() != sfmData.number_tracks()) {
return false;
}
// check each camera
for (size_t i = 0; i < number_cameras(); ++i) {
if (!camera(i).equals(sfmData.camera(i), tol)) {
return false;
}
}
// check each track
for (size_t j = 0; j < number_tracks(); ++j) {
if (!track(j).equals(sfmData.track(j), tol)) {
return false;
}
}
return true;
}
/// print
void print(const std::string& s = "") const {
std::cout << "Number of cameras = " << number_cameras() << std::endl;
std::cout << "Number of tracks = " << number_tracks() << std::endl;
}
};
/* ************************************************************************* */
/// traits
template<>
struct traits<SfmData> : public Testable<SfmData> {
};
/**

11
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -75,8 +75,15 @@ NonlinearFactorGraph graph2() {
g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information
g.add(BetweenFactor<Pose3>(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin
// random pose, but zero information
auto noise_zero_info = noiseModel::Isotropic::Precision(6, 0.0);
g.add(BetweenFactor<Pose3>(
x2, x0, Pose3(Rot3::Ypr(0.1, 0.0, 0.1), Point3(0.0, 0.0, 0.0)),
noise_zero_info));
// random pose, but zero information
g.add(BetweenFactor<Pose3>(
x0, x3, Pose3(Rot3::Ypr(0.5, -0.2, 0.2), Point3(10, 20, 30)),
noise_zero_info));
g.addPrior(x0, pose0, model);
return g;
}

4
gtsam/slam/tests/testLago.cpp
View File

@ -284,7 +284,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
Values::shared_ptr expected;
boost::tie(gmatlab, expected) = readG2o(matlabFile);
for(const Values::KeyValuePair& key_val: *expected){
for(const auto key_val: *expected){
Key k = key_val.key;
EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
}
@ -310,7 +310,7 @@ TEST( Lago, largeGraphNoisy ) {
Values::shared_ptr expected;
boost::tie(gmatlab, expected) = readG2o(matlabFile);
for(const Values::KeyValuePair& key_val: *expected){
for(const auto key_val: *expected){
Key k = key_val.key;
EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
}

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

@ -10,20 +10,24 @@
* -------------------------------------------------------------------------- */
/*
* @file testOrientedPlane3.cpp
* @file testOrientedPlane3Factor.cpp
* @date Dec 19, 2012
* @author Alex Trevor
* @brief Tests the OrientedPlane3Factor class
*/
#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std.hpp>
#include <boost/bind.hpp>
using namespace boost::assign;
using namespace gtsam;
@ -200,6 +204,175 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
EXPECT(assert_equal(expectedH3, actualH3, 1e-8));
}
/* ************************************************************************* */
// Test by Marco Camurri to debug issue #561
TEST(OrientedPlane3Factor, Issue561) {
// Typedefs
using symbol_shorthand::P; //< Planes
using symbol_shorthand::X; //< Pose3 (x,y,z,r,p,y)
using Plane = OrientedPlane3;
NonlinearFactorGraph graph;
// Setup prior factors
Pose3 x0_prior(
Rot3(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195,
0.117236322, -0.234214312, 0.0187322547, 0.972004505),
Vector3{-91.7500013, -0.47569366, -2.2});
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.addPrior<Pose3>(X(0), x0_prior, x0_noise);
// Plane p1_prior(0.211098835, 0.214292752, 0.95368543, 26.4269514);
// auto p1_noise =
// noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5});
// graph.addPrior<Plane>(P(1), p1_prior, p1_noise);
// ADDING THIS PRIOR MAKES OPTIMIZATION FAIL
// Plane p2_prior(0.301901811, 0.151751467, 0.941183717, 33.4388229);
// auto p2_noise =
// noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5});
// graph.addPrior<Plane>(P(2), p2_prior, p2_noise);
// First plane factor
Plane p1_meas = Plane(0.0638967294, 0.0755284627, 0.995094297, 2.55956073);
const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.0451801);
graph.emplace_shared<OrientedPlane3Factor>(p1_meas.planeCoefficients(),
x0_p1_noise, X(0), P(1));
// Second plane factor
Plane p2_meas = Plane(0.104902077, -0.0275756528, 0.994100165, 1.32765088);
const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.0322889);
graph.emplace_shared<OrientedPlane3Factor>(p2_meas.planeCoefficients(),
x0_p2_noise, X(0), P(2));
// Optimize
try {
// Initial values
Values initialEstimate;
Plane p1(0.211098835, 0.214292752, 0.95368543, 26.4269514);
Plane p2(0.301901811, 0.151751467, 0.941183717, 33.4388229);
Pose3 x0(
Rot3(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195,
0.117236322, -0.234214312, 0.0187322547, 0.972004505),
Vector3{-91.7500013, -0.47569366, -2.2});
initialEstimate.insert(P(1), p1);
initialEstimate.insert(P(2), p2);
initialEstimate.insert(X(0), x0);
GaussNewtonParams params;
GTSAM_PRINT(graph);
Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first
params.setOrdering(ordering);
params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution
params.setVerbosity("TERMINATION"); // show info about stopping conditions
GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
} catch (const IndeterminantLinearSystemException &e) {
std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl;
EXPECT(false); // fail if this happens
}
}
/* ************************************************************************* */
// Simplified version of the test by Marco Camurri to debug issue #561
TEST(OrientedPlane3Factor, Issue561Simplified) {
// Typedefs
using symbol_shorthand::P; //< Planes
using symbol_shorthand::X; //< Pose3 (x,y,z,r,p,y)
using Plane = OrientedPlane3;
NonlinearFactorGraph graph;
// Setup prior factors
Pose3 x0_prior(Rot3::identity(), Vector3(99, 0, 0));
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.addPrior<Pose3>(X(0), x0_prior, x0_noise);
// Two horizontal planes with different heights.
const Plane p1(0,0,1,1), p2(0,0,1,2);
auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
graph.addPrior<Plane>(P(1), p1, p1_noise);
// ADDING THIS PRIOR MAKES OPTIMIZATION FAIL
auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
graph.addPrior<Plane>(P(2), p2, p2_noise);
// First plane factor
const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05);
graph.emplace_shared<OrientedPlane3Factor>(p1.planeCoefficients(),
x0_p1_noise, X(0), P(1));
// Second plane factor
const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05);
graph.emplace_shared<OrientedPlane3Factor>(p2.planeCoefficients(),
x0_p2_noise, X(0), P(2));
// Initial values
// Just offset the initial pose by 1m. This is what we are trying to optimize.
Values initialEstimate;
Pose3 x0 = x0_prior.compose(Pose3(Rot3::identity(), Vector3(1,0,0)));
initialEstimate.insert(P(1), p1);
initialEstimate.insert(P(2), p2);
initialEstimate.insert(X(0), x0);
// For testing only
HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(initialEstimate);
const auto hessian = hessianFactor->hessianBlockDiagonal();
Matrix hessianP1 = hessian.at(P(1)),
hessianP2 = hessian.at(P(2)),
hessianX0 = hessian.at(X(0));
Eigen::JacobiSVD<Matrix> svdP1(hessianP1, Eigen::ComputeThinU),
svdP2(hessianP2, Eigen::ComputeThinU),
svdX0(hessianX0, Eigen::ComputeThinU);
double conditionNumberP1 = svdP1.singularValues()[0] / svdP1.singularValues()[2],
conditionNumberP2 = svdP2.singularValues()[0] / svdP2.singularValues()[2],
conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5];
std::cout << "Hessian P1:\n" << hessianP1 << "\n"
<< "Condition number:\n" << conditionNumberP1 << "\n"
<< "Singular values:\n" << svdP1.singularValues().transpose() << "\n"
<< "SVD U:\n" << svdP1.matrixU() << "\n" << std::endl;
std::cout << "Hessian P2:\n" << hessianP2 << "\n"
<< "Condition number:\n" << conditionNumberP2 << "\n"
<< "Singular values:\n" << svdP2.singularValues().transpose() << "\n"
<< "SVD U:\n" << svdP2.matrixU() << "\n" << std::endl;
std::cout << "Hessian X0:\n" << hessianX0 << "\n"
<< "Condition number:\n" << conditionNumberX0 << "\n"
<< "Singular values:\n" << svdX0.singularValues().transpose() << "\n"
<< "SVD U:\n" << svdX0.matrixU() << "\n" << std::endl;
// std::cout << "Hessian P2:\n" << hessianP2 << std::endl;
// std::cout << "Hessian X0:\n" << hessianX0 << std::endl;
// For testing only
// Optimize
try {
GaussNewtonParams params;
//GTSAM_PRINT(graph);
//Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first
//params.setOrdering(ordering);
// params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution
params.setVerbosity("TERMINATION"); // show info about stopping conditions
GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
EXPECT(x0_prior.equals(result.at<Pose3>(X(0))));
EXPECT(p1.equals(result.at<Plane>(P(1))));
EXPECT(p2.equals(result.at<Plane>(P(2))));
} catch (const IndeterminantLinearSystemException &e) {
std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl;
EXPECT(false); // fail if this happens
}
}
/* ************************************************************************* */
int main() {
srand(time(nullptr));

58
gtsam/slam/tests/testSerializationDataset.cpp Normal file
View File

@ -0,0 +1,58 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationDataset.cpp
* @brief serialization tests for dataset.cpp
* @author Ayush Baid
* @date Jan 1, 2021
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
TEST(dataSet, sfmDataSerialization) {
// Test the serialization of SfmData
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
// round-trip equality check on serialization and subsequent deserialization
EXPECT(equalsObj(mydata));
EXPECT(equalsXML(mydata));
EXPECT(equalsBinary(mydata));
}
/* ************************************************************************* */
TEST(dataSet, sfmTrackSerialization) {
// Test the serialization of SfmTrack
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
SfmTrack track = mydata.track(0);
// round-trip equality check on serialization and subsequent deserialization
EXPECT(equalsObj(track));
EXPECT(equalsXML(track));
EXPECT(equalsBinary(track));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

4
gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
View File

@ -308,11 +308,11 @@ int main(int argc, char** argv) {
// And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
cout << "After 15.0 seconds, each version contains to the following keys:" << endl;
cout << " Concurrent Filter Keys: " << endl;
for(const auto& key_value: concurrentFilter.getLinearizationPoint()) {
for(const auto key_value: concurrentFilter.getLinearizationPoint()) {
cout << setprecision(5) << " Key: " << key_value.key << endl;
}
cout << " Concurrent Smoother Keys: " << endl;
for(const auto& key_value: concurrentSmoother.getLinearizationPoint()) {
for(const auto key_value: concurrentSmoother.getLinearizationPoint()) {
cout << setprecision(5) << " Key: " << key_value.key << endl;
}
cout << " Fixed-Lag Smoother Keys: " << endl;

0
examples/ISAM2_SmartFactorStereo_IMU.cpp → gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
View File

7
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
View File

@ -188,7 +188,8 @@ int main(int argc, char** argv) {
smartFactors[j]->addRange(i, range);
printf("adding range %g for %d",range,(int)j);
} catch (const invalid_argument& e) {
printf("warning: omitting duplicate range %g for %d",range,(int)j);
printf("warning: omitting duplicate range %g for %d: %s", range,
(int)j, e.what());
}
cout << endl;
}
@ -233,7 +234,7 @@ int main(int argc, char** argv) {
}
}
countK = 0;
for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
for(const auto it: result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl;
if (smart) {
@ -256,7 +257,7 @@ int main(int argc, char** argv) {
// Write result to file
Values result = isam.calculateEstimate();
ofstream os("rangeResult.txt");
for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
for(const auto it: result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl;
exit(0);

4
gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
View File

@ -202,11 +202,11 @@ int main(int argc, char** argv) {
// Write result to file
Values result = isam.calculateEstimate();
ofstream os2("rangeResultLM.txt");
for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
for(const auto it: result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl;
ofstream os("rangeResult.txt");
for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
for(const auto it: result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl;
exit(0);

4
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -59,7 +59,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(
// Add the new variables to theta
theta_.insert(newTheta);
// Add new variables to the end of the ordering
for (const auto& key_value : newTheta) {
for (const auto key_value : newTheta) {
ordering_.push_back(key_value.key);
}
// Augment Delta
@ -267,7 +267,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
// Put the linearization points and deltas back for specific variables
if (enforceConsistency_ && (linearKeys_.size() > 0)) {
theta_.update(linearKeys_);
for(const auto& key_value: linearKeys_) {
for(const auto key_value: linearKeys_) {
delta_.at(key_value.key) = newDelta.at(key_value.key);
}
}

10
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -139,7 +139,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto
// Add the new variables to theta
theta_.insert(newTheta);
// Add new variables to the end of the ordering
for(const Values::ConstKeyValuePair& key_value: newTheta) {
for(const auto key_value: newTheta) {
ordering_.push_back(key_value.key);
}
// Augment Delta
@ -222,7 +222,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
// Find the set of new separator keys
KeySet newSeparatorKeys;
for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
for(const auto key_value: separatorValues_) {
newSeparatorKeys.insert(key_value.key);
}
@ -236,7 +236,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
graph.push_back(smootherShortcut_);
Values values;
values.insert(smootherSummarizationValues);
for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
for(const auto key_value: separatorValues_) {
if(!values.exists(key_value.key)) {
values.insert(key_value.key, key_value.value);
}
@ -471,7 +471,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values
// Put the linearization points and deltas back for specific variables
if(linearValues.size() > 0) {
theta.update(linearValues);
for(const Values::ConstKeyValuePair& key_value: linearValues) {
for(const auto key_value: linearValues) {
delta.at(key_value.key) = newDelta.at(key_value.key);
}
}
@ -574,7 +574,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
// Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove
KeySet newSeparatorKeys = removedFactors.keys();
for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
for(const auto key_value: separatorValues_) {
newSeparatorKeys.insert(key_value.key);
}
for(Key key: keysToMove) {

10
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -61,7 +61,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF
theta_.insert(newTheta);
// Add new variables to the end of the ordering
for(const Values::ConstKeyValuePair& key_value: newTheta) {
for(const auto key_value: newTheta) {
ordering_.push_back(key_value.key);
}
@ -135,7 +135,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa
removeFactors(filterSummarizationSlots_);
// Insert new linpoints into the values, augment the ordering, and store new dims to augment delta
for(const Values::ConstKeyValuePair& key_value: smootherValues) {
for(const auto key_value: smootherValues) {
std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value);
if(iter_inserted.second) {
// If the insert succeeded
@ -146,7 +146,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa
iter_inserted.first->value = key_value.value;
}
}
for(const Values::ConstKeyValuePair& key_value: separatorValues) {
for(const auto key_value: separatorValues) {
std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value);
if(iter_inserted.second) {
// If the insert succeeded
@ -322,7 +322,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
// Put the linearization points and deltas back for specific variables
if(separatorValues_.size() > 0) {
theta_.update(separatorValues_);
for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
for(const auto key_value: separatorValues_) {
delta_.at(key_value.key) = newDelta.at(key_value.key);
}
}
@ -372,7 +372,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
// Get the set of separator keys
gtsam::KeySet separatorKeys;
for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
for(const auto key_value: separatorValues_) {
separatorKeys.insert(key_value.key);
}

6
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -69,13 +69,13 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
int group = 1;
// Set all existing variables to Group1
if(isam2_.getLinearizationPoint().size() > 0) {
for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) {
for(const auto key_value: isam2_.getLinearizationPoint()) {
orderingConstraints->operator[](key_value.key) = group;
}
++group;
}
// Assign new variables to the root
for(const Values::ConstKeyValuePair& key_value: newTheta) {
for(const auto key_value: newTheta) {
orderingConstraints->operator[](key_value.key) = group;
}
// Set marginalizable variables to Group0
@ -201,7 +201,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
// Force iSAM2 not to relinearize anything during this iteration
FastList<Key> noRelinKeys;
for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) {
for(const auto key_value: isam2_.getLinearizationPoint()) {
noRelinKeys.push_back(key_value.key);
}

8
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
View File

@ -66,7 +66,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
// Also, mark the separator keys as fixed linearization points
FastMap<Key,int> constrainedKeys;
FastList<Key> noRelinKeys;
for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
for(const auto key_value: separatorValues_) {
constrainedKeys[key_value.key] = 1;
noRelinKeys.push_back(key_value.key);
}
@ -82,12 +82,12 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
Values values(newTheta);
// Unfortunately, we must be careful here, as some of the smoother values
// and/or separator values may have been added previously
for(const Values::ConstKeyValuePair& key_value: smootherValues_) {
for(const auto key_value: smootherValues_) {
if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
values.insert(key_value.key, smootherValues_.at(key_value.key));
}
}
for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
for(const auto key_value: separatorValues_) {
if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
values.insert(key_value.key, separatorValues_.at(key_value.key));
}
@ -246,7 +246,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
// Get the set of separator keys
KeySet separatorKeys;
for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
for(const auto key_value: separatorValues_) {
separatorKeys.insert(key_value.key);
}

2
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -64,7 +64,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
std::set<Key> KeysToKeep;
for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph
for(const auto key_value: linPoint) { // we cycle over all the keys of factorGraph
KeysToKeep.insert(key_value.key);
} // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
for(Key key: keysToMarginalize) {

2
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -560,7 +560,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues);
KeySet eliminateKeys = linearFactors->keys();
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
for(const auto key_value: filterSeparatorValues) {
eliminateKeys.erase(key_value.key);
}
KeyVector variables(eliminateKeys.begin(), eliminateKeys.end());

2
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -80,7 +80,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
std::set<Key> KeysToKeep;
for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph
for(const auto key_value: linPoint) { // we cycle over all the keys of factorGraph
KeysToKeep.insert(key_value.key);
} // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
for(Key key: keysToMarginalize) {

4
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

@ -512,7 +512,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 )
// Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
Values expectedLinearizationPoint = filterSeparatorValues;
Values actualLinearizationPoint;
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
for(const auto key_value: filterSeparatorValues) {
actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
}
CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
@ -580,7 +580,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
KeySet allkeys = LinFactorGraph->keys();
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
for(const auto key_value: filterSeparatorValues) {
allkeys.erase(key_value.key);
}
KeyVector variables(allkeys.begin(), allkeys.end());

4
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
// Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
Values expectedLinearizationPoint = filterSeparatorValues;
Values actualLinearizationPoint;
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
for(const auto key_value: filterSeparatorValues) {
actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
}
CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
@ -582,7 +582,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
KeySet allkeys = LinFactorGraph->keys();
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues)
for(const auto key_value: filterSeparatorValues)
allkeys.erase(key_value.key);
KeyVector variables(allkeys.begin(), allkeys.end());
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);

97
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp Normal file
View File

@ -0,0 +1,97 @@
/* ----------------------------------------------------------------------------
* 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 SmartStereoProjectionPoseFactor.cpp
* @brief Smart stereo factor on poses, assuming camera calibration is fixed
* @author Luca Carlone
* @author Antoni Rosinol
* @author Chris Beall
* @author Zsolt Kira
* @author Frank Dellaert
*/
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
namespace gtsam {
SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params,
const boost::optional<Pose3>& body_P_sensor)
: Base(sharedNoiseModel, params, body_P_sensor) {}
void SmartStereoProjectionPoseFactor::add(
const StereoPoint2& measured, const Key& poseKey,
const boost::shared_ptr<Cal3_S2Stereo>& K) {
Base::add(measured, poseKey);
K_all_.push_back(K);
}
void SmartStereoProjectionPoseFactor::add(
const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks) {
assert(measurements.size() == poseKeys.size());
assert(poseKeys.size() == Ks.size());
Base::add(measurements, poseKeys);
K_all_.insert(K_all_.end(), Ks.begin(), Ks.end());
}
void SmartStereoProjectionPoseFactor::add(
const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
const boost::shared_ptr<Cal3_S2Stereo>& K) {
assert(poseKeys.size() == measurements.size());
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements[i], poseKeys[i]);
K_all_.push_back(K);
}
}
void SmartStereoProjectionPoseFactor::print(
const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
for (const boost::shared_ptr<Cal3_S2Stereo>& K : K_all_) {
K->print("calibration = ");
}
Base::print("", keyFormatter);
}
bool SmartStereoProjectionPoseFactor::equals(const NonlinearFactor& p,
double tol) const {
const SmartStereoProjectionPoseFactor* e =
dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
return e && Base::equals(p, tol);
}
double SmartStereoProjectionPoseFactor::error(const Values& values) const {
if (this->active(values)) {
return this->totalReprojectionError(cameras(values));
} else { // else of active flag
return 0.0;
}
}
SmartStereoProjectionPoseFactor::Base::Cameras
SmartStereoProjectionPoseFactor::cameras(const Values& values) const {
assert(keys_.size() == K_all_.size());
Base::Cameras cameras;
for (size_t i = 0; i < keys_.size(); i++) {
Pose3 pose = values.at<Pose3>(keys_[i]);
if (Base::body_P_sensor_) {
pose = pose.compose(*(Base::body_P_sensor_));
}
cameras.push_back(StereoCamera(pose, K_all_[i]));
}
return cameras;
}
} // \ namespace gtsam

140
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -13,6 +13,7 @@
* @file SmartStereoProjectionPoseFactor.h
* @brief Smart stereo factor on poses, assuming camera calibration is fixed
* @author Luca Carlone
* @author Antoni Rosinol
* @author Chris Beall
* @author Zsolt Kira
* @author Frank Dellaert
@ -28,8 +29,9 @@ namespace gtsam {
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
* Eliminating conditionally independent sets in factor graphs:
* a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
*/
@ -41,14 +43,12 @@ namespace gtsam {
* This factor requires that values contains the involved poses (Pose3).
* @addtogroup SLAM
*/
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor {
protected:
std::vector<boost::shared_ptr<Cal3_S2Stereo> > K_all_; ///< shared pointer to calibration object (one for each camera)
public:
class SmartStereoProjectionPoseFactor : public SmartStereoProjectionFactor {
protected:
/// shared pointer to calibration object (one for each camera)
std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
@ -65,54 +65,49 @@ public:
* @param Isotropic measurement noise
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
SmartStereoProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
const boost::optional<Pose3> body_P_sensor = boost::none) :
Base(sharedNoiseModel, params, body_P_sensor) {
}
const boost::optional<Pose3>& body_P_sensor = boost::none);
/** Virtual destructor */
virtual ~SmartStereoProjectionPoseFactor() {}
virtual ~SmartStereoProjectionPoseFactor() = default;
/**
* add a new measurement and pose key
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKey is key corresponding to the camera observing the same landmark
* @param measured is the 2m dimensional location of the projection of a
* single landmark in the m view (the measurement)
* @param poseKey is key corresponding to the camera observing the same
* landmark
* @param K is the (fixed) camera calibration
*/
void add(const StereoPoint2 measured, const Key poseKey,
const boost::shared_ptr<Cal3_S2Stereo> K) {
Base::add(measured, poseKey);
K_all_.push_back(K);
}
void add(const StereoPoint2& measured, const Key& poseKey,
const boost::shared_ptr<Cal3_S2Stereo>& K);
/**
* Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing
* the same landmark
* @param Ks vector of calibration objects
*/
void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
Base::add(measurements, poseKeys);
for (size_t i = 0; i < measurements.size(); i++) {
K_all_.push_back(Ks.at(i));
}
}
void add(const std::vector<StereoPoint2>& measurements,
const KeyVector& poseKeys,
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks);
/**
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* Variant of the previous one in which we include a set of measurements with
* the same noise and calibration
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the
* same landmark
* @param K the (known) camera calibration (same for all measurements)
*/
void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
const boost::shared_ptr<Cal3_S2Stereo> K) {
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i));
K_all_.push_back(K);
}
}
void add(const std::vector<StereoPoint2>& measurements,
const KeyVector& poseKeys,
const boost::shared_ptr<Cal3_S2Stereo>& K);
/**
* print
@ -120,74 +115,45 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
for(const boost::shared_ptr<Cal3_S2Stereo>& K: K_all_)
K->print("calibration = ");
Base::print("", keyFormatter);
}
DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const SmartStereoProjectionPoseFactor *e =
dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
return e && Base::equals(p, tol);
}
virtual bool equals(const NonlinearFactor& p,
double tol = 1e-9) const override;
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(cameras(values));
} else { // else of active flag
return 0.0;
}
}
double error(const Values& values) const override;
/** return the calibration object */
inline const std::vector<boost::shared_ptr<Cal3_S2Stereo> > calibration() const {
inline std::vector<boost::shared_ptr<Cal3_S2Stereo>> calibration() const {
return K_all_;
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* @param values Values structure which must contain camera poses
* corresponding
* to keys involved in this factor
* @return vector of Values
*/
Base::Cameras cameras(const Values& values) const override {
Base::Cameras cameras;
size_t i=0;
for(const Key& k: this->keys_) {
Pose3 pose = values.at<Pose3>(k);
if (Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
StereoCamera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}
return cameras;
}
private:
Base::Cameras cameras(const Values& values) const override;
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(K_all_);
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(K_all_);
}
}; // end of class declaration
}; // end of class declaration
/// traits
template<>
struct traits<SmartStereoProjectionPoseFactor> : public Testable<
SmartStereoProjectionPoseFactor> {
};
template <>
struct traits<SmartStereoProjectionPoseFactor>
: public Testable<SmartStereoProjectionPoseFactor> {};
} // \ namespace gtsam
} // namespace gtsam

1
matlab/+gtsam/Contents.m
View File

@ -99,6 +99,7 @@
%
%% SLAM and SFM
% BearingFactor2D - class BearingFactor2D, see Doxygen page for details
% BearingFactor3D - class BearingFactor3D, see Doxygen page for details
% BearingRangeFactor2D - class BearingRangeFactor2D, see Doxygen page for details
% BetweenFactorLieMatrix - class BetweenFactorLieMatrix, see Doxygen page for details
% BetweenFactorLieScalar - class BetweenFactorLieScalar, see Doxygen page for details

115
python/CMakeLists.txt
View File

@ -4,20 +4,26 @@ if (NOT GTSAM_BUILD_PYTHON)
return()
endif()
# Common directory for storing data/datasets stored with the package.
# This will store the data in the Python site package directly.
set(GTSAM_PYTHON_DATASET_DIR "./gtsam/Data")
# Generate setup.py.
file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS)
configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in
${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py)
set(WRAP_USE_CUSTOM_PYTHON_LIBRARY ${GTSAM_USE_CUSTOM_PYTHON_LIBRARY})
set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION})
# Supply MANIFEST.in for older versions of Python
file(COPY ${PROJECT_SOURCE_DIR}/python/MANIFEST.in
DESTINATION ${GTSAM_PYTHON_BUILD_DIRECTORY})
include(PybindWrap)
############################################################
## Load the necessary files to compile the wrapper
# Load the pybind11 code
add_subdirectory(${PROJECT_SOURCE_DIR}/wrap/pybind11 pybind11)
# Set the wrapping script variable
set(PYBIND_WRAP_SCRIPT "${PROJECT_SOURCE_DIR}/wrap/scripts/pybind_wrap.py")
############################################################
add_custom_target(gtsam_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam/gtsam.i")
add_custom_target(gtsam_unstable_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i")
@ -67,55 +73,68 @@ set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam)
create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
"${GTSAM_MODULE_PATH}")
# Common directory for data/datasets stored with the package.
# This will store the data in the Python site package directly.
file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}")
# Add gtsam as a dependency to the install target
set(GTSAM_PYTHON_DEPENDENCIES gtsam_py)
if(GTSAM_UNSTABLE_BUILD_PYTHON)
set(ignore
gtsam::Point2
gtsam::Point3
gtsam::LieVector
gtsam::LieMatrix
gtsam::ISAM2ThresholdMapValue
gtsam::FactorIndices
gtsam::FactorIndexSet
gtsam::BetweenFactorPose3s
gtsam::Point2Vector
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsUnit3
gtsam::CameraSetCal3_S2
gtsam::CameraSetCal3Bundler
gtsam::KeyPairDoubleMap)
pybind_wrap(gtsam_unstable_py # target
${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
"gtsam_unstable.cpp" # generated_cpp
"gtsam_unstable" # module_name
"gtsam" # top_namespace
"${ignore}" # ignore_classes
${PROJECT_SOURCE_DIR}/python/gtsam_unstable/gtsam_unstable.tpl
gtsam_unstable # libs
"gtsam_unstable;gtsam_unstable_header" # dependencies
ON # use_boost
)
set(ignore
gtsam::Point2
gtsam::Point3
gtsam::LieVector
gtsam::LieMatrix
gtsam::ISAM2ThresholdMapValue
gtsam::FactorIndices
gtsam::FactorIndexSet
gtsam::BetweenFactorPose3s
gtsam::Point2Vector
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsUnit3
gtsam::CameraSetCal3_S2
gtsam::CameraSetCal3Bundler
gtsam::KeyPairDoubleMap)
pybind_wrap(gtsam_unstable_py # target
${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
"gtsam_unstable.cpp" # generated_cpp
"gtsam_unstable" # module_name
"gtsam" # top_namespace
"${ignore}" # ignore_classes
${PROJECT_SOURCE_DIR}/python/gtsam_unstable/gtsam_unstable.tpl
gtsam_unstable # libs
"gtsam_unstable;gtsam_unstable_header" # dependencies
ON # use_boost
)
set_target_properties(gtsam_unstable_py PROPERTIES
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
INSTALL_RPATH_USE_LINK_PATH TRUE
OUTPUT_NAME "gtsam_unstable"
LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable"
DEBUG_POSTFIX "" # Otherwise you will have a wrong name
RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name
)
set_target_properties(gtsam_unstable_py PROPERTIES
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
INSTALL_RPATH_USE_LINK_PATH TRUE
OUTPUT_NAME "gtsam_unstable"
LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable"
DEBUG_POSTFIX "" # Otherwise you will have a wrong name
RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name
)
set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable)
set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable)
# Symlink all tests .py files to build folder.
create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable"
"${GTSAM_UNSTABLE_MODULE_PATH}")
# Add gtsam_unstable to the install target
list(APPEND GTSAM_PYTHON_DEPENDENCIES gtsam_unstable_py)
# Symlink all tests .py files to build folder.
create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable"
"${GTSAM_UNSTABLE_MODULE_PATH}")
endif()
# Add custom target so we can install with `make python-install`
set(GTSAM_PYTHON_INSTALL_TARGET python-install)
add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET}
COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install
DEPENDS gtsam_py gtsam_unstable_py
DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}
WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY})

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