diff --git a/.cproject b/.cproject index 1134896ad..fa20e5794 100644 --- a/.cproject +++ b/.cproject @@ -532,14 +532,6 @@ true true - - make - -j4 - testSimilarity3.run - true - true - true - make -j2 @@ -755,6 +747,14 @@ true true + + make + -j4 + testSimilarity3.run + true + true + true + make -j5 @@ -1035,6 +1035,14 @@ true true + + make + -j4 + testCyclic.run + true + true + true + make -j2 @@ -1301,7 +1309,6 @@ make - testSimulated2DOriented.run true false @@ -1341,7 +1348,6 @@ make - testSimulated2D.run true false @@ -1349,7 +1355,6 @@ make - testSimulated3D.run true false @@ -1453,6 +1458,7 @@ make + testErrors.run true false @@ -1763,6 +1769,7 @@ cpack + -G DEB true false @@ -1770,6 +1777,7 @@ cpack + -G RPM true false @@ -1777,6 +1785,7 @@ cpack + -G TGZ true false @@ -1784,6 +1793,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -1975,7 +1985,6 @@ make - tests/testGaussianISAM2 true false @@ -2037,22 +2046,6 @@ true true - - make - -j5 - testExpressionMeta.run - true - true - true - - - make - -j4 - testCustomChartExpression.run - true - true - true - make -j5 @@ -2127,6 +2120,7 @@ make + tests/testBayesTree.run true false @@ -2134,6 +2128,7 @@ make + testBinaryBayesNet.run true false @@ -2181,6 +2176,7 @@ make + testSymbolicBayesNet.run true false @@ -2188,6 +2184,7 @@ make + tests/testSymbolicFactor.run true false @@ -2195,6 +2192,7 @@ make + testSymbolicFactorGraph.run true false @@ -2210,6 +2208,7 @@ make + tests/testBayesTree true false @@ -2783,6 +2782,14 @@ true true + + make + -j4 + testExecutionTrace.run + true + true + true + make -j5 @@ -3191,6 +3198,14 @@ true true + + make + -j4 + testGroup.run + true + true + true + make -j5 @@ -3329,6 +3344,7 @@ make + testGraph.run true false @@ -3336,6 +3352,7 @@ make + testJunctionTree.run true false @@ -3343,6 +3360,7 @@ make + testSymbolicBayesNetB.run true false @@ -3412,6 +3430,14 @@ true true + + make + -j4 + testLie.run + true + true + true + make -j2 diff --git a/CMakeLists.txt b/CMakeLists.txt index 38ee89760..a77173ba0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,6 +158,12 @@ else() set(GTSAM_USE_TBB 0) # This will go into config.h endif() +############################################################################### +# Prohibit Timing build mode in combination with TBB +if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") +endif() + ############################################################################### # Find Google perftools @@ -192,36 +198,40 @@ endif() ############################################################################### # Option for using system Eigen or GTSAM-bundled Eigen -### Disabled until our patches are included in Eigen ### +### These patches only affect usage of MKL. If you want to enable MKL, you *must* +### use our patched version of Eigen ### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) ### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) -### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization) -# option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) -set(GTSAM_USE_SYSTEM_EIGEN OFF) +option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) - # Use generic Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "") - find_package(Eigen3 REQUIRED) include_directories(AFTER "${EIGEN3_INCLUDE_DIR}") + + # Use generic Eigen include paths e.g. + set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") + + # check if MKL is also enabled - can have one or the other, but not both! + if(EIGEN_USE_MKL_ALL) + message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL") + endif() else() - # Use bundled Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/") - + # Use bundled Eigen include path. # Clear any variables set by FindEigen3 if(EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) endif() + # Add the bundled version of eigen to the include path so that it can still be included + # with #include + include_directories(BEFORE "gtsam/3rdparty/Eigen/") + + # set full path to be used by external projects + # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in + set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/") + endif() -# Write Eigen include file with the paths for either the system Eigen or the GTSAM-bundled Eigen -configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eigen_includes.h) - -# Install the configuration file for Eigen -install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty) - ############################################################################### # Global compile options @@ -389,6 +399,11 @@ if(NOT MSVC AND NOT XCODE_VERSION) message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") endif() +if(GTSAM_USE_SYSTEM_EIGEN) + message(STATUS " Use System Eigen : Yes") +else() + message(STATUS " Use System Eigen : No") +endif() if(GTSAM_USE_TBB) message(STATUS " Use Intel TBB : Yes") elseif(TBB_FOUND) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 1bead58d8..c2cd7b449 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -53,11 +53,12 @@ if(NOT FIRST_PASS_DONE) endif() endif() -# Clang on Mac uses a template depth that is less than standard and is too small +# Clang uses a template depth that is less than standard and is too small if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") - if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") - endif() + # Apple Clang before 5.0 does not support -ftemplate-depth. + if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") + endif() endif() # Set up build type library postfixes @@ -97,7 +98,8 @@ if( NOT cmake_build_type_tolower STREQUAL "" AND NOT cmake_build_type_tolower STREQUAL "release" AND NOT cmake_build_type_tolower STREQUAL "timing" AND NOT cmake_build_type_tolower STREQUAL "profiling" - AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo") + AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo" + AND NOT cmake_build_type_tolower STREQUAL "minsizerel") message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).") endif() diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index 2da2d02c2..d1d3d93dd 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -367,6 +367,11 @@ endfunction() # should be installed to all build type toolboxes function(install_matlab_scripts source_directory patterns) set(patterns_args "") + set(exclude_patterns "") + if(NOT GTSAM_WRAP_SERIALIZATION) + set(exclude_patterns "testSerialization.m") + endif() + foreach(pattern ${patterns}) list(APPEND patterns_args PATTERN "${pattern}") endforeach() @@ -381,10 +386,10 @@ function(install_matlab_scripts source_directory patterns) # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) - install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE) + install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE) + install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 923b0b9de..0393affe1 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -31,28 +31,29 @@ * */ -#include -#include -#include -#include -#include #include #include -#include -#include -#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include -#include -#include -#include #include -#include +#include #include #include #include +#include + +#include +#include #ifdef GTSAM_USE_TBB #include @@ -72,23 +73,6 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -//GTSAM_VALUE_EXPORT(Value); -//GTSAM_VALUE_EXPORT(Pose); -//GTSAM_VALUE_EXPORT(Rot2); -//GTSAM_VALUE_EXPORT(Point2); -//GTSAM_VALUE_EXPORT(NonlinearFactor); -//GTSAM_VALUE_EXPORT(NoiseModelFactor); -//GTSAM_VALUE_EXPORT(NM1); -//GTSAM_VALUE_EXPORT(NM2); -//GTSAM_VALUE_EXPORT(BetweenFactor); -//GTSAM_VALUE_EXPORT(PriorFactor); -//GTSAM_VALUE_EXPORT(BR); -//GTSAM_VALUE_EXPORT(noiseModel::Base); -//GTSAM_VALUE_EXPORT(noiseModel::Isotropic); -//GTSAM_VALUE_EXPORT(noiseModel::Gaussian); -//GTSAM_VALUE_EXPORT(noiseModel::Diagonal); -//GTSAM_VALUE_EXPORT(noiseModel::Unit); - double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { // Compute degrees of freedom (observations - variables) // In ocaml, +1 was added to the observations to account for the prior, but @@ -269,12 +253,12 @@ void runIncremental() boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { Key key1 = measurement->key1(), key2 = measurement->key2(); - if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) { + if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { // We found an odometry starting at firstStep firstPose = std::min(key1, key2); break; } - if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) { + if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) { // We found an odometry joining firstStep with a previous pose havePreviousPose = true; firstPose = std::max(key1, key2); @@ -303,7 +287,9 @@ void runIncremental() cout << "Playing forward time steps..." << endl; - for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step) + for (size_t step = firstPose; + nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep); + ++step) { Values newVariables; NonlinearFactorGraph newFactors; diff --git a/gtsam.h b/gtsam.h index 33e95b558..b9edfe14f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -288,6 +288,32 @@ class Point2 { void serialize() const; }; +// std::vector +class Point2Vector +{ + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); + + //Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + //Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; + + //Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; + class StereoPoint2 { // Standard Constructors StereoPoint2(); @@ -304,8 +330,6 @@ class StereoPoint2 { gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::StereoPoint2 retract(Vector v) const; Vector localCoordinates(const gtsam::StereoPoint2& p) const; @@ -550,6 +574,16 @@ class Pose3 { void serialize() const; }; +// std::vector +class Pose3Vector +{ + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& x); +}; + #include class Unit3 { // Standard Constructors @@ -788,56 +822,16 @@ class CalibratedCamera { void serialize() const; }; -class SimpleCamera { - // Standard Constructors and Named Constructors - SimpleCamera(); - SimpleCamera(const gtsam::Pose3& pose); - SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, - const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, - const gtsam::Point3& target, const gtsam::Point3& upVector, - const gtsam::Cal3_S2& K); - - // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - gtsam::Cal3_S2 calibration(); - - // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::SimpleCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; -}; - -template +template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); - static This Level(const gtsam::Cal3DS2& K, - const gtsam::Pose2& pose, double height); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); static This Level(const gtsam::Pose2& pose, double height); - static This Lookat(const gtsam::Point3& eye, - const gtsam::Point3& target, const gtsam::Point3& upVector, - const gtsam::Cal3DS2& K); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); // Testable void print(string s) const; @@ -865,6 +859,50 @@ class PinholeCamera { void serialize() const; }; +virtual class SimpleCamera { + // Standard Constructors and Named Constructors + SimpleCamera(); + SimpleCamera(const gtsam::Pose3& pose); + SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); + static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); + + // Testable + void print(string s) const; + bool equals(const gtsam::SimpleCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + gtsam::Cal3_S2 calibration() const; + + // Manifold + gtsam::SimpleCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::SimpleCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; + +}; + +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; + class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); @@ -893,6 +931,16 @@ class StereoCamera { void serialize() const; }; +#include + +// Templates appear not yet supported for free functions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); + //************************************************************************* // Symbolic //************************************************************************* @@ -2176,9 +2224,6 @@ class NonlinearISAM { //************************************************************************* // Nonlinear factor types //************************************************************************* -#include -#include -#include #include #include #include diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 301548dcf..4adbfb250 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -58,10 +58,10 @@ add_subdirectory(ceres) include(GeographicLib/cmake/FindGeographicLib.cmake) # Set up the option to install GeographicLib -if(GEOGRAPHICLIB_FOUND) - set(install_geographiclib_default OFF) -else() +if(GEOGRAPHICLIB-NOTFOUND) set(install_geographiclib_default ON) +else() + set(install_geographiclib_default OFF) endif() option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default}) diff --git a/gtsam/3rdparty/ceres/eigen.h b/gtsam/3rdparty/ceres/eigen.h index 18a602cf4..a25fde97f 100644 --- a/gtsam/3rdparty/ceres/eigen.h +++ b/gtsam/3rdparty/ceres/eigen.h @@ -31,7 +31,7 @@ #ifndef CERES_INTERNAL_EIGEN_H_ #define CERES_INTERNAL_EIGEN_H_ -#include +#include namespace ceres { diff --git a/gtsam/3rdparty/ceres/fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h index db1591636..455fce383 100644 --- a/gtsam/3rdparty/ceres/fixed_array.h +++ b/gtsam/3rdparty/ceres/fixed_array.h @@ -33,7 +33,7 @@ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #include -#include +#include #include #include diff --git a/gtsam/3rdparty/ceres/jet.h b/gtsam/3rdparty/ceres/jet.h index 12d4e8bc9..4a7683f50 100644 --- a/gtsam/3rdparty/ceres/jet.h +++ b/gtsam/3rdparty/ceres/jet.h @@ -162,7 +162,7 @@ #include #include -#include +#include #include namespace ceres { diff --git a/gtsam/3rdparty/gtsam_eigen_includes.h.in b/gtsam/3rdparty/gtsam_eigen_includes.h.in deleted file mode 100644 index f53e37f07..000000000 --- a/gtsam/3rdparty/gtsam_eigen_includes.h.in +++ /dev/null @@ -1,33 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file gtsam_eigen_includes.h - * @brief File to include the Eigen headers that we use - generated by CMake - * @author Richard Roberts - */ - -#pragma once - -#ifndef MKL_BLAS -#define MKL_BLAS MKL_DOMAIN_BLAS -#endif - -#cmakedefine EIGEN_USE_MKL_ALL // This is also defined in config.h -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Dense> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/QR> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry> - - - - diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 0ebd6c07d..e26d85ff8 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -131,12 +131,6 @@ else() set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) endif() -# Set dataset paths -set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp" - APPEND PROPERTY COMPILE_DEFINITIONS - "SOURCE_TREE_DATASET_DIR=\"${PROJECT_SOURCE_DIR}/examples/Data\"" - "INSTALLED_DATASET_DIR=\"${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data\"") - # Special cases if(MSVC) set_property(SOURCE diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 336ea7e05..b8388057d 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -95,7 +95,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void save(Archive& ar, const unsigned int version) const + void save(Archive& ar, const unsigned int /*version*/) const { // Copy to an STL container and serialize that FastVector > map(this->size()); @@ -103,7 +103,7 @@ private: ar & BOOST_SERIALIZATION_NVP(map); } template - void load(Archive& ar, const unsigned int version) + void load(Archive& ar, const unsigned int /*version*/) { // Load into STL container and then fill our map FastVector > map; diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 78155d308..f01156bd6 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -129,7 +129,7 @@ public: protected: /// Assignment operator, protected because only the Value or DERIVED /// assignment operators should be used. - DerivedValue& operator=(const DerivedValue& rhs) { + DerivedValue& operator=(const DerivedValue& /*rhs*/) { // Nothing to do, do not call base class assignment operator return *this; } diff --git a/gtsam/base/FastDefaultAllocator.h b/gtsam/base/FastDefaultAllocator.h index 156a87f55..bf5cfc498 100644 --- a/gtsam/base/FastDefaultAllocator.h +++ b/gtsam/base/FastDefaultAllocator.h @@ -17,8 +17,7 @@ */ #pragma once - -#include +#include // Configuration from CMake #if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL # ifdef GTSAM_USE_TBB @@ -85,4 +84,4 @@ namespace gtsam }; } -} \ No newline at end of file +} diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 4b5d1caf1..380836d1d 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -74,7 +74,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 0a76c08b0..65d532191 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -19,9 +19,9 @@ #pragma once #include -#include #include #include +#include namespace gtsam { @@ -70,7 +70,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 4ef963f5d..73df17b0d 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -111,7 +112,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index be3eaa981..d154ea52a 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -95,7 +95,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index dd0811d8b..e83a64ba9 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -183,7 +183,7 @@ public: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("GenericValue", boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("value", value_); diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index aec4b5f1c..39541416e 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -13,16 +13,20 @@ * @file Group.h * * @brief Concept check class for variable types with Group properties - * @date Nov 5, 2011 + * @date November, 2011 * @author Alex Cunningham + * @author Frank Dellaert */ #pragma once +#include + #include #include #include #include +#include namespace gtsam { @@ -83,21 +87,119 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) { && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol); } -/// Macro to add group traits, additive flavor -#define GTSAM_ADDITIVE_GROUP(GROUP) \ - typedef additive_group_tag group_flavor; \ - static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \ - static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \ - static GROUP Inverse(const GROUP &g) { return -g;} +namespace internal { -/// Macro to add group traits, multiplicative flavor -#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \ - typedef additive_group_tag group_flavor; \ - static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \ - static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \ - static GROUP Inverse(const GROUP &g) { return g.inverse();} +/// A helper class that implements the traits interface for multiplicative groups. +/// Assumes existence of identity, operator* and inverse method +template +struct MultiplicativeGroupTraits { + typedef group_tag structure_category; + typedef multiplicative_group_tag group_flavor; + static Class Identity() { return Class::identity(); } + static Class Compose(const Class &g, const Class & h) { return g * h;} + static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} + static Class Inverse(const Class &g) { return g.inverse();} +}; -} // \ namespace gtsam +/// Both multiplicative group traits and Testable +template +struct MultiplicativeGroup : MultiplicativeGroupTraits, Testable {}; + +/// A helper class that implements the traits interface for additive groups. +/// Assumes existence of identity and three additive operators +template +struct AdditiveGroupTraits { + typedef group_tag structure_category; + typedef additive_group_tag group_flavor; + static Class Identity() { return Class::identity(); } + static Class Compose(const Class &g, const Class & h) { return g + h;} + static Class Between(const Class &g, const Class & h) { return h - g;} + static Class Inverse(const Class &g) { return -g;} +}; + +/// Both additive group traits and Testable +template +struct AdditiveGroup : AdditiveGroupTraits, Testable {}; + +} // namespace internal + +/// compose multiple times +template +BOOST_CONCEPT_REQUIRES(((IsGroup)),(G)) // +compose_pow(const G& g, size_t n) { + if (n == 0) return traits::Identity(); + else if (n == 1) return g; + else return traits::Compose(compose_pow(g, n - 1), g); +} + +/// Template to construct the direct product of two arbitrary groups +/// Assumes nothing except group structure and Testable from G and H +template +class DirectProduct: public std::pair { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); + +public: + /// Default constructor yields identity + DirectProduct():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + DirectProduct(const G& g, const H& h):std::pair(g,h) {} + + // identity + static DirectProduct identity() { return DirectProduct(); } + + DirectProduct operator*(const DirectProduct& other) const { + return DirectProduct(traits::Compose(this->first, other.first), + traits::Compose(this->second, other.second)); + } + DirectProduct inverse() const { + return DirectProduct(this->first.inverse(), this->second.inverse()); + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : + internal::MultiplicativeGroupTraits > {}; + +/// Template to construct the direct sum of two additive groups +/// Assumes existence of three additive operators for both groups +template +class DirectSum: public std::pair { + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + + const G& g() const { return this->first; } + const H& h() const { return this->second;} + +public: + /// Default constructor yields identity + DirectSum():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + DirectSum(const G& g, const H& h):std::pair(g,h) {} + + // identity + static DirectSum identity() { return DirectSum(); } + + DirectSum operator+(const DirectSum& other) const { + return DirectSum(g()+other.g(), h()+other.h()); + } + DirectSum operator-(const DirectSum& other) const { + return DirectSum(g()-other.g(), h()-other.h()); + } + DirectSum operator-() const { + return DirectSum(- g(), - h()); + } +}; + +// Define direct sums to be a model of the Additive Group concept +template +struct traits > : + internal::AdditiveGroupTraits > {}; + +} // namespace gtsam /** * Macros for using the IsGroup diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index bf2056cc8..05c7bc20f 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -136,8 +136,10 @@ namespace internal { /// A helper class that implements the traits interface for GTSAM lie groups. /// To use this for your gtsam type, define: /// template<> struct traits : public internal::LieGroupTraits {}; +/// Assumes existence of: identity, dimension, localCoordinates, retract, +/// and additionally Logmap, Expmap, compose, between, and inverse template -struct LieGroupTraits : Testable { +struct LieGroupTraits { typedef lie_group_tag structure_category; /// @name Group @@ -167,12 +169,10 @@ struct LieGroupTraits : Testable { ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { return origin.retract(v, Horigin, Hv); } - /// @} /// @name Lie Group /// @{ - static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { return Class::Logmap(m, Hm); } @@ -195,11 +195,12 @@ struct LieGroupTraits : Testable { ChartJacobian H = boost::none) { return m.inverse(H); } - /// @} - }; +/// Both LieGroupTraits and Testable +template struct LieGroup: LieGroupTraits, Testable {}; + } // \ namepsace internal /** @@ -248,7 +249,7 @@ public: // log and exp map without Jacobians g = traits::Expmap(v); v = traits::Logmap(g); - // log and expnential map with Jacobians + // log and exponential map with Jacobians g = traits::Expmap(v, Hg); v = traits::Logmap(g, Hg); } diff --git a/gtsam/base/LieMatrix_Deprecated.h b/gtsam/base/LieMatrix_Deprecated.h index 5dbe9935f..15b4401f2 100644 --- a/gtsam/base/LieMatrix_Deprecated.h +++ b/gtsam/base/LieMatrix_Deprecated.h @@ -124,7 +124,7 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("Matrix", boost::serialization::base_object(*this)); diff --git a/gtsam/base/LieVector_Deprecated.h b/gtsam/base/LieVector_Deprecated.h index 3cb73319d..67c42c748 100644 --- a/gtsam/base/LieVector_Deprecated.h +++ b/gtsam/base/LieVector_Deprecated.h @@ -103,7 +103,7 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("Vector", boost::serialization::base_object(*this)); } diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 6998b3b18..d7ea9ea4c 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -46,7 +46,7 @@ struct manifold_tag {}; * There may be multiple possible retractions for a given manifold, which can be chosen * between depending on the computational complexity. The important criteria for * the creation for the retract and localCoordinates functions is that they be - * inverse operations. The new notion of a Chart guarantees that. + * inverse operations. * */ @@ -90,9 +90,9 @@ struct ManifoldImpl { /// A helper that implements the traits interface for GTSAM manifolds. /// To use this for your class type, define: -/// template<> struct traits : public Manifold { }; +/// template<> struct traits : public internal::ManifoldTraits { }; template -struct Manifold: Testable, ManifoldImpl { +struct ManifoldTraits: ManifoldImpl { // Check that Class has the necessary machinery BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); @@ -116,6 +116,9 @@ struct Manifold: Testable, ManifoldImpl { } }; +/// Both ManifoldTraits and Testable +template struct Manifold: ManifoldTraits, Testable {}; + } // \ namespace internal /// Check invariants for Manifold type @@ -135,7 +138,7 @@ class IsManifold { public: typedef typename traits::structure_category structure_category_tag; - static const size_t dim = traits::dimension; + static const int dim = traits::dimension; typedef typename traits::ManifoldType ManifoldType; typedef typename traits::TangentVector TangentVector; @@ -165,6 +168,53 @@ struct FixedDimension { "FixedDimension instantiated for dymanically-sized type."); }; +/// Helper class to construct the product manifold of two other manifolds, M1 and M2 +/// Assumes nothing except manifold structure from M1 and M2 +template +class ProductManifold: public std::pair { + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + +protected: + enum { dimension1 = traits::dimension }; + enum { dimension2 = traits::dimension }; + +public: + enum { dimension = dimension1 + dimension2 }; + inline static size_t Dim() { return dimension;} + inline size_t dim() const { return dimension;} + + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + /// Default constructor yields identity + ProductManifold():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two original manifold values + ProductManifold(const M1& m1, const M2& m2):std::pair(m1,m2) {} + + /// Retract delta to manifold + ProductManifold retract(const TangentVector& xi) const { + M1 m1 = traits::Retract(this->first, xi.template head()); + M2 m2 = traits::Retract(this->second, xi.template tail()); + return ProductManifold(m1,m2); + } + + /// Compute the coordinates in the tangent space + TangentVector localCoordinates(const ProductManifold& other) const { + typename traits::TangentVector v1 = traits::Local(this->first, other.first); + typename traits::TangentVector v2 = traits::Local(this->second, other.second); + TangentVector v; + v << v1, v2; + return v; + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::Manifold > { +}; + } // \ namespace gtsam ///** diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 7fa0992ca..9e56dfb6c 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -20,8 +20,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -706,6 +706,7 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, return ss.str(); } +/* ************************************************************************* */ void inplace_QR(Matrix& A){ size_t rows = A.rows(); size_t cols = A.cols(); @@ -716,7 +717,13 @@ void inplace_QR(Matrix& A){ HCoeffsType hCoeffs(size); RowVectorType temp(cols); +#ifdef GTSAM_USE_SYSTEM_EIGEN + // System-Eigen is used, and MKL is off + Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data()); +#else + // Patched Eigen is used, and MKL is either on or off Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data()); +#endif zeroBelowDiagonal(A); } diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 27b7ec8f7..caee2071c 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -23,9 +23,9 @@ #pragma once #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -535,7 +535,7 @@ namespace boost { // split version - sends sizes ahead template - void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) { + void save(Archive & ar, const gtsam::Matrix & m, unsigned int /*version*/) { const size_t rows = m.rows(), cols = m.cols(); ar << BOOST_SERIALIZATION_NVP(rows); ar << BOOST_SERIALIZATION_NVP(cols); @@ -543,7 +543,7 @@ namespace boost { } template - void load(Archive & ar, gtsam::Matrix & m, unsigned int version) { + void load(Archive & ar, gtsam::Matrix & m, unsigned int /*version*/) { size_t rows, cols; ar >> BOOST_SERIALIZATION_NVP(rows); ar >> BOOST_SERIALIZATION_NVP(cols); diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 2ea9d672e..7055a287a 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #ifndef OPTIONALJACOBIAN_NOBOOST #include @@ -83,7 +83,7 @@ public: #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t none) : + OptionalJacobian(boost::none_t /*none*/) : map_(NULL) { } @@ -142,7 +142,7 @@ public: #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t none) : + OptionalJacobian(boost::none_t /*none*/) : pointer_(NULL) { } @@ -168,5 +168,20 @@ public: Jacobian* operator->(){ return pointer_; } }; +// forward declare +template struct traits; + +/** + * @brief: meta-function to generate JacobianTA optional reference + * Used mainly by Expressions + * @param T return type + * @param A argument type + */ +template +struct MakeOptionalJacobian { + typedef OptionalJacobian::dimension, + traits::dimension> type; +}; + } // namespace gtsam diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h new file mode 100644 index 000000000..90aeb54d1 --- /dev/null +++ b/gtsam/base/ProductLieGroup.h @@ -0,0 +1,197 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file ProductLieGroup.h + * @date May, 2015 + * @author Frank Dellaert + * @brief Group product of two Lie Groups + */ + +#pragma once + +#include +#include // pair + +namespace gtsam { + +/// Template to construct the product Lie group of two other Lie groups +/// Assumes Lie group structure for G and H +template +class ProductLieGroup: public std::pair { + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + typedef std::pair Base; + +protected: + enum {dimension1 = traits::dimension}; + enum {dimension2 = traits::dimension}; + +public: + /// Default constructor yields identity + ProductLieGroup():Base(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductLieGroup(const G& g, const H& h):Base(g,h) {} + + // Construct from base + ProductLieGroup(const Base& base):Base(base) {} + + /// @name Group + /// @{ + typedef multiplicative_group_tag group_flavor; + static ProductLieGroup identity() {return ProductLieGroup();} + + ProductLieGroup operator*(const ProductLieGroup& other) const { + return ProductLieGroup(traits::Compose(this->first,other.first), + traits::Compose(this->second,other.second)); + } + ProductLieGroup inverse() const { + return ProductLieGroup(this->first.inverse(), this->second.inverse()); + } + ProductLieGroup compose(const ProductLieGroup& g) const { + return (*this) * g; + } + ProductLieGroup between(const ProductLieGroup& g) const { + return this->inverse() * g; + } + /// @} + + /// @name Manifold + /// @{ + enum {dimension = dimension1 + dimension2}; + inline static size_t Dim() {return dimension;} + inline size_t dim() const {return dimension;} + + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + static ProductLieGroup Retract(const TangentVector& v) { + return ProductLieGroup::ChartAtOrigin::Retract(v); + } + static TangentVector LocalCoordinates(const ProductLieGroup& g) { + return ProductLieGroup::ChartAtOrigin::Local(g); + } + ProductLieGroup retract(const TangentVector& v) const { + return compose(ProductLieGroup::ChartAtOrigin::Retract(v)); + } + TangentVector localCoordinates(const ProductLieGroup& g) const { + return ProductLieGroup::ChartAtOrigin::Local(between(g)); + } + /// @} + + /// @name Lie Group + /// @{ +protected: + typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix Jacobian1; + typedef Eigen::Matrix Jacobian2; + +public: + ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup inverse(ChartJacobian D) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Inverse(this->first, D ? &D_g_first : 0); + H h = traits::Inverse(this->second, D ? &D_h_second : 0); + if (D) { + D->setZero(); + D->template topLeftCorner() = D_g_first; + D->template bottomRightCorner() = D_h_second; + } + return ProductLieGroup(g,h); + } + static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet"); + G g = traits::Expmap(v.template head()); + H h = traits::Expmap(v.template tail()); + return ProductLieGroup(g,h); + } + static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet"); + typename traits::TangentVector v1 = traits::Logmap(p.first); + typename traits::TangentVector v2 = traits::Logmap(p.second); + TangentVector v; + v << v1, v2; + return v; + } + struct ChartAtOrigin { + static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) { + return Logmap(m, Hm); + } + static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) { + return Expmap(v, Hv); + } + }; + ProductLieGroup expmap(const TangentVector& v) const { + return compose(ProductLieGroup::Expmap(v)); + } + TangentVector logmap(const ProductLieGroup& g) const { + return ProductLieGroup::Logmap(between(g)); + } + static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) { + return ProductLieGroup::ChartAtOrigin::Retract(v,H1); + } + static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) { + return ProductLieGroup::ChartAtOrigin::Local(g,H1); + } + ProductLieGroup retract(const TangentVector& v, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Jacobian D_g_v; + ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); + ProductLieGroup h = compose(g,H1,H2); + if (H2) *H2 = (*H2) * D_g_v; + return h; + } + TangentVector localCoordinates(const ProductLieGroup& g, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ProductLieGroup h = between(g,H1,H2); + Jacobian D_v_h; + TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); + if (H1) *H1 = D_v_h * (*H1); + if (H2) *H2 = D_v_h * (*H2); + return v; + } + /// @} + +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::LieGroupTraits< + ProductLieGroup > { +}; +} // namespace gtsam + diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 546b6a7f1..0fbdfeefc 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -61,13 +61,13 @@ VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( // Split conditional - // Create one big conditionals with many frontal variables. - gttic(Construct_conditional); - const size_t varDim = offset(nFrontals); - VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); - Ab.full() = matrix_.topRows(varDim); - Ab.full().triangularView().setZero(); - gttoc(Construct_conditional); + // Create one big conditionals with many frontal variables. + gttic(Construct_conditional); + const size_t varDim = offset(nFrontals); + VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); + Ab.full() = matrix_.topRows(varDim); + Ab.full().triangularView().setZero(); + gttoc(Construct_conditional); gttic(Remaining_factor); // Take lower-right block of Ab_ to get the remaining factor diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 5dcc79a68..41584c7f9 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -199,6 +199,7 @@ namespace gtsam { void checkBlock(DenseIndex block) const { + static_cast(block); //Disable unused varibale warnings. assert(matrix_.rows() == matrix_.cols()); assert(matrix_.cols() == variableColOffsets_.back()); assert(block >= 0); @@ -235,7 +236,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // Fill in the lower triangle part of the matrix, so boost::serialization won't // complain about uninitialized data with an input_stream_error exception // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error diff --git a/gtsam/base/SymmetricBlockMatrixBlockExpr.h b/gtsam/base/SymmetricBlockMatrixBlockExpr.h index 6a65b2748..dd999ae6c 100644 --- a/gtsam/base/SymmetricBlockMatrixBlockExpr.h +++ b/gtsam/base/SymmetricBlockMatrixBlockExpr.h @@ -93,7 +93,7 @@ namespace gtsam /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a /// SymmetricBlockMatrix. - SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char dummy) : + SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char /*dummy*/) : xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) { initIndices(firstBlock, firstBlock, blocks, blocks); diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 70677cad1..9537baa31 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -121,7 +121,7 @@ namespace gtsam { virtual Vector localCoordinates_(const Value& value) const = 0; /** Assignment operator */ - virtual Value& operator=(const Value& rhs) { + virtual Value& operator=(const Value& /*rhs*/) { //needs a empty definition so recursion in implicit derived assignment operators work return *this; } @@ -166,7 +166,7 @@ namespace gtsam { * */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) { } }; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 0a708427a..ed6373f5b 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -222,11 +222,7 @@ double norm_2(const Vector& v) { /* ************************************************************************* */ Vector reciprocal(const Vector &a) { - size_t n = a.size(); - Vector b(n); - for( size_t i = 0; i < n; i++ ) - b(i) = 1.0/a(i); - return b; + return a.array().inverse(); } /* ************************************************************************* */ diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index fc543acc3..20477a205 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -26,7 +26,7 @@ #endif #include -#include +#include #include #include @@ -355,14 +355,14 @@ namespace boost { // split version - copies into an STL vector for serialization template - void save(Archive & ar, const gtsam::Vector & v, unsigned int version) { + void save(Archive & ar, const gtsam::Vector & v, unsigned int /*version*/) { const size_t size = v.size(); ar << BOOST_SERIALIZATION_NVP(size); ar << make_nvp("data", make_array(v.data(), v.size())); } template - void load(Archive & ar, gtsam::Vector & v, unsigned int version) { + void load(Archive & ar, gtsam::Vector & v, unsigned int /*version*/) { size_t size; ar >> BOOST_SERIALIZATION_NVP(size); v.resize(size); @@ -371,12 +371,12 @@ namespace boost { // split version - copies into an STL vector for serialization template - void save(Archive & ar, const Eigen::Matrix & v, unsigned int version) { + void save(Archive & ar, const Eigen::Matrix & v, unsigned int /*version*/) { ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); } template - void load(Archive & ar, Eigen::Matrix & v, unsigned int version) { + void load(Archive & ar, Eigen::Matrix & v, unsigned int /*version*/) { ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); } diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 094e6f162..c356dcc07 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -20,17 +20,10 @@ template struct traits; namespace internal { -/// VectorSpace Implementation for Fixed sizes +/// VectorSpaceTraits Implementation for Fixed sizes template struct VectorSpaceImpl { - /// @name Group - /// @{ - static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} - static Class Between(const Class& v1, const Class& v2) { return v2-v1;} - static Class Inverse(const Class& m) { return -m;} - /// @} - /// @name Manifold /// @{ typedef Eigen::Matrix TangentVector; @@ -68,21 +61,21 @@ struct VectorSpaceImpl { return Class(v); } - static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v1 + v2; } - static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v2 - v1; } - static Class Inverse(const Class& v, ChartJacobian H) { + static Class Inverse(const Class& v, ChartJacobian H = boost::none) { if (H) *H = - Jacobian::Identity(); return -v; } @@ -90,7 +83,7 @@ struct VectorSpaceImpl { /// @} }; -/// VectorSpace implementation for dynamic types. +/// VectorSpaceTraits implementation for dynamic types. template struct VectorSpaceImpl { @@ -166,11 +159,11 @@ struct VectorSpaceImpl { /// @} }; -/// A helper that implements the traits interface for GTSAM lie groups. +/// A helper that implements the traits interface for GTSAM vector space types. /// To use this for your gtsam type, define: -/// template<> struct traits : public VectorSpace { }; +/// template<> struct traits : public VectorSpaceTraits { }; template -struct VectorSpace: Testable, VectorSpaceImpl { +struct VectorSpaceTraits: VectorSpaceImpl { typedef vector_space_tag structure_category; @@ -185,9 +178,12 @@ struct VectorSpace: Testable, VectorSpaceImpl { enum { dimension = Class::dimension}; typedef Class ManifoldType; /// @} - }; +/// Both VectorSpaceTRaits and Testable +template +struct VectorSpace: Testable, VectorSpaceTraits {}; + /// A helper that implements the traits interface for GTSAM lie groups. /// To use this for your gtsam type, define: /// template<> struct traits : public ScalarTraits { }; @@ -401,7 +397,8 @@ struct DynamicTraits { return result; } - static Dynamic Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) { + static_cast(H); throw std::runtime_error("Expmap not defined for dynamic types"); } diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index b075d73b3..c6a3eb034 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -199,6 +199,7 @@ namespace gtsam { } void checkBlock(DenseIndex block) const { + static_cast(block); //Disable unused varibale warnings. assert(matrix_.cols() == variableColOffsets_.back()); assert(block < (DenseIndex)variableColOffsets_.size() - 1); assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); @@ -220,7 +221,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); ar & BOOST_SERIALIZATION_NVP(rowStart_); diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp new file mode 100644 index 000000000..bce9a6036 --- /dev/null +++ b/gtsam/base/tests/testGroup.cpp @@ -0,0 +1,144 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGroup.cpp + * @brief Unit tests for groups + * @author Frank Dellaert + **/ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Symmetric group +template +class Symmetric: private Eigen::PermutationMatrix { + Symmetric(const Eigen::PermutationMatrix& P) : + Eigen::PermutationMatrix(P) { + } +public: + static Symmetric identity() { return Symmetric(); } + Symmetric() { + Eigen::PermutationMatrix::setIdentity(); + } + static Symmetric Transposition(int i, int j) { + Symmetric g; + return g.applyTranspositionOnTheRight(i, j); + } + Symmetric operator*(const Symmetric& other) const { + return Eigen::PermutationMatrix::operator*(other); + } + bool operator==(const Symmetric& other) const { + for (size_t i = 0; i < N; i++) + if (this->indices()[i] != other.indices()[i]) + return false; + return true; + } + Symmetric inverse() const { + return Eigen::PermutationMatrix(Eigen::PermutationMatrix::inverse()); + } + friend std::ostream &operator<<(std::ostream &os, const Symmetric& m) { + for (size_t i = 0; i < N; i++) + os << m.indices()[i] << " "; + return os; + } + void print(const std::string& s = "") const { + std::cout << s << *this << std::endl; + } + bool equals(const Symmetric& other, double tol = 0) const { + return this->indices() == other.indices(); + } +}; + +/// Define permutation group traits to be a model of the Multiplicative Group concept +template +struct traits > : internal::MultiplicativeGroupTraits >, + Testable > { +}; + +} // namespace gtsam + +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +typedef Symmetric<2> S2; +TEST(Group, S2) { + S2 e, s1 = S2::Transposition(0, 1); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, s1)); +} + +//****************************************************************************** +typedef Symmetric<3> S3; +TEST(Group, S3) { + S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, s1)); + EXPECT(assert_equal(s1, s1 * e)); + EXPECT(assert_equal(s1, e * s1)); + EXPECT(assert_equal(e, s1 * s1)); + S3 g = s1 * s2; // 1 2 0 + EXPECT(assert_equal(s1, g * s2)); + EXPECT(assert_equal(e, compose_pow(g, 0))); + EXPECT(assert_equal(g, compose_pow(g, 1))); + EXPECT(assert_equal(e, compose_pow(g, 3))); // g is generator of Z3 subgroup +} + +//****************************************************************************** +// The direct product of S2=Z2 and S3 is the symmetry group of a hexagon, +// i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon) +namespace gtsam { +typedef DirectProduct Dih6; + +std::ostream &operator<<(std::ostream &os, const Dih6& m) { + os << "( " << m.first << ", " << m.second << ")"; + return os; +} + +// Provide traits with Testable + +template<> +struct traits : internal::MultiplicativeGroupTraits { + static void Print(const Dih6& m, const string& s = "") { + cout << s << m << endl; + } + static bool Equals(const Dih6& m1, const Dih6& m2, double tol = 1e-8) { + return m1 == m2; + } +}; +} // namespace gtsam + +TEST(Group, Dih6) { + Dih6 e, g(S2::Transposition(0, 1), + S3::Transposition(0, 1) * S3::Transposition(1, 2)); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, g)); + EXPECT(assert_equal(e, compose_pow(g, 0))); + EXPECT(assert_equal(g, compose_pow(g, 1))); + EXPECT(assert_equal(e, compose_pow(g, 6))); // g is generator of Z6 subgroup +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 141b55761..07e666fbe 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -37,8 +37,8 @@ TEST(LieScalar , Concept) { //****************************************************************************** TEST(LieScalar , Invariants) { LieScalar lie1(2), lie2(3); - check_group_invariants(lie1, lie2); - check_manifold_invariants(lie1, lie2); + CHECK(check_group_invariants(lie1, lie2)); + CHECK(check_manifold_invariants(lie1, lie2)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp index c9083f781..8fe5e53ef 100644 --- a/gtsam/base/tests/testTreeTraversal.cpp +++ b/gtsam/base/tests/testTreeTraversal.cpp @@ -45,11 +45,11 @@ struct TestForest { }; TestForest makeTestForest() { - // 0 1 - // / \ - // 2 3 - // | - // 4 + // 0 1 + // / | + // 2 3 + // | + // 4 TestForest forest; forest.roots_.push_back(boost::make_shared(0)); forest.roots_.push_back(boost::make_shared(1)); diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 36f1c2f5f..b76746ba0 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -16,24 +16,28 @@ * @date Oct 5, 2010 */ -#include -#include -#include -#include -#include -#include -#include -#include - #include #include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + namespace gtsam { namespace internal { -GTSAM_EXPORT boost::shared_ptr timingRoot( +GTSAM_EXPORT boost::shared_ptr gTimingRoot( new TimingOutline("Total", getTicTocID("Total"))); -GTSAM_EXPORT boost::weak_ptr timingCurrent(timingRoot); +GTSAM_EXPORT boost::weak_ptr gCurrentTimer(gTimingRoot); /* ************************************************************************* */ // Implementation of TimingOutline @@ -50,8 +54,8 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) { } /* ************************************************************************* */ -TimingOutline::TimingOutline(const std::string& label, size_t myId) : - myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( +TimingOutline::TimingOutline(const std::string& label, size_t id) : + id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( 0), lastChildOrder_(0), label_(label) { #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); @@ -153,7 +157,7 @@ const boost::shared_ptr& TimingOutline::child(size_t child, } /* ************************************************************************* */ -void TimingOutline::ticInternal() { +void TimingOutline::tic() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(timer_.is_stopped()); timer_.start(); @@ -169,7 +173,7 @@ void TimingOutline::ticInternal() { } /* ************************************************************************* */ -void TimingOutline::tocInternal() { +void TimingOutline::toc() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(!timer_.is_stopped()); @@ -212,7 +216,6 @@ void TimingOutline::finishedIteration() { } /* ************************************************************************* */ -// Generate or retrieve a unique global ID number that will be used to look up tic_/toc statements size_t getTicTocID(const char *descriptionC) { const std::string description(descriptionC); // Global (static) map from strings to ID numbers and current next ID number @@ -232,37 +235,33 @@ size_t getTicTocID(const char *descriptionC) { } /* ************************************************************************* */ -void ticInternal(size_t id, const char *labelC) { +void tic(size_t id, const char *labelC) { const std::string label(labelC); - if (ISDEBUG("timing-verbose")) - std::cout << "gttic_(" << id << ", " << label << ")" << std::endl; boost::shared_ptr node = // - timingCurrent.lock()->child(id, label, timingCurrent); - timingCurrent = node; - node->ticInternal(); + gCurrentTimer.lock()->child(id, label, gCurrentTimer); + gCurrentTimer = node; + node->tic(); } /* ************************************************************************* */ -void tocInternal(size_t id, const char *label) { - if (ISDEBUG("timing-verbose")) - std::cout << "gttoc(" << id << ", " << label << ")" << std::endl; - boost::shared_ptr current(timingCurrent.lock()); - if (id != current->myId_) { - timingRoot->print(); +void toc(size_t id, const char *label) { + boost::shared_ptr current(gCurrentTimer.lock()); + if (id != current->id_) { + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".") % label % current->label_).str()); } if (!current->parent_.lock()) { - timingRoot->print(); + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root") % label).str()); } - current->tocInternal(); - timingCurrent = current->parent_; + current->toc(); + gCurrentTimer = current->parent_; } } // namespace internal diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 7a43ef884..a904c5f08 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -17,12 +17,15 @@ */ #pragma once -#include -#include -#include -#include -#include #include +#include + +#include +#include +#include + +#include +#include // This file contains the GTSAM timing instrumentation library, a low-overhead method for // learning at a medium-fine level how much time various components of an algorithm take @@ -125,16 +128,21 @@ namespace gtsam { namespace internal { + // Generate/retrieve a unique global ID number that will be used to look up tic/toc statements GTSAM_EXPORT size_t getTicTocID(const char *description); - GTSAM_EXPORT void ticInternal(size_t id, const char *label); - GTSAM_EXPORT void tocInternal(size_t id, const char *label); + + // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, and call tic method + GTSAM_EXPORT void tic(size_t id, const char *label); + + // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of gCurrentTimer + GTSAM_EXPORT void toc(size_t id, const char *label); /** * Timing Entry, arranged in a tree */ class GTSAM_EXPORT TimingOutline { protected: - size_t myId_; + size_t id_; size_t t_; size_t tWall_; double t2_ ; ///< cache the \sum t_i^2 @@ -176,29 +184,38 @@ namespace gtsam { void print2(const std::string& outline = "", const double parentTotal = -1.0) const; const boost::shared_ptr& child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); - void ticInternal(); - void tocInternal(); + void tic(); + void toc(); void finishedIteration(); - GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); + GTSAM_EXPORT friend void toc(size_t id, const char *label); }; // \TimingOutline /** - * No documentation + * Small class that calls internal::tic at construction, and internol::toc when destroyed */ class AutoTicToc { - private: + private: size_t id_; - const char *label_; + const char* label_; bool isSet_; - public: - AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } - void stop() { tocInternal(id_, label_); isSet_ = false; } - ~AutoTicToc() { if(isSet_) stop(); } + + public: + AutoTicToc(size_t id, const char* label) + : id_(id), label_(label), isSet_(true) { + tic(id_, label_); + } + void stop() { + toc(id_, label_); + isSet_ = false; + } + ~AutoTicToc() { + if (isSet_) stop(); + } }; - GTSAM_EXTERN_EXPORT boost::shared_ptr timingRoot; - GTSAM_EXTERN_EXPORT boost::weak_ptr timingCurrent; + GTSAM_EXTERN_EXPORT boost::shared_ptr gTimingRoot; + GTSAM_EXTERN_EXPORT boost::weak_ptr gCurrentTimer; } // Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) @@ -210,7 +227,7 @@ namespace gtsam { // tic #define gttic_(label) \ static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) + ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label) // toc #define gttoc_(label) \ @@ -228,26 +245,26 @@ namespace gtsam { // indicate iteration is finished inline void tictoc_finishedIteration_() { - ::gtsam::internal::timingRoot->finishedIteration(); } + ::gtsam::internal::gTimingRoot->finishedIteration(); } // print inline void tictoc_print_() { - ::gtsam::internal::timingRoot->print(); } + ::gtsam::internal::gTimingRoot->print(); } // print mean and standard deviation inline void tictoc_print2_() { - ::gtsam::internal::timingRoot->print2(); } + ::gtsam::internal::gTimingRoot->print2(); } // get a node by label and assign it to variable #define tictoc_getNode(variable, label) \ static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ const boost::shared_ptr variable = \ - ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent); + ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer); // reset inline void tictoc_reset_() { - ::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); - ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; } + ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); + ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; } #ifdef ENABLE_TIMING #define gttic(label) gttic_(label) diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 64136511d..20073152e 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -25,7 +25,7 @@ #define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@" // Paths to example datasets distributed with GTSAM -#define GTSAM_SOURCE_TREE_DATASET_DIR "@CMAKE_SOURCE_DIR@/examples/Data" +#define GTSAM_SOURCE_TREE_DATASET_DIR "@PROJECT_SOURCE_DIR@/examples/Data" #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data" // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices) @@ -42,6 +42,9 @@ // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake) #cmakedefine GTSAM_USE_TBB +// Whether we are using system-Eigen or our own patched version +#cmakedefine GTSAM_USE_SYSTEM_EIGEN + // Whether Eigen will use MKL (if MKL was found and GTSAM_WITH_EIGEN_MKL is enabled in CMake) #cmakedefine GTSAM_USE_EIGEN_MKL #cmakedefine EIGEN_USE_MKL_ALL // This is also defined in gtsam_eigen_includes.h diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index cd56780e5..c1648888e 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -467,7 +467,7 @@ namespace gtsam { // find highest label among branches boost::optional highestLabel; - boost::optional nrChoices; + size_t nrChoices = 0; for (Iterator it = begin; it != end; it++) { if (it->root_->isLeaf()) continue; @@ -475,22 +475,21 @@ namespace gtsam { boost::dynamic_pointer_cast(it->root_); if (!highestLabel || c->label() > *highestLabel) { highestLabel.reset(c->label()); - nrChoices.reset(c->nrChoices()); + nrChoices = c->nrChoices(); } } // if label is already in correct order, just put together a choice on label - if (!highestLabel || !nrChoices || label > *highestLabel) { + if (!nrChoices || !highestLabel || label > *highestLabel) { boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); for (Iterator it = begin; it != end; it++) choiceOnLabel->push_back(it->root_); return Choice::Unique(choiceOnLabel); } else { // Set up a new choice on the highest label - boost::shared_ptr choiceOnHighestLabel( - new Choice(*highestLabel, *nrChoices)); + boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, nrChoices)); // now, for all possible values of highestLabel - for (size_t index = 0; index < *nrChoices; index++) { + for (size_t index = 0; index < nrChoices; index++) { // make a new set of functions for composing by iterating over the given // functions, and selecting the appropriate branch. std::vector functions; diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index de5ec8042..dcc336f89 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -90,7 +90,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index e90ae32a4..234ee261a 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -159,7 +159,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 6ae8ec14b..9f4641f71 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -109,7 +109,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("Cal3DS2", boost::serialization::base_object(*this)); diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index d4f4cabe5..cfbdde07c 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -153,7 +153,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 2127fb200..99bd04fb1 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -134,7 +134,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("Cal3Unified", boost::serialization::base_object(*this)); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index b9e2ef581..3d5342c92 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -21,7 +21,6 @@ #pragma once -#include #include namespace gtsam { @@ -211,7 +210,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 651a7fabb..365a6c40e 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -144,7 +144,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(b_); diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 6fbef721c..2d27b4dc7 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -93,12 +93,6 @@ Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { return Point2(u, v); } -/* ************************************************************************* */ -Point2 PinholeBase::project_to_camera_old(const Point3& pc, - OptionalJacobian<2, 3> Dpoint) { - return Project(pc); -} - /* ************************************************************************* */ Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { if (Dpoint) { diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 6e2f153c8..f31bdaec2 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -167,10 +167,6 @@ public: static Point2 Project(const Point3& pc, // OptionalJacobian<2, 3> Dpoint = boost::none); - /// @deprecated not correct naming for static function, use Project above - static Point2 project_to_camera_old(const Point3& pc, // - OptionalJacobian<2, 3> Dpoint = boost::none); - /** * Project from 3D point at infinity in camera coordinates into image * Does *not* throw a CheiralityException, even if pc behind image plane @@ -222,7 +218,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(pose_); } @@ -374,7 +370,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("PinholeBase", boost::serialization::base_object(*this)); diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 1ee53d2c8..a60528909 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -319,7 +319,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & (*this); } }; diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 2c883707f..15d8154b8 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -16,10 +16,8 @@ **/ #include -#include -#include -#include -#include +#include +#include // for cout :-( namespace gtsam { @@ -33,10 +31,11 @@ public: i_(i) { assert(i < N); } - /// Idenity element - static Cyclic Identity() { - return Cyclic(0); + /// Default constructor yields identity + Cyclic():i_(0) { } + static Cyclic identity() { return Cyclic();} + /// Cast to size_t operator size_t() const { return i_; @@ -63,20 +62,10 @@ public: } }; -/// Define cyclic group traits to be a model of the Group concept +/// Define cyclic group to be a model of the Additive Group concept template -struct traits > { - typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic) - static Cyclic Identity() { - return Cyclic::Identity(); - } - static bool Equals(const Cyclic& a, const Cyclic& b, - double tol = 1e-9) { - return a.equals(b, tol); - } - static void Print(const Cyclic& c, const std::string &s = "") { - c.print(s); - } +struct traits > : internal::AdditiveGroupTraits >, // + Testable > { }; } // \namespace gtsam diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 062178fea..7b90edc39 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -13,64 +13,46 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb, OptionalJacobian<5, 6> H) { - const Rot3& _1R2_ = _1P2_.rotation(); - const Point3& _1T2_ = _1P2_.translation(); + const Rot3& aRb = aPb.rotation(); + const Point3& aTb = aPb.translation(); if (!H) { // just make a direction out of translation and create E - Unit3 direction(_1T2_); - return EssentialMatrix(_1R2_, direction); + Unit3 direction(aTb); + return EssentialMatrix(aRb, direction); } else { // Calculate the 5*6 Jacobian H = D_E_1P2 // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation // First get 2*3 derivative from Unit3::FromPoint3 Matrix23 D_direction_1T2; - Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); + Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2); *H << I_3x3, Z_3x3, // - Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix(); - return EssentialMatrix(_1R2_, direction); + Matrix23::Zero(), D_direction_1T2 * aRb.matrix(); + return EssentialMatrix(aRb, direction); } } /* ************************************************************************* */ void EssentialMatrix::print(const string& s) const { cout << s; - aRb_.print("R:\n"); - aTb_.print("d: "); -} - -/* ************************************************************************* */ -EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { - assert(xi.size() == 5); - Vector3 omega(sub(xi, 0, 3)); - Vector2 z(sub(xi, 3, 5)); - Rot3 R = aRb_.retract(omega); - Unit3 t = aTb_.retract(z); - return EssentialMatrix(R, t); -} - -/* ************************************************************************* */ -Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - Vector5 v; - v << aRb_.localCoordinates(other.aRb_), - aTb_.localCoordinates(other.aTb_); - return v; + rotation().print("R:\n"); + direction().print("d: "); } /* ************************************************************************* */ Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, OptionalJacobian<3, 3> Dpoint) const { - Pose3 pose(aRb_, aTb_.point3()); + Pose3 pose(rotation(), direction().point3()); Matrix36 DE_; Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); if (DE) { // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 // The last 3 columns are derivative with respect to change in translation - // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() + // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis() // Duy made an educated guess that this needs to be rotated to the local frame Matrix35 H; - H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); + H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis(); *DE = H; } return q; @@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const { // The rotation must be conjugated to act in the camera frame - Rot3 c1Rc2 = aRb_.conjugate(cRb); + Rot3 c1Rc2 = rotation().conjugate(cRb); if (!HE && !HR) { // Rotate translation direction and return - Unit3 c1Tc2 = cRb * aTb_; + Unit3 c1Tc2 = cRb * direction(); return EssentialMatrix(c1Rc2, c1Tc2); } else { // Calculate derivatives Matrix23 D_c1Tc2_cRb; // 2*3 Matrix2 D_c1Tc2_aTb; // 2*2 - Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb); if (HE) *HE << cRb.matrix(), Matrix32::Zero(), // Matrix23::Zero(), D_c1Tc2_aTb; @@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // if (H) { // See math.lyx Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) - * aTb_.basis(); + Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) + * direction().basis(); *H << HR, HD; } return dot(vA, E_ * vB); diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 9ebcbcf5c..697bd462d 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,6 +10,7 @@ #include #include #include +#include #include namespace gtsam { @@ -20,17 +21,18 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix { +class GTSAM_EXPORT EssentialMatrix : private ProductManifold { private: - - Rot3 aRb_; ///< Rotation between a and b - Unit3 aTb_; ///< translation direction from a to b + typedef ProductManifold Base; Matrix3 E_; ///< Essential matrix -public: + /// Construct from Base + EssentialMatrix(const Base& base) : + Base(base), E_(direction().skew() * rotation().matrix()) { + } - enum { dimension = 5 }; +public: /// Static function to convert Point2 to homogeneous coordinates static Vector3 Homogeneous(const Point2& p) { @@ -42,12 +44,12 @@ public: /// Default constructor EssentialMatrix() : - aTb_(1, 0, 0), E_(aTb_.skew()) { + Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) { } /// Construct from rotation and translation EssentialMatrix(const Rot3& aRb, const Unit3& aTb) : - aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { + Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) { } /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) @@ -72,7 +74,8 @@ public: /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { - return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol); + return rotation().equals(other.rotation(), tol) + && direction().equals(other.direction(), tol); } /// @} @@ -80,22 +83,19 @@ public: /// @name Manifold /// @{ - /// Dimensionality of tangent space = 5 DOF - inline static size_t Dim() { - return 5; - } - - /// Return the dimensionality of the tangent space - size_t dim() const { - return 5; - } + using Base::dimension; + using Base::dim; + using Base::Dim; /// Retract delta to manifold - EssentialMatrix retract(const Vector& xi) const; + EssentialMatrix retract(const TangentVector& v) const { + return Base::retract(v); + } /// Compute the coordinates in the tangent space - Vector5 localCoordinates(const EssentialMatrix& other) const; - + TangentVector localCoordinates(const EssentialMatrix& other) const { + return Base::localCoordinates(other); + } /// @} /// @name Essential matrix methods @@ -103,12 +103,12 @@ public: /// Rotation inline const Rot3& rotation() const { - return aRb_; + return this->first; } /// Direction inline const Unit3& direction() const { - return aTb_; + return this->second; } /// Return 3*3 matrix representation @@ -118,12 +118,12 @@ public: /// Return epipole in image_a , as Unit3 to allow for infinity inline const Unit3& epipole_a() const { - return aTb_; // == direction() + return direction(); } /// Return epipole in image_b, as Unit3 to allow for infinity inline Unit3 epipole_b() const { - return aRb_.unrotate(aTb_); // == rotation.unrotate(direction()) + return rotation().unrotate(direction()); } /** @@ -179,9 +179,9 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(aRb_); - ar & BOOST_SERIALIZATION_NVP(aTb_); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(first); + ar & BOOST_SERIALIZATION_NVP(second); ar & boost::serialization::make_nvp("E11", E_(0,0)); ar & boost::serialization::make_nvp("E12", E_(0,1)); @@ -195,7 +195,6 @@ private: } /// @} - }; template<> diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 088a84243..e96708942 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -73,7 +73,7 @@ Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const { } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { +OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { // Retract the Unit3 Vector2 n_v(v(0), v(1)); Unit3 n_retracted = n_.retract(n_v); @@ -83,7 +83,7 @@ OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { /* ************************************************************************* */ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { - Vector n_local = n_.localCoordinates(y.n_); + Vector2 n_local = n_.localCoordinates(y.n_); double d_local = d_ - y.d_; Vector3 e; e << n_local(0), n_local(1), -d_local; diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 28b67cb4e..d987ad7b3 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -51,7 +51,7 @@ public: n_(s), d_(d) { } - OrientedPlane3(Vector vec) : + OrientedPlane3(const Vector4& vec) : n_(vec(0), vec(1), vec(2)), d_(vec(3)) { } @@ -62,7 +62,7 @@ public: d_ = d; } - /// The print fuction + /// The print function void print(const std::string& s = std::string()) const; /// The equals function with tolerance @@ -89,7 +89,7 @@ public: } /// The retract function - OrientedPlane3 retract(const Vector& v) const; + OrientedPlane3 retract(const Vector3& v) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 6353ddcdc..12e9f023b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -295,7 +295,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("PinholeBaseK", boost::serialization::base_object(*this)); diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 247d11823..ac453e048 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -42,6 +42,8 @@ private: public: + typedef CALIBRATION CalibrationType; + /// @name Standard Constructors /// @{ @@ -203,7 +205,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("PinholeBase", boost::serialization::base_object(*this)); @@ -313,7 +315,10 @@ public: /// print void print(const std::string& s = "PinholePose") const { Base::print(s); - K_->print(s + ".calibration"); + if (!K_) + std::cout << "s No calibration given" << std::endl; + else + K_->print(s + ".calibration"); } /// @} @@ -380,7 +385,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("PinholeBaseK", boost::serialization::base_object(*this)); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 6f4f93cf9..56809ae53 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -148,7 +148,7 @@ public: /// @{ /// equality - inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} + inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;} /// get x double x() const {return x_;} @@ -185,16 +185,18 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); } /// @} - }; +// For MATLAB wrapper +typedef std::vector Point2Vector; + /// multiply with scalar inline Point2 operator*(double s, const Point2& p) {return p*s;} diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 95f728e39..883e5fb62 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -181,7 +181,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); @@ -189,15 +189,14 @@ namespace gtsam { } /// @} - }; - /// Syntactic sugar for multiplying coordinates by a scalar s*p - inline Point3 operator*(double s, const Point3& p) { return p*s;} +/// Syntactic sugar for multiplying coordinates by a scalar s*p +inline Point3 operator*(double s, const Point3& p) { return p*s;} - template<> - struct traits : public internal::VectorSpace {}; +template<> +struct traits : public internal::VectorSpace {}; - template<> - struct traits : public internal::VectorSpace {}; +template<> +struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 0b0172857..9e87407f4 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -59,7 +59,7 @@ bool Pose2::equals(const Pose2& q, double tol) const { } /* ************************************************************************* */ -Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) { +Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) { if (H) *H = Pose2::ExpmapDerivative(xi); assert(xi.size() == 3); Point2 v(xi(0),xi(1)); @@ -130,7 +130,7 @@ Matrix3 Pose2::AdjointMap() const { } /* ************************************************************************* */ -Matrix3 Pose2::adjointMap(const Vector& v) { +Matrix3 Pose2::adjointMap(const Vector3& v) { // See Chirikjian12book2, vol.2, pg. 36 Matrix3 ad = zeros(3,3); ad(0,1) = -v[2]; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d4b647949..f3cb9e2a1 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -118,7 +118,7 @@ public: /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none); + static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); @@ -128,15 +128,14 @@ public: * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ Matrix3 AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const { - assert(xi.size() == 3); + inline Vector3 Adjoint(const Vector3& xi) const { return AdjointMap()*xi; } /** * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 */ - static Matrix3 adjointMap(const Vector& v); + static Matrix3 adjointMap(const Vector3& v); /** * wedge for SE(2): @@ -271,7 +270,7 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); } @@ -291,10 +290,10 @@ typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index be8e1bfed..32fd75eb8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -111,8 +111,10 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { - if (H) *H = ExpmapDerivative(xi); +Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { + if (H) { + *H = ExpmapDerivative(xi); + } // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -254,6 +256,14 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { return J; } +/* ************************************************************************* */ +const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { + if (H) { + *H << Z_3x3, rotation().matrix(); + } + return t_; +} + /* ************************************************************************* */ Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); @@ -280,8 +290,9 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); (*Dpose) << DR, R; } - if (Dpoint) + if (Dpoint) { *Dpoint = R_.matrix(); + } return R_ * p + t_; } @@ -299,17 +310,18 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, +wz, 0.0, -wx, 0.0,-1.0, 0.0, -wy, +wx, 0.0, 0.0, 0.0,-1.0; } - if (Dpoint) + if (Dpoint) { *Dpoint = Rt; + } return q; } /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) const { - if (!H1 && !H2) + if (!H1 && !H2) { return transform_to(point).norm(); - else { + } else { Matrix36 D1; Matrix3 D2; Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); @@ -342,25 +354,25 @@ boost::optional align(const vector& pairs) { return boost::none; // we need at least three pairs // calculate centroids - Vector cp = zero(3), cq = zero(3); - BOOST_FOREACH(const Point3Pair& pair, pairs){ - cp += pair.first.vector(); - cq += pair.second.vector(); -} + Vector3 cp = Vector3::Zero(), cq = Vector3::Zero(); + BOOST_FOREACH(const Point3Pair& pair, pairs) { + cp += pair.first.vector(); + cq += pair.second.vector(); + } double f = 1.0 / n; cp *= f; cq *= f; // Add to form H matrix - Matrix3 H = Eigen::Matrix3d::Zero(); - BOOST_FOREACH(const Point3Pair& pair, pairs){ - Vector dp = pair.first.vector() - cp; - Vector dq = pair.second.vector() - cq; - H += dp * dq.transpose(); -} + Matrix3 H = Z_3x3; + BOOST_FOREACH(const Point3Pair& pair, pairs) { + Vector3 dp = pair.first.vector() - cp; + Vector3 dq = pair.second.vector() - cq; + H += dp * dq.transpose(); + } // Compute SVD - Matrix U,V; + Matrix U, V; Vector S; svd(H, U, S, V); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4130a252e..8a0f23404 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -110,7 +110,7 @@ public: /// @{ /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ - static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none); + static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none); /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); @@ -125,7 +125,7 @@ public: * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ */ - Vector Adjoint(const Vector& xi_b) const { + Vector6 Adjoint(const Vector6& xi_b) const { return AdjointMap() * xi_b; } /// FIXME Not tested - marked as incorrect @@ -223,9 +223,7 @@ public: } /// get translation - const Point3& translation() const { - return t_; - } + const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; /// get x double x() const { @@ -294,7 +292,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(t_); } @@ -322,11 +320,14 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -template<> -struct traits : public internal::LieGroupTraits {}; +// For MATLAB wrapper +typedef std::vector Pose3Vector; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; + +template<> +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index deae1f3a0..198cd5862 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -118,12 +118,12 @@ namespace gtsam { Matrix1 AdjointMap() const { return I_1x1; } /// Left-trivialized derivative of the exponential map - static Matrix ExpmapDerivative(const Vector& v) { + static Matrix ExpmapDerivative(const Vector& /*v*/) { return ones(1); } /// Left-trivialized derivative inverse of the exponential map - static Matrix LogmapDerivative(const Vector& v) { + static Matrix LogmapDerivative(const Vector& /*v*/) { return ones(1); } @@ -200,7 +200,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(s_); } @@ -208,9 +208,9 @@ namespace gtsam { }; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; } // gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d35fa52f4..881c58579 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -428,7 +428,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { #ifndef GTSAM_USE_QUATERNIONS ar & boost::serialization::make_nvp("rot11", rot_(0,0)); @@ -463,9 +463,9 @@ namespace gtsam { GTSAM_EXPORT std::pair RQ(const Matrix3& A); template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index a07c3601d..a08168ed8 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -129,11 +129,11 @@ public: }; template<> -struct traits : public internal::LieGroupTraits { +struct traits : public internal::LieGroup { }; template<> -struct traits : public internal::LieGroupTraits { +struct traits : public internal::LieGroup { }; } // end namespace gtsam diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index c6d33bcb3..a119096d4 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -24,7 +24,114 @@ namespace gtsam { /// A simple camera class with a Cal3_S2 calibration - typedef PinholeCamera SimpleCamera; +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; + +/** + * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x + * Use PinholeCameraCal3_S2 instead + */ +class SimpleCamera : public PinholeCameraCal3_S2 { + + typedef PinholeCamera Base; + typedef boost::shared_ptr shared_ptr; + +public: + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + SimpleCamera() : + Base() { + } + + /** constructor with pose */ + explicit SimpleCamera(const Pose3& pose) : + Base(pose) { + } + + /** constructor with pose and calibration */ + SimpleCamera(const Pose3& pose, const Cal3_S2& K) : + Base(pose, K) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static SimpleCamera Level(const Cal3_S2 &K, const Pose2& pose2, + double height) { + return SimpleCamera(Base::LevelPose(pose2, height), K); + } + + /// PinholeCamera::level with default calibration + static SimpleCamera Level(const Pose2& pose2, double height) { + return SimpleCamera::Level(Cal3_S2(), pose2, height); + } + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + * @param K optional calibration parameter + */ + static SimpleCamera Lookat(const Point3& eye, const Point3& target, + const Point3& upVector, const Cal3_S2& K = Cal3_S2()) { + return SimpleCamera(Base::LookatPose(eye, target, upVector), K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + /// Init from vector, can be 6D (default calibration) or dim + explicit SimpleCamera(const Vector &v) : + Base(v) { + } + + /// Init from Vector and calibration + SimpleCamera(const Vector &v, const Vector &K) : + Base(v, K) { + } + + /// Copy this object as its actual derived type. + SimpleCamera::shared_ptr clone() const { return boost::make_shared(*this); } + + + /// @} + /// @name Manifold + /// @{ + + /// move a cameras according to d + SimpleCamera retract(const Vector& d) const { + if ((size_t) d.size() == 6) + return SimpleCamera(this->pose().retract(d), calibration()); + else + return SimpleCamera(this->pose().retract(d.head(6)), + calibration().retract(d.tail(calibration().dim()))); + } + + /// @} + +}; + +template<> +struct traits : public internal::Manifold { +}; + +template<> +struct traits : public internal::Manifold { +}; /// Recover camera from 3*4 camera matrix GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 9e5b88b31..5c6646454 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -91,8 +91,42 @@ namespace gtsam { double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); double X = Z * (measured[0] - K_->px()) / K_->fx(); double Y = Z * (measured[2] - K_->py()) / K_->fy(); - Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); - return world_point; + Point3 point = leftCamPose_.transform_from(Point3(X, Y, Z)); + return point; + } + + /* ************************************************************************* */ + Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1, + OptionalJacobian<3, 3> H2) const { + const Cal3_S2Stereo& K = *K_; + const double fx = K.fx(), fy = K.fy(), cx = K.px(), cy = K.py(), b = K.baseline(); + + double uL = z.uL(), uR = z.uR(), v = z.v(); + double disparity = uL - uR; + + double local_z = b * fx / disparity; + const Point3 local(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z); + + if(H1 || H2) { + double z_partial_uR = local_z/disparity; + double x_partial_uR = local.x()/disparity; + double y_partial_uR = local.y()/disparity; + Matrix3 D_local_z; + D_local_z << -x_partial_uR + local.z()/fx, x_partial_uR, 0, + -y_partial_uR, y_partial_uR, local.z() / fy, + -z_partial_uR, z_partial_uR, 0; + + Matrix3 D_point_local; + const Point3 point = leftCamPose_.transform_from(local, H1, D_point_local); + + if(H2) { + *H2 = D_point_local * D_local_z; + } + + return point; + } + + return leftCamPose_.transform_from(local); } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index ea28ecfc7..f09626c9d 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -137,6 +137,14 @@ public: /// back-project a measurement Point3 backproject(const StereoPoint2& z) const; + /** Back-project the 2D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + */ + Point3 backproject2(const StereoPoint2& z, + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + /// @} /// @name Deprecated /// @{ @@ -157,7 +165,7 @@ private: friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 803c59a4b..1b9559e67 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -18,165 +18,145 @@ #pragma once -#include #include +#include +#include namespace gtsam { - /** - * A 2D stereo point, v will be same for rectified images - * @addtogroup geometry - * \nosubgrouping - */ - class GTSAM_EXPORT StereoPoint2 { - public: - static const size_t dimension = 3; - private: - double uL_, uR_, v_; +/** + * A 2D stereo point, v will be same for rectified images + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT StereoPoint2 { +private: - public: + double uL_, uR_, v_; - /// @name Standard Constructors - /// @{ +public: + enum { dimension = 3 }; + /// @name Standard Constructors + /// @{ - /** default constructor */ - StereoPoint2() : + /** default constructor */ + StereoPoint2() : uL_(0), uR_(0), v_(0) { - } + } - /** constructor */ - StereoPoint2(double uL, double uR, double v) : + /** constructor */ + StereoPoint2(double uL, double uR, double v) : uL_(uL), uR_(uR), v_(v) { - } + } - /// @} - /// @name Testable - /// @{ + /// construct from 3D vector + StereoPoint2(const Vector3& v) : + uL_(v(0)), uR_(v(1)), v_(v(2)) {} - /** print */ - void print(const std::string& s="") const; + /// @} + /// @name Testable + /// @{ - /** equals */ - bool equals(const StereoPoint2& q, double tol=1e-9) const { - return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_ - - q.v_) < tol); - } + /** print */ + void print(const std::string& s = "") const; - /// @} - /// @name Group - /// @{ + /** equals */ + bool equals(const StereoPoint2& q, double tol = 1e-9) const { + return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol + && fabs(v_ - q.v_) < tol); + } - /// identity - inline static StereoPoint2 identity() { return StereoPoint2(); } + /// @} + /// @name Group + /// @{ - /// inverse - inline StereoPoint2 inverse() const { - return StereoPoint2()- (*this); - } + /// identity + inline static StereoPoint2 identity() { + return StereoPoint2(); + } - /// "Compose", just adds the coordinates of two points. - inline StereoPoint2 compose(const StereoPoint2& p1) const { - return *this + p1; - } + /// inverse + StereoPoint2 operator-() const { + return StereoPoint2(-uL_, -uR_, -v_); + } - /// add two stereo points - StereoPoint2 operator+(const StereoPoint2& b) const { - return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); - } + /// add + inline StereoPoint2 operator +(const StereoPoint2& b) const { + return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); + } - /// subtract two stereo points - StereoPoint2 operator-(const StereoPoint2& b) const { - return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); - } - - /// @} - /// @name Manifold - /// @{ + /// subtract + inline StereoPoint2 operator -(const StereoPoint2& b) const { + return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); + } - /// dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Standard Interface + /// @{ - /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return dimension; } + /// equality + inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;} - /// Updates a with tangent space delta - inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + /// get uL + inline double uL() const {return uL_;} - /// Returns inverse retraction - inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + /// get uR + inline double uR() const {return uR_;} - /// @} - /// @name Lie Group - /// @{ + /// get v + inline double v() const {return v_;} - /** Exponential map around identity - just create a Point2 from a vector */ - static inline StereoPoint2 Expmap(const Vector& d) { - return StereoPoint2(d(0), d(1), d(2)); - } + /** convert to vector */ + Vector3 vector() const { + return Vector3(uL_, uR_, v_); + } - /** Log map around identity - just return the Point2 as a vector */ - static inline Vector Logmap(const StereoPoint2& p) { - return p.vector(); - } + /** convenient function to get a Point2 from the left image */ + Point2 point2() const { + return Point2(uL_, v_); + } - /** The difference between another point and this point */ - inline StereoPoint2 between(const StereoPoint2& p2) const { - return p2 - *this; - } + /** convenient function to get a Point2 from the right image */ + Point2 right() const { + return Point2(uR_, v_); + } - /// @} - /// @name Standard Interface - /// @{ + /// @name Deprecated + /// @{ + inline StereoPoint2 inverse() const { return StereoPoint2()- (*this);} + inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1;} + inline StereoPoint2 between(const StereoPoint2& p2) const { return p2 - *this; } + inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); } + static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); } + /// @} - /// get uL - inline double uL() const {return uL_;} + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); - /// get uR - inline double uR() const {return uR_;} +private: - /// get v - inline double v() const {return v_;} + /// @} + /// @name Advanced Interface + /// @{ - /** convert to vector */ - Vector3 vector() const { - return Vector3(uL_, uR_, v_); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(uL_); + ar & BOOST_SERIALIZATION_NVP(uR_); + ar & BOOST_SERIALIZATION_NVP(v_); + } - /** convenient function to get a Point2 from the left image */ - Point2 point2() const { - return Point2(uL_, v_); - } + /// @} - /** convenient function to get a Point2 from the right image */ - Point2 right() const { - return Point2(uR_, v_); - } +}; - /// Streaming - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); +template<> +struct traits : public internal::VectorSpace {}; - private: - - /// @} - /// @name Advanced Interface - /// @{ - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(uL_); - ar & BOOST_SERIALIZATION_NVP(uR_); - ar & BOOST_SERIALIZATION_NVP(v_); - } - - /// @} - - }; - - template<> - struct traits : public internal::Manifold {}; - - template<> - struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index be48aecc9..cc3865b8e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -119,7 +119,7 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 Matrix23 Bt = basis().transpose(); Vector2 xi = Bt * q.p_.vector(); diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 7d145c213..12bfac5ce 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -108,7 +108,7 @@ public: } /// Return unit-norm Vector - Vector unitVector(boost::optional H = boost::none) const { + Vector3 unitVector(boost::optional H = boost::none) const { if (H) *H = basis(); return (p_.vector()); @@ -120,7 +120,7 @@ public: } /// Signed, vector-valued error between two directions - Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; + Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; /// Distance between two directions double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; @@ -160,7 +160,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(p_); // homebrew serialize Eigen Matrix ar & boost::serialization::make_nvp("B11", (*B_)(0, 0)); diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index a15d7e2c2..7becfc75f 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -22,52 +22,114 @@ using namespace std; using namespace gtsam; -typedef Cyclic<3> G; // Let's use the cyclic group of order 3 +typedef Cyclic<3> Z3; // Let's use the cyclic group of order 3 +typedef Cyclic<2> Z2; //****************************************************************************** TEST(Cyclic, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - EXPECT_LONGS_EQUAL(0, traits::Identity()); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT_LONGS_EQUAL(0, traits::Identity()); } //****************************************************************************** TEST(Cyclic, Constructor) { - G g(0); + Z3 g(0); } //****************************************************************************** TEST(Cyclic, Compose) { - EXPECT_LONGS_EQUAL(0, traits::Compose(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits::Compose(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits::Compose(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits::Compose(Z3(0),Z3(0))); + EXPECT_LONGS_EQUAL(1, traits::Compose(Z3(0),Z3(1))); + EXPECT_LONGS_EQUAL(2, traits::Compose(Z3(0),Z3(2))); - EXPECT_LONGS_EQUAL(2, traits::Compose(G(2),G(0))); - EXPECT_LONGS_EQUAL(0, traits::Compose(G(2),G(1))); - EXPECT_LONGS_EQUAL(1, traits::Compose(G(2),G(2))); + EXPECT_LONGS_EQUAL(2, traits::Compose(Z3(2),Z3(0))); + EXPECT_LONGS_EQUAL(0, traits::Compose(Z3(2),Z3(1))); + EXPECT_LONGS_EQUAL(1, traits::Compose(Z3(2),Z3(2))); } //****************************************************************************** TEST(Cyclic, Between) { - EXPECT_LONGS_EQUAL(0, traits::Between(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits::Between(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits::Between(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits::Between(Z3(0),Z3(0))); + EXPECT_LONGS_EQUAL(1, traits::Between(Z3(0),Z3(1))); + EXPECT_LONGS_EQUAL(2, traits::Between(Z3(0),Z3(2))); - EXPECT_LONGS_EQUAL(1, traits::Between(G(2),G(0))); - EXPECT_LONGS_EQUAL(2, traits::Between(G(2),G(1))); - EXPECT_LONGS_EQUAL(0, traits::Between(G(2),G(2))); + EXPECT_LONGS_EQUAL(1, traits::Between(Z3(2),Z3(0))); + EXPECT_LONGS_EQUAL(2, traits::Between(Z3(2),Z3(1))); + EXPECT_LONGS_EQUAL(0, traits::Between(Z3(2),Z3(2))); } //****************************************************************************** -TEST(Cyclic, Ivnverse) { - EXPECT_LONGS_EQUAL(0, traits::Inverse(G(0))); - EXPECT_LONGS_EQUAL(2, traits::Inverse(G(1))); - EXPECT_LONGS_EQUAL(1, traits::Inverse(G(2))); +TEST(Cyclic, Inverse) { + EXPECT_LONGS_EQUAL(0, traits::Inverse(Z3(0))); + EXPECT_LONGS_EQUAL(2, traits::Inverse(Z3(1))); + EXPECT_LONGS_EQUAL(1, traits::Inverse(Z3(2))); +} + +//****************************************************************************** +TEST(Cyclic, Negation) { + EXPECT_LONGS_EQUAL(0, -Z3(0)); + EXPECT_LONGS_EQUAL(2, -Z3(1)); + EXPECT_LONGS_EQUAL(1, -Z3(2)); +} + +//****************************************************************************** +TEST(Cyclic, Negation2) { + EXPECT_LONGS_EQUAL(0, -Z2(0)); + EXPECT_LONGS_EQUAL(1, -Z2(1)); } //****************************************************************************** TEST(Cyclic , Invariants) { - G g(2), h(1); - check_group_invariants(g,h); + Z3 g(2), h(1); + EXPECT(check_group_invariants(g,h)); +} + +//****************************************************************************** +// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the +// smallest non-cyclic group called the Klein four-group: +typedef DirectSum K4; + +namespace gtsam { + +/// Define K4 to be a model of the Additive Group concept, and provide Testable +template<> +struct traits : internal::AdditiveGroupTraits { + static void Print(const K4& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second << ")" << endl; + } + static bool Equals(const K4& m1, const K4& m2, double tol = 1e-8) { + return m1 == m2; + } +}; + +} // namespace gtsam + +TEST(Cyclic , DirectSum) { + // The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the + // smallest non-cyclic group called the Klein four-group: + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsTestable)); + + // Refer to http://en.wikipedia.org/wiki/Klein_four-group + K4 e(0,0), a(0, 1), b(1, 0), c(1, 1); + EXPECT(assert_equal(a, - a)); + EXPECT(assert_equal(b, - b)); + EXPECT(assert_equal(c, - c)); + EXPECT(assert_equal(a, a + e)); + EXPECT(assert_equal(b, b + e)); + EXPECT(assert_equal(c, c + e)); + EXPECT(assert_equal(e, a + a)); + EXPECT(assert_equal(e, b + b)); + EXPECT(assert_equal(e, c + c)); + EXPECT(assert_equal(c, a + b)); + EXPECT(assert_equal(b, a + c)); + EXPECT(assert_equal(a, b + c)); + EXPECT(assert_equal(c, a - b)); + EXPECT(assert_equal(a, b - c)); + EXPECT(assert_equal(b, c - a)); + EXPECT(check_group_invariants(a, b)); + EXPECT(check_group_invariants(b, c)); + EXPECT(check_group_invariants(c, a)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index d2990a747..fe27b2911 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -30,6 +30,14 @@ TEST (EssentialMatrix, equality) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, FromPose3) { + EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)); + Pose3 pose(c1Rc2, c1Tc2); + EssentialMatrix actual = EssentialMatrix::FromPose3(pose); + EXPECT(assert_equal(expected, actual)); +} + //******************************************************************************* TEST(EssentialMatrix, localCoordinates0) { EssentialMatrix E; @@ -47,11 +55,11 @@ TEST (EssentialMatrix, localCoordinates) { Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); EXPECT(assert_equal(zero(5), actual, 1e-8)); - Vector d = zero(6); - d(4) += 1e-5; + Vector6 d; + d << 0.1, 0.2, 0.3, 0, 0, 0; Vector actual2 = hx.localCoordinates( EssentialMatrix::FromPose3(pose.retract(d))); - EXPECT(assert_equal(zero(5), actual2, 1e-8)); + EXPECT(assert_equal(d.head(5), actual2, 1e-8)); } //************************************************************************* @@ -75,6 +83,15 @@ TEST (EssentialMatrix, retract2) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, RoundTrip) { + Vector5 d; + d << 0.1, 0.2, 0.3, 0.4, 0.5; + EssentialMatrix e, hx = e.retract(d); + Vector actual = e.localCoordinates(hx); + EXPECT(assert_equal(d, actual, 1e-8)); +} + //************************************************************************* Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index fce7955e9..6f8d45b3e 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -37,8 +37,8 @@ TEST(Double , Concept) { //****************************************************************************** TEST(Double , Invariants) { double p1(2), p2(5); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } //****************************************************************************** @@ -51,8 +51,8 @@ TEST(Point2 , Concept) { //****************************************************************************** TEST(Point2 , Invariants) { Point2 p1(1, 2), p2(4, 5); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } /* ************************************************************************* */ @@ -61,6 +61,12 @@ TEST(Point2, constructor) { EXPECT(assert_equal(p1, p2)); } +/* ************************************************************************* */ +TEST(Point2, equality) { + Point2 p1(1, 2), p2(1,3); + EXPECT(!(p1 == p2)); +} + /* ************************************************************************* */ TEST(Point2, Lie) { Point2 p1(1, 2), p2(4, 5); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 5c6b32dca..6811ed122 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -36,8 +36,8 @@ TEST(Point3 , Concept) { //****************************************************************************** TEST(Point3 , Invariants) { Point3 p1(1, 2, 3), p2(4, 5, 6); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 803bb7c3f..5b835200a 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -775,15 +775,15 @@ Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0)); TEST(Pose2 , Invariants) { Pose2 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index a716406a4..98c80e356 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -186,6 +186,17 @@ TEST(Pose3, expmaps_galore_full) EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); } +/* ************************************************************************* */ +// Check translation and its pushforward +TEST(Pose3, translation) { + Matrix actualH; + EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8)); + + Matrix numericalH = numericalDerivative11( + boost::bind(&Pose3::translation, _1, boost::none), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + /* ************************************************************************* */ TEST(Pose3, Adjoint_compose_full) { @@ -749,15 +760,15 @@ TEST( Pose3, stream) TEST(Pose3 , Invariants) { Pose3 id; - check_group_invariants(id,id); - check_group_invariants(id,T3); - check_group_invariants(T2,id); - check_group_invariants(T2,T3); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T3)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T3)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T3); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T3); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T3)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T3)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 82d3283bd..0421e1e44 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -107,15 +107,15 @@ TEST(Quaternion , Inverse) { //****************************************************************************** TEST(Quaternion , Invariants) { - check_group_invariants(id, id); - check_group_invariants(id, R1); - check_group_invariants(R2, id); - check_group_invariants(R2, R1); + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, R1)); + EXPECT(check_group_invariants(R2, id)); + EXPECT(check_group_invariants(R2, R1)); - check_manifold_invariants(id, id); - check_manifold_invariants(id, R1); - check_manifold_invariants(R2, id); - check_manifold_invariants(R2, R1); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, R1)); + EXPECT(check_manifold_invariants(R2, id)); + EXPECT(check_manifold_invariants(R2, R1)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 37054fd83..6ead22860 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -163,15 +163,15 @@ Rot2 T2(0.2); TEST(Rot2 , Invariants) { Rot2 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 7e0dcc43f..349f87029 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -659,17 +659,17 @@ Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); TEST(Rot3 , Invariants) { Rot3 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); - check_group_invariants(T1,T2); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); + EXPECT(check_group_invariants(T1,T2)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); - check_manifold_invariants(T1,T2); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); + EXPECT(check_manifold_invariants(T1,T2)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index acbf3bcf5..bc32e0df0 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -57,15 +57,15 @@ TEST(SO3 , Retract) { //****************************************************************************** TEST(SO3 , Invariants) { - check_group_invariants(id,id); - check_group_invariants(id,R1); - check_group_invariants(R2,id); - check_group_invariants(R2,R1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,R1)); + EXPECT(check_group_invariants(R2,id)); + EXPECT(check_group_invariants(R2,R1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,R1); - check_manifold_invariants(R2,id); - check_manifold_invariants(R2,R1); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,R1)); + EXPECT(check_manifold_invariants(R2,id)); + EXPECT(check_manifold_invariants(R2,R1)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 45f26c244..3adf2257d 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -99,13 +99,13 @@ TEST( StereoCamera, Dproject) Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); CHECK(assert_equal(expected1,actual1,1e-7)); - Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K); + Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K); Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); CHECK(assert_equal(expected2,actual2,1e-7)); } /* ************************************************************************* */ -TEST( StereoCamera, backproject) +TEST( StereoCamera, backproject_case1) { Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); StereoCamera stereoCam2(Pose3(), K2); @@ -117,7 +117,7 @@ TEST( StereoCamera, backproject) } /* ************************************************************************* */ -TEST( StereoCamera, backproject2) +TEST( StereoCamera, backproject_case2) { Rot3 R(0.589511291, -0.804859792, 0.0683931805, -0.804435942, -0.592650676, -0.0405925523, @@ -132,6 +132,53 @@ TEST( StereoCamera, backproject2) CHECK(assert_equal(z, actual, 1e-3)); } +static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) { + return StereoCamera(pose, boost::make_shared(K)).backproject(point); +} + +/* ************************************************************************* */ +TEST( StereoCamera, backproject2_case1) +{ + Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam2(Pose3(), K2); + + Point3 expected_point(1.2, 2.3, 4.5); + StereoPoint2 stereo_point = stereoCam2.project(expected_point); + + Matrix actual_jacobian_1, actual_jacobian_2; + Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2); + CHECK(assert_equal(expected_point, actual_point, 1e-8)); + + Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6)); + + Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6)); +} + +TEST( StereoCamera, backproject2_case2) +{ + Rot3 R(0.589511291, -0.804859792, 0.0683931805, + -0.804435942, -0.592650676, -0.0405925523, + 0.0732045588, -0.0310882277, -0.996832359); + Point3 t(53.5239823, 23.7866016, -4.42379876); + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + StereoCamera camera(Pose3(R,t), K); + StereoPoint2 z(184.812, 129.068, 714.768); + + Matrix actual_jacobian_1, actual_jacobian_2; + Point3 l = camera.backproject2(z, actual_jacobian_1, actual_jacobian_2); + + StereoPoint2 actual = camera.project(l); + CHECK(assert_equal(z, actual, 1e-3)); + + Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(R,t), z, *K); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6)); + + Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(R,t), z, *K); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 5496b8aac..ddcc9238a 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -31,7 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) //****************************************************************************** TEST(StereoPoint2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index f473b4010..c92aa8237 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -23,14 +23,8 @@ namespace gtsam { -/** - * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 - * @param projection_matrices Projection matrices (K*P^-1) - * @param measurements 2D measurements - * @param rank_tol SVD rank tolerance - * @return Triangulated Point3 - */ -Point3 triangulateDLT(const std::vector& projection_matrices, +Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { // number of cameras @@ -56,7 +50,16 @@ Point3 triangulateDLT(const std::vector& projection_matrices, if (rank < 3) throw(TriangulationUnderconstrainedException()); - // Create 3D point from eigenvector + return v; +} + +Point3 triangulateDLT(const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol) { + + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, + rank_tol); + + // Create 3D point from homogeneous coordinates return Point3(sub((v / v(3)), 0, 3)); } @@ -88,6 +91,5 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } - } // \namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index e7b2a53f3..adaa3dbaf 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -43,6 +43,17 @@ public: } }; +/** + * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 + * @param projection_matrices Projection matrices (K*P^-1) + * @param measurements 2D measurements + * @param rank_tol SVD rank tolerance + * @return Triangulated point, in homogeneous coordinates + */ +GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol = 1e-9); + /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) @@ -52,7 +63,7 @@ public: */ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol); + const std::vector& measurements, double rank_tol = 1e-9); /** * Create a factor graph with projection factors from poses and one calibration @@ -94,8 +105,9 @@ std::pair triangulationGraph( */ template std::pair triangulationGraph( - const std::vector& cameras, const std::vector& measurements, - Key landmarkKey, const Point3& initialEstimate) { + const std::vector& cameras, + const std::vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; @@ -159,7 +171,8 @@ Point3 triangulateNonlinear(const std::vector& poses, * @return refined Point3 */ template -Point3 triangulateNonlinear(const std::vector& cameras, +Point3 triangulateNonlinear( + const std::vector& cameras, const std::vector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values @@ -180,6 +193,25 @@ Point3 triangulateNonlinear( (cameras, measurements, initialEstimate); } +/** + * Create a 3*4 camera projection matrix from calibration and pose. + * Functor for partial application on calibration + * @param pose The camera pose + * @param cal The calibration + * @return Returns a Matrix34 + */ +template +struct CameraProjectionMatrix { + CameraProjectionMatrix(const CALIBRATION& calibration) : + K_(calibration.K()) { + } + Matrix34 operator()(const Pose3& pose) const { + return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); + } +private: + const Matrix3 K_; +}; + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -204,11 +236,11 @@ Point3 triangulatePoint3(const std::vector& poses, // construct projection matrices from poses & calibration std::vector projection_matrices; - BOOST_FOREACH(const Pose3& pose, poses) { - projection_matrices.push_back( - sharedCal->K() * (pose.inverse().matrix()).block<3, 4>(0, 0)); - } - // Do DLT: throws TriangulationUnderconstrainedException if rank<3 + CameraProjectionMatrix createP(*sharedCal); // partially apply + BOOST_FOREACH(const Pose3& pose, poses) + projection_matrices.push_back(createP(pose)); + + // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // Then refine using non-linear optimization @@ -241,24 +273,23 @@ Point3 triangulatePoint3(const std::vector& poses, * @return Returns a Point3 */ template -Point3 triangulatePoint3(const std::vector& cameras, +Point3 triangulatePoint3( + const std::vector& cameras, const std::vector& measurements, double rank_tol = 1e-9, bool optimize = false) { size_t m = cameras.size(); - assert(measurements.size()==m); + assert(measurements.size() == m); if (m < 2) throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration std::vector projection_matrices; - BOOST_FOREACH(const CAMERA& camera, cameras) { - Matrix P_ = (camera.pose().inverse().matrix()); + BOOST_FOREACH(const CAMERA& camera, cameras) projection_matrices.push_back( - camera.calibration().K() * (P_.block<3, 4>(0, 0))); - } - // Do DLT: throws TriangulationUnderconstrainedException if rank<3 + CameraProjectionMatrix(camera.calibration())( + camera.pose())); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 830ddd3ec..ff50c9781 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -253,7 +253,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(nodes_); ar & BOOST_SERIALIZATION_NVP(roots_); } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index beb222178..eae886785 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -160,7 +160,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(conditional_); ar & BOOST_SERIALIZATION_NVP(parent_); ar & BOOST_SERIALIZATION_NVP(children); diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 13130bf2e..ad7ab0063 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -99,7 +99,7 @@ namespace gtsam // Do dense elimination step std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, Ordering(node->keys)); + eliminationFunction(gatheredFactors, node->orderedFrontalKeys); // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor myData.bayesTreeNode->setEliminationResult(eliminationResult); @@ -123,7 +123,7 @@ namespace gtsam const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s; - BOOST_FOREACH(Key j, keys) + BOOST_FOREACH(Key j, orderedFrontalKeys) std::cout << j << " "; std::cout << "problemSize = " << problemSize_ << std::endl; } diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 5a412a79e..435631b21 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -11,7 +11,7 @@ #include #include -#include +#include namespace gtsam { @@ -37,11 +37,11 @@ namespace gtsam typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine struct Cluster { - typedef FastVector Keys; + typedef Ordering Keys; typedef FastVector Factors; typedef FastVector > Children; - Keys keys; ///< Frontal keys of this node + Keys orderedFrontalKeys; ///< Frontal keys of this node Factors factors; ///< Factors associated with this node Children children; ///< sub-trees int problemSize_; diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index bdfec9cd5..39c738a19 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -141,7 +141,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(nrFrontals_); } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index d66f6b8ac..3e41d7692 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -160,7 +160,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(keys_); } diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index adb1c0aad..4d5428e5c 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -342,7 +342,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(factors_); } diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 9d96c5b9c..f12e5afd2 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -49,7 +49,7 @@ namespace gtsam { // structure with its own JT node, and create a child pointer in its parent. ConstructorTraversalData myData = ConstructorTraversalData(&parentData); myData.myJTNode = boost::make_shared::Node>(); - myData.myJTNode->keys.push_back(node->key); + myData.myJTNode->orderedFrontalKeys.push_back(node->key); myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end()); parentData.myJTNode->children.push_back(myData.myJTNode); return myData; @@ -101,13 +101,20 @@ namespace gtsam { const typename JunctionTree::Node& childToMerge = *myData.myJTNode->children[child - nrMergedChildren]; // Merge keys, factors, and children. - myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end()); - myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end()); - myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end()); + myData.myJTNode->orderedFrontalKeys.insert( + myData.myJTNode->orderedFrontalKeys.begin(), + childToMerge.orderedFrontalKeys.begin(), + childToMerge.orderedFrontalKeys.end()); + myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), + childToMerge.factors.begin(), + childToMerge.factors.end()); + myData.myJTNode->children.insert(myData.myJTNode->children.end(), + childToMerge.children.begin(), + childToMerge.children.end()); // Increment problem size combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_); // Increment number of frontal variables - myNrFrontals += childToMerge.keys.size(); + myNrFrontals += childToMerge.orderedFrontalKeys.size(); // Remove child from list. myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren)); // Increment number of merged children diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 23936afed..452c98434 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -109,7 +109,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(label_); ar & BOOST_SERIALIZATION_NVP(j_); diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index e25590578..9de3fb66a 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -18,15 +18,15 @@ #pragma once -#include -#include -#include - -#include #include #include #include #include +#include + +#include +#include +#include namespace gtsam { @@ -104,9 +104,9 @@ namespace gtsam { /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains /// the variables in \c constrainLast to the end of the ordering, and orders all other variables /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c - /// constrainLast will be ordered in the same order specified in the vector \c - /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be - /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + /// constrainFirst will be ordered in the same order specified in the vector \c + /// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be + /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template static Ordering colamdConstrainedFirst(const FactorGraph& graph, const std::vector& constrainFirst, bool forceOrder = false) { @@ -117,7 +117,7 @@ namespace gtsam { /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the /// variables in \c constrainFirst will be ordered in the same order specified in the /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c - /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to + /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to /// reduce fill-in as well. static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder = false); @@ -187,7 +187,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 4f9734639..68d927baf 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -117,7 +117,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(j_); } diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h new file mode 100644 index 000000000..23d11964c --- /dev/null +++ b/gtsam/linear/BinaryJacobianFactor.h @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * 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 BinaryJacobianFactor.h + * + * @brief A binary JacobianFactor specialization that uses fixed matrix math for speed + * + * @date June 2015 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * A binary JacobianFactor specialization that uses fixed matrix math for speed + */ +template +struct BinaryJacobianFactor: JacobianFactor { + + /// Constructor + BinaryJacobianFactor(Key key1, const Eigen::Matrix& A1, + Key key2, const Eigen::Matrix& A2, + const Eigen::Matrix& b, // + const SharedDiagonal& model = SharedDiagonal()) : + JacobianFactor(key1, A1, key2, A2, b, model) { + } + + inline Key key1() const { + return keys_[0]; + } + inline Key key2() const { + return keys_[1]; + } + + // Fixed-size matrix update + void updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_BinaryJacobianFactor); + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw std::invalid_argument( + "BinaryJacobianFactor::updateHessian: cannot update information with " + "constrained noise model"); + BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())), + key2(), model->Whiten(getA(end())), model->whiten(getb())); + whitenedFactor.updateHessian(infoKeys, info); + } else { + // First build an array of slots + DenseIndex slot1 = Slot(infoKeys, key1()); + DenseIndex slot2 = Slot(infoKeys, key2()); + DenseIndex slotB = info->nBlocks() - 1; + + const Matrix& Ab = Ab_.matrix(); + Eigen::Block A1(Ab, 0, 0); + Eigen::Block A2(Ab, 0, N1); + Eigen::Block b(Ab, 0, N1 + N2); + + // We perform I += A'*A to the upper triangle + (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose()); + (*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2; + (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; + (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); + (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; + (*info)(slotB, slotB)(0, 0) += b.transpose() * b; + } + } +}; + +template +struct traits > : Testable< + BinaryJacobianFactor > { +}; + +} //namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index a89e7e21c..401583bbf 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -166,7 +166,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index e0a820819..d9b75c69f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -135,7 +135,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index c67636341..7f9c5ea3c 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -28,6 +28,8 @@ namespace gtsam { // Forward declarations class VectorValues; + class Scatter; + class SymmetricBlockMatrix; /** * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a @@ -119,6 +121,14 @@ namespace gtsam { */ virtual GaussianFactor::shared_ptr negate() const = 0; + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + virtual void updateHessian(const FastVector& keys, + SymmetricBlockMatrix* info) const = 0; + /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; @@ -131,16 +141,22 @@ namespace gtsam { /// Gradient wrt a key at any values virtual Vector gradient(Key key, const VectorValues& x) const = 0; + // Determine position of a given key + template + static DenseIndex Slot(const CONTAINER& keys, Key key) { + return std::find(keys.begin(), keys.end(), key) - keys.begin(); + } + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; // GaussianFactor - + /// traits template<> struct traits : public Testable { diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 6953d2969..cd60a3eb9 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -123,26 +123,32 @@ namespace gtsam { /* ************************************************************************* */ vector > GaussianFactorGraph::sparseJacobian() const { // First find dimensions of each variable - vector dims; + typedef std::map KeySizeMap; + KeySizeMap dims; BOOST_FOREACH(const sharedFactor& factor, *this) { - for (GaussianFactor::const_iterator pos = factor->begin(); - pos != factor->end(); ++pos) { - if (dims.size() <= *pos) - dims.resize(*pos + 1, 0); - dims[*pos] = factor->getDim(pos); + if (!static_cast(factor)) continue; + + for (GaussianFactor::const_iterator key = factor->begin(); + key != factor->end(); ++key) { + dims[*key] = factor->getDim(key); } } // Compute first scalar column of each variable - vector columnIndices(dims.size() + 1, 0); - for (size_t j = 1; j < dims.size() + 1; ++j) - columnIndices[j] = columnIndices[j - 1] + dims[j - 1]; + size_t currentColIndex = 0; + KeySizeMap columnIndices = dims; + BOOST_FOREACH(const KeySizeMap::value_type& col, dims) { + columnIndices[col.first] = currentColIndex; + currentColIndex += dims[col.first]; + } // Iterate over all factors, adding sparse scalar entries typedef boost::tuple triplet; vector entries; size_t row = 0; BOOST_FOREACH(const sharedFactor& factor, *this) { + if (!static_cast(factor)) continue; + // Convert to JacobianFactor if necessary JacobianFactor::shared_ptr jacobianFactor( boost::dynamic_pointer_cast(factor)); @@ -159,11 +165,11 @@ namespace gtsam { // Whiten the factor and add entries for it // iterate over all variables in the factor const JacobianFactor whitened(jacobianFactor->whiten()); - for (JacobianFactor::const_iterator pos = whitened.begin(); - pos < whitened.end(); ++pos) { - JacobianFactor::constABlock whitenedA = whitened.getA(pos); + 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[*pos]; + 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); @@ -173,7 +179,7 @@ namespace gtsam { } JacobianFactor::constBVector whitenedb(whitened.getb()); - size_t bcolumn = columnIndices.back(); + size_t bcolumn = currentColIndex; for (size_t i = 0; i < (size_t) whitenedb.size(); i++) entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i))); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 811c0f8b0..832114ff6 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -322,7 +322,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f2bebcab9..21f4b117f 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -15,17 +15,17 @@ * @date Dec 8, 2010 */ -#include -#include -#include -#include -#include #include #include #include #include #include #include +#include +#include +#include +#include +#include #include #include @@ -49,232 +49,185 @@ using namespace std; using namespace boost::assign; -namespace br { using namespace boost::range; using namespace boost::adaptors; } +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} namespace gtsam { -/* ************************************************************************* */ -string SlotEntry::toString() const { - ostringstream oss; - oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; - return oss.str(); -} - -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { - gttic(Scatter_Constructor); - static const size_t none = std::numeric_limits::max(); - - // First do the set union. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { - if (factor) { - for (GaussianFactor::const_iterator variable = factor->begin(); - variable != factor->end(); ++variable) { - // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers - const JacobianFactor* asJacobian = - dynamic_cast(factor.get()); - if (!asJacobian || asJacobian->cols() > 1) - this->insert( - make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); - } - } - } - - // If we have an ordering, pre-fill the ordered variables first - size_t slot = 0; - if (ordering) { - BOOST_FOREACH(Key key, *ordering) { - const_iterator entry = find(key); - if (entry == end()) - throw std::invalid_argument( - "The ordering provided to the HessianFactor Scatter constructor\n" - "contained extra variables that did not appear in the factors to combine."); - at(key).slot = (slot++); - } - } - - // Next fill in the slot indices (we can only get these after doing the set - // union. - BOOST_FOREACH(value_type& var_slot, *this) { - if (var_slot.second.slot == none) - var_slot.second.slot = (slot++); - } -} - /* ************************************************************************* */ HessianFactor::HessianFactor() : - info_(cref_list_of<1>(1)) -{ + info_(cref_list_of<1>(1)) { linearTerm().setZero(); constantTerm() = 0.0; } /* ************************************************************************* */ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) : - GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) -{ - if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0,0) = G; - info_(0,1) = g; - info_(1,1)(0,0) = f; + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) { + if (G.rows() != G.cols() || G.rows() != g.size()) + throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0, 0) = G; + info_(0, 1) = g; + info_(1, 1)(0, 0) = f; } /* ************************************************************************* */ // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : - GaussianFactor(cref_list_of<1>(j)), - info_(cref_list_of<2> (Sigma.cols()) (1) ) -{ - if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0,0) = Sigma.inverse(); // G - info_(0,1) = info_(0,0).selfadjointView() * mu; // g - info_(1,1)(0,0) = mu.dot(info_(0,1).knownOffDiagonal().col(0)); // f + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) { + if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) + throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0, 0) = Sigma.inverse(); // G + info_(0, 1) = info_(0, 0).selfadjointView() * mu; // g + info_(1, 1)(0, 0) = mu.dot(info_(0, 1).knownOffDiagonal().col(0)); // f } /* ************************************************************************* */ -HessianFactor::HessianFactor(Key j1, Key j2, - const Matrix& G11, const Matrix& G12, const Vector& g1, - const Matrix& G22, const Vector& g2, double f) : - GaussianFactor(cref_list_of<2>(j1)(j2)), - info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) -{ - info_(0,0) = G11; - info_(0,1) = G12; - info_(0,2) = g1; - info_(1,1) = G22; - info_(1,2) = g2; - info_(2,2)(0,0) = f; +HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11, + const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2, + double f) : + GaussianFactor(cref_list_of<2>(j1)(j2)), info_( + cref_list_of<3>(G11.cols())(G22.cols())(1)) { + info_(0, 0) = G11; + info_(0, 1) = G12; + info_(0, 2) = g1; + info_(1, 1) = G22; + info_(1, 2) = g2; + info_(2, 2)(0, 0) = f; } /* ************************************************************************* */ -HessianFactor::HessianFactor(Key j1, Key j2, Key j3, - const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, - const Matrix& G22, const Matrix& G23, const Vector& g2, - const Matrix& G33, const Vector& g3, double f) : - GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), - info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) -{ - if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || - G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - info_(0,0) = G11; - info_(0,1) = G12; - info_(0,2) = G13; - info_(0,3) = g1; - info_(1,1) = G22; - info_(1,2) = G23; - info_(1,3) = g2; - info_(2,2) = G33; - info_(2,3) = g3; - info_(3,3)(0,0) = f; +HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, + const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22, + const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3, + double f) : + GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_( + cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) { + if (G11.rows() != G11.cols() || G11.rows() != G12.rows() + || G11.rows() != G13.rows() || G11.rows() != g1.size() + || G22.cols() != G12.cols() || G33.cols() != G13.cols() + || G22.cols() != g2.size() || G33.cols() != g3.size()) + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + info_(0, 0) = G11; + info_(0, 1) = G12; + info_(0, 2) = G13; + info_(0, 3) = g1; + info_(1, 1) = G22; + info_(1, 2) = G23; + info_(1, 3) = g2; + info_(2, 2) = G33; + info_(2, 3) = g3; + info_(3, 3)(0, 0) = f; } /* ************************************************************************* */ namespace { -DenseIndex _getSizeHF(const Vector& m) { return m.size(); } +DenseIndex _getSizeHF(const Vector& m) { + return m.size(); +} } /* ************************************************************************* */ -HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, - const std::vector& gs, double f) : - GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) -{ +HessianFactor::HessianFactor(const std::vector& js, + const std::vector& Gs, const std::vector& gs, double f) : + GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) { // Get the number of variables size_t variable_count = js.size(); // Verify the provided number of entries in the vectors are consistent - if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2) - throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ + if (gs.size() != variable_count + || Gs.size() != (variable_count * (variable_count + 1)) / 2) + throw invalid_argument( + "Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2"); // Verify the dimensions of each provided matrix are consistent // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula - for(size_t i = 0; i < variable_count; ++i){ + for (size_t i = 0; i < variable_count; ++i) { DenseIndex block_size = gs[i].size(); // Check rows - for(size_t j = 0; j < variable_count-i; ++j){ - size_t index = i*(2*variable_count - i + 1)/2 + j; - if(Gs[index].rows() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + for (size_t j = 0; j < variable_count - i; ++j) { + size_t index = i * (2 * variable_count - i + 1) / 2 + j; + if (Gs[index].rows() != block_size) { + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); } } // Check cols - for(size_t j = 0; j <= i; ++j){ - size_t index = j*(2*variable_count - j + 1)/2 + (i-j); - if(Gs[index].cols() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + for (size_t j = 0; j <= i; ++j) { + size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j); + if (Gs[index].cols() != block_size) { + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); } } } // Fill in the blocks size_t index = 0; - for(size_t i = 0; i < variable_count; ++i){ - for(size_t j = i; j < variable_count; ++j){ + for (size_t i = 0; i < variable_count; ++i) { + for (size_t j = i; j < variable_count; ++j) { info_(i, j) = Gs[index++]; } info_(i, variable_count) = gs[i]; } - info_(variable_count, variable_count)(0,0) = f; + info_(variable_count, variable_count)(0, 0) = f; } /* ************************************************************************* */ namespace { -void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) -{ +void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) { gttic(HessianFactor_fromJacobian); const SharedDiagonal& jfModel = jf.get_model(); - if(jfModel) - { - if(jf.get_model()->isConstrained()) - throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - info.full().triangularView() = jf.matrixObject().full().transpose() * - (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() * - jf.matrixObject().full(); + if (jfModel) { + if (jf.get_model()->isConstrained()) + throw invalid_argument( + "Cannot construct HessianFactor from JacobianFactor with constrained noise model"); + info.full().triangularView() = + jf.matrixObject().full().transpose() + * (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() + * jf.matrixObject().full(); } else { - info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); + info.full().triangularView() = jf.matrixObject().full().transpose() + * jf.matrixObject().full(); } } } /* ************************************************************************* */ HessianFactor::HessianFactor(const JacobianFactor& jf) : - GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) -{ + GaussianFactor(jf), info_( + SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) { _FromJacobianHelper(jf, info_); } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactor& gf) : - GaussianFactor(gf) -{ + GaussianFactor(gf) { // Copy the matrix data depending on what type of factor we're copying from - if(const JacobianFactor* jf = dynamic_cast(&gf)) - { + if (const JacobianFactor* jf = dynamic_cast(&gf)) { info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject()); _FromJacobianHelper(*jf, info_); - } - else if(const HessianFactor* hf = dynamic_cast(&gf)) - { + } else if (const HessianFactor* hf = dynamic_cast(&gf)) { info_ = hf->info_; - } - else - { - throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); + } else { + throw std::invalid_argument( + "In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); } } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter) -{ + boost::optional scatter) { gttic(HessianFactor_MergeConstructor); boost::optional computedScatter; - if(!scatter) { + if (!scatter) { computedScatter = Scatter(factors); scatter = computedScatter; } @@ -296,64 +249,50 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Form A' * A gttic(update); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) - { - if(factor) { - if(const HessianFactor* hessian = dynamic_cast(factor.get())) - updateATA(*hessian, *scatter); - else if(const JacobianFactor* jacobian = dynamic_cast(factor.get())) - updateATA(*jacobian, *scatter); - else - throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); - } - } + if (factor) + factor->updateHessian(keys_, &info_); gttoc(update); } /* ************************************************************************* */ -void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const { +void HessianFactor::print(const std::string& s, + const KeyFormatter& formatter) const { cout << s << "\n"; cout << " keys: "; - for(const_iterator key=this->begin(); key!=this->end(); ++key) - cout << formatter(*key) << "(" << this->getDim(key) << ") "; + for (const_iterator key = begin(); key != end(); ++key) + cout << formatter(*key) << "(" << getDim(key) << ") "; cout << "\n"; - gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: "); + gtsam::print(Matrix(info_.full().selfadjointView()), + "Augmented information matrix: "); } /* ************************************************************************* */ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { - if(!dynamic_cast(&lf)) + const HessianFactor* rhs = dynamic_cast(&lf); + if (!rhs || !Factor::equals(lf, tol)) return false; - else { - if(!Factor::equals(lf, tol)) - return false; - Matrix thisMatrix = this->info_.full().selfadjointView(); - thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); - rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; - return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); - } + return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(), + tol); } /* ************************************************************************* */ -Matrix HessianFactor::augmentedInformation() const -{ +Matrix HessianFactor::augmentedInformation() const { return info_.full().selfadjointView(); } /* ************************************************************************* */ -Matrix HessianFactor::information() const -{ - return info_.range(0, this->size(), 0, this->size()).selfadjointView(); +Matrix HessianFactor::information() const { + return info_.range(0, size(), 0, size()).selfadjointView(); } /* ************************************************************************* */ VectorValues HessianFactor::hessianDiagonal() const { VectorValues d; // Loop over all variables - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal Matrix B = info_(j, j).selfadjointView(); - d.insert(keys_[j],B.diagonal()); + d.insert(keys_[j], B.diagonal()); } return d; } @@ -366,26 +305,24 @@ void HessianFactor::hessianDiagonal(double* d) const { } /* ************************************************************************* */ -map HessianFactor::hessianBlockDiagonal() const { - map blocks; +map HessianFactor::hessianBlockDiagonal() const { + map blocks; // Loop over all variables - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert it Matrix B = info_(j, j).selfadjointView(); - blocks.insert(make_pair(keys_[j],B)); + blocks.insert(make_pair(keys_[j], B)); } return blocks; } /* ************************************************************************* */ -Matrix HessianFactor::augmentedJacobian() const -{ +Matrix HessianFactor::augmentedJacobian() const { return JacobianFactor(*this).augmentedJacobian(); } /* ************************************************************************* */ -std::pair HessianFactor::jacobian() const -{ +std::pair HessianFactor::jacobian() const { return JacobianFactor(*this).jacobian(); } @@ -396,103 +333,34 @@ double HessianFactor::error(const VectorValues& c) const { double xtg = 0, xGx = 0; // extract the relevant subset of the VectorValues // NOTE may not be as efficient - const Vector x = c.vector(this->keys()); + const Vector x = c.vector(keys()); xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; - return 0.5 * (f - 2.0 * xtg + xGx); + xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x; + return 0.5 * (f - 2.0 * xtg + xGx); } /* ************************************************************************* */ -void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) -{ - gttic(updateATA); - // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in - // the update factor to slots in the combined factor. - - // First build an array of slots - gttic(slots); - FastVector slots(update.size()); - DenseIndex slot = 0; - BOOST_FOREACH(Key j, update) { - slots[slot] = scatter.at(j).slot; - ++ slot; - } - gttoc(slots); - +void HessianFactor::updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_HessianFactor); // Apply updates to the upper triangle - gttic(update); - size_t nrInfoBlocks = this->info_.nBlocks(); - for(DenseIndex j2=0; j2nBlocks() - 1; + vector slots(n + 1); + for (DenseIndex j = 0; j <= n; ++j) { + const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); + slots[j] = J; + for (DenseIndex i = 0; i <= j; ++i) { + const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid. + (*info)(I, J) += info_(i, j); } } - gttoc(update); -} - -/* ************************************************************************* */ -void HessianFactor::updateATA(const JacobianFactor& update, - const Scatter& scatter) { - - // This function updates 'combined' with the information in 'update'. - // 'scatter' maps variables in the update factor to slots in the combined - // factor. - - gttic(updateATA); - - if (update.rows() > 0) { - gttic(whiten); - // Whiten the factor if it has a noise model - boost::optional _whitenedFactor; - const JacobianFactor* whitenedFactor = &update; - if (update.get_model()) { - if (update.get_model()->isConstrained()) - throw invalid_argument( - "Cannot update HessianFactor from JacobianFactor with constrained noise model"); - _whitenedFactor = update.whiten(); - whitenedFactor = &(*_whitenedFactor); - } - gttoc(whiten); - - // A is the whitened Jacobian matrix A, and we are going to perform I += A'*A below - const VerticalBlockMatrix& A = whitenedFactor->matrixObject(); - - // N is number of variables in HessianFactor, n in JacobianFactor - DenseIndex N = this->info_.nBlocks() - 1, n = A.nBlocks() - 1; - - // First build an array of slots - gttic(slots); - FastVector slots(n + 1); - DenseIndex slot = 0; - BOOST_FOREACH(Key j, update) - slots[slot++] = scatter.at(j).slot; - slots[n] = N; - gttoc(slots); - - // Apply updates to the upper triangle - gttic(update); - // Loop over blocks of A, including RHS with j==n - for (DenseIndex j = 0; j <= n; ++j) { - DenseIndex J = slots[j]; // get block in Hessian - // Fill off-diagonal blocks with Ai'*Aj - for (DenseIndex i = 0; i < j; ++i) { - DenseIndex I = slots[i]; // get block in Hessian - info_(I, J).knownOffDiagonal() += A(i).transpose() * A(j); - } - // Fill diagonal block with Aj'*Aj - info_(J, J).selfadjointView().rankUpdate(A(j).transpose()); - } - gttoc(update); - } } /* ************************************************************************* */ -GaussianFactor::shared_ptr HessianFactor::negate() const -{ +GaussianFactor::shared_ptr HessianFactor::negate() const { shared_ptr result = boost::make_shared(*this); - result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result + result->info_.full().triangularView() = + -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result return result; } @@ -509,7 +377,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column // And fill the above temporary y values, to be added into yvalues after - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // xj is the input vector Vector xj = x.at(keys_[j]); DenseIndex i = 0; @@ -518,13 +386,13 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // blocks on the diagonal are only half y[i] += info_(j, j).selfadjointView() * xj; // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex)size(); ++i) + for (i = j + 1; i < (DenseIndex) size(); ++i) y[i] += info_(i, j).knownOffDiagonal() * xj; } // copy to yvalues - for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) { - bool didNotExist; + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + bool didNotExist; VectorValues::iterator it; boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector()); if (didNotExist) @@ -539,7 +407,7 @@ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; size_t n = size(); for (size_t j = 0; j < n; ++j) - g.insert(keys_[j], -info_(j,n).knownOffDiagonal()); + g.insert(keys_[j], -info_(j, n).knownOffDiagonal()); return g; } @@ -562,8 +430,7 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { if (i > j) { Matrix Gji = info(j, i); Gij = Gji.transpose(); - } - else { + } else { Gij = info(i, j); } // Accumulate Gij*xj to gradf @@ -575,30 +442,34 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { } /* ************************************************************************* */ -std::pair, boost::shared_ptr > -EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) -{ +std::pair, + boost::shared_ptr > EliminateCholesky( + const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminateCholesky); // Build joint factor HessianFactor::shared_ptr jointFactor; try { - jointFactor = boost::make_shared(factors, Scatter(factors, keys)); - } catch(std::invalid_argument&) { + jointFactor = boost::make_shared(factors, + Scatter(factors, keys)); + } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateCholesky was called with a request to eliminate variables that are not\n" - "involved in the provided factors."); + "involved in the provided factors."); } // Do dense elimination GaussianConditional::shared_ptr conditional; try { size_t numberOfKeysToEliminate = keys.size(); - VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate); - conditional = boost::make_shared(jointFactor->keys(), numberOfKeysToEliminate, Ab); + VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial( + numberOfKeysToEliminate); + conditional = boost::make_shared(jointFactor->keys(), + numberOfKeysToEliminate, Ab); // Erase the eliminated keys in the remaining factor - jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate); - } catch(CholeskyFailed&) { + jointFactor->keys_.erase(jointFactor->begin(), + jointFactor->begin() + numberOfKeysToEliminate); + } catch (CholeskyFailed&) { throw IndeterminantLinearSystemException(keys.front()); } @@ -607,9 +478,9 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) } /* ************************************************************************* */ -std::pair, boost::shared_ptr > -EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) -{ +std::pair, + boost::shared_ptr > EliminatePreferCholesky( + const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminatePreferCholesky); // If any JacobianFactors have constrained noise models, we have to convert diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 553978e37..b74d557ea 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -18,10 +18,10 @@ #pragma once +#include +#include #include #include -#include -#include #include @@ -41,30 +41,6 @@ namespace gtsam { GTSAM_EXPORT std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); - /** - * One SlotEntry stores the slot index for a variable, as well its dimension. - */ - struct GTSAM_EXPORT SlotEntry { - size_t slot, dimension; - SlotEntry(size_t _slot, size_t _dimension) - : slot(_slot), dimension(_dimension) {} - std::string toString() const; - }; - - /** - * Scatter is an intermediate data structure used when building a HessianFactor - * incrementally, to get the keys in the right order. The "scatter" is a map from - * global variable indices to slot indices in the union of involved variables. - * We also include the dimensionality of the variable. - */ - class Scatter: public FastMap { - public: - Scatter() { - } - Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering = boost::none); - }; - /** * @brief A Gaussian factor using the canonical parameters (information form) * @@ -363,19 +339,12 @@ namespace gtsam { /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */ const SymmetricBlockMatrix& matrixObject() const { return info_; } - /** Update the factor by adding the information from the JacobianFactor + /** Update an information matrix by adding the information corresponding to this factor * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated */ - void updateATA(const JacobianFactor& update, const Scatter& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactor& update, const Scatter& scatter); + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; @@ -434,7 +403,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); ar & BOOST_SERIALIZATION_NVP(info_); } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index eba06f99a..6e14960fd 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -66,10 +66,10 @@ JacobianFactor::JacobianFactor() : /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactor& gf) { // Copy the matrix data depending on what type of factor we're copying from - if (const JacobianFactor* rhs = dynamic_cast(&gf)) - *this = JacobianFactor(*rhs); - else if (const HessianFactor* rhs = dynamic_cast(&gf)) - *this = JacobianFactor(*rhs); + if (const JacobianFactor* asJacobian = dynamic_cast(&gf)) + *this = JacobianFactor(*asJacobian); + else if (const HessianFactor* asHessian = dynamic_cast(&gf)) + *this = JacobianFactor(*asHessian); else throw std::invalid_argument( "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); @@ -365,32 +365,50 @@ void JacobianFactor::print(const string& s, /* ************************************************************************* */ // Check if two linear factors are equal bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const { - if (!dynamic_cast(&f_)) + static const bool verbose = false; + if (!dynamic_cast(&f_)) { + if (verbose) + cout << "JacobianFactor::equals: Incorrect type" << endl; return false; - else { + } else { const JacobianFactor& f(static_cast(f_)); // Check keys - if (keys() != f.keys()) + if (keys() != f.keys()) { + if (verbose) + cout << "JacobianFactor::equals: keys do not match" << endl; return false; + } // Check noise model - if ((model_ && !f.model_) || (!model_ && f.model_)) + if ((model_ && !f.model_) || (!model_ && f.model_)) { + if (verbose) + cout << "JacobianFactor::equals: noise model mismatch" << endl; return false; - if (model_ && f.model_ && !model_->equals(*f.model_, tol)) + } + if (model_ && f.model_ && !model_->equals(*f.model_, tol)) { + if (verbose) + cout << "JacobianFactor::equals: noise modesl are not equal" << endl; return false; + } // Check matrix sizes - if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) + if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) { + if (verbose) + cout << "JacobianFactor::equals: augmented size mismatch" << endl; return false; + } // Check matrix contents constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); for (size_t row = 0; row < (size_t) Ab1.rows(); ++row) if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) - && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) + && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) { + if (verbose) + cout << "JacobianFactor::equals: matrix mismatch at row " << row << endl; return false; + } return true; } @@ -414,8 +432,6 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const { /* ************************************************************************* */ double JacobianFactor::error(const VectorValues& c) const { - if (empty()) - return 0; Vector weighted = error_vector(c); return 0.5 * weighted.dot(weighted); } @@ -479,6 +495,43 @@ map JacobianFactor::hessianBlockDiagonal() const { return blocks; } +/* ************************************************************************* */ +void JacobianFactor::updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_JacobianFactor); + + if (rows() == 0) return; + + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw invalid_argument( + "JacobianFactor::updateHessian: cannot update information with " + "constrained noise model"); + JacobianFactor whitenedFactor = whiten(); + whitenedFactor.updateHessian(infoKeys, info); + } else { + // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below + DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1; + + // Apply updates to the upper triangle + // Loop over blocks of A, including RHS with j==n + vector slots(n+1); + for (DenseIndex j = 0; j <= n; ++j) { + const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); + slots[j] = J; + // Fill off-diagonal blocks with Ai'*Aj + for (DenseIndex i = 0; i < j; ++i) { + const DenseIndex I = slots[i]; // because i& accumulatedDims) const { + + /// Use Eigen magic to access raw memory + typedef Eigen::Map VectorMap; + typedef Eigen::Map ConstVectorMap; + + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + /// Just iterate over all A matrices and multiply in correct config part (looping over keys) + /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + Ax += Ab_(pos) * ConstVectorMap(x + offset, dim); + } + /// Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + /// multiply with alpha + Ax *= alpha; + + /// Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax; + } +} + /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; @@ -629,8 +727,8 @@ std::pair, jointFactor->Ab_.matrix().triangularView().setZero(); // Split elimination result into conditional and remaining factor - GaussianConditional::shared_ptr conditional = jointFactor->splitConditional( - keys.size()); + GaussianConditional::shared_ptr conditional = // + jointFactor->splitConditional(keys.size()); return make_pair(conditional, jointFactor); } @@ -659,11 +757,11 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional( } GaussianConditional::shared_ptr conditional = boost::make_shared< GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); - const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) + const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim; const DenseIndex remainingRows = - model_ ? - std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) : + model_ ? std::min(model_->sigmas().size() - frontalDim, + maxRemainingRows) : maxRemainingRows; Ab_.rowStart() += frontalDim; Ab_.rowEnd() = Ab_.rowStart() + remainingRows; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 02ae21566..ea9958474 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -273,6 +273,13 @@ namespace gtsam { /** Get a view of the A matrix */ ABlock getA() { return Ab_.range(0, size()); } + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; + /** Return A*x */ Vector operator*(const VectorValues& x) const; @@ -283,6 +290,17 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; + /** + * Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * Requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! + * TODO(frank): we should probably kill this if no longer needed + */ + void multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const; + /// A'*b for Jacobian VectorValues gradientAtZero() const; @@ -349,7 +367,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(Ab_); ar & BOOST_SERIALIZATION_NVP(model_); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index f9a9ca804..609c03acf 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -29,6 +29,7 @@ #include #include #include +#include using namespace std; @@ -71,6 +72,11 @@ boost::optional checkIfDiagonal(const Matrix M) { } } +/* ************************************************************************* */ +Vector Base::sigmas() const { + throw("Base::sigmas: sigmas() not implemented for this noise model"); +} + /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { size_t m = R.rows(), n = R.cols(); @@ -80,24 +86,25 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { if (smart) diagonal = checkIfDiagonal(R); if (diagonal) - return Diagonal::Sigmas(reciprocal(*diagonal), true); + return Diagonal::Sigmas(diagonal->array().inverse(), true); else return shared_ptr(new Gaussian(R.rows(), R)); } /* ************************************************************************* */ -Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) { - size_t m = M.rows(), n = M.cols(); +Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart) { + size_t m = information.rows(), n = information.cols(); if (m != n) throw invalid_argument("Gaussian::Information: R not square"); boost::optional diagonal = boost::none; if (smart) - diagonal = checkIfDiagonal(M); + diagonal = checkIfDiagonal(information); if (diagonal) return Diagonal::Precisions(*diagonal, true); else { - Matrix R = RtR(M); - return shared_ptr(new Gaussian(R.rows(), R)); + Eigen::LLT llt(information); + Matrix R = llt.matrixU(); + return shared_ptr(new Gaussian(n, R)); } } @@ -112,13 +119,15 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, variances = checkIfDiagonal(covariance); if (variances) return Diagonal::Variances(*variances, true); - else - return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); + else { + // TODO: can we do this more efficiently and still get an upper triangular nmatrix?? + return Information(covariance.inverse(), false); + } } /* ************************************************************************* */ void Gaussian::print(const string& name) const { - gtsam::print(thisR(), "Gaussian"); + gtsam::print(thisR(), name + "Gaussian"); } /* ************************************************************************* */ @@ -130,6 +139,12 @@ bool Gaussian::equals(const Base& expected, double tol) const { return equal_with_abs_tol(R(), p->R(), sqrt(tol)); } +/* ************************************************************************* */ +Vector Gaussian::sigmas() const { + // TODO(frank): can this be done faster? + return Vector((thisR().transpose() * thisR()).inverse().diagonal()).cwiseSqrt(); +} + /* ************************************************************************* */ Vector Gaussian::whiten(const Vector& v) const { return thisR() * v; @@ -222,9 +237,11 @@ Diagonal::Diagonal() : } /* ************************************************************************* */ -Diagonal::Diagonal(const Vector& sigmas) : - Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_( - emul(invsigmas_, invsigmas_)) { +Diagonal::Diagonal(const Vector& sigmas) + : Gaussian(sigmas.size()), + sigmas_(sigmas), + invsigmas_(sigmas.array().inverse()), + precisions_(invsigmas_.array().square()) { } /* ************************************************************************* */ @@ -263,12 +280,12 @@ void Diagonal::print(const string& name) const { /* ************************************************************************* */ Vector Diagonal::whiten(const Vector& v) const { - return emul(v, invsigmas()); + return v.cwiseProduct(invsigmas_); } /* ************************************************************************* */ Vector Diagonal::unwhiten(const Vector& v) const { - return emul(v, sigmas_); + return v.cwiseProduct(sigmas_); } /* ************************************************************************* */ @@ -294,7 +311,7 @@ namespace internal { // switch precisions and invsigmas to finite value // TODO: why?? And, why not just ask s==0.0 below ? static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { - for (size_t i = 0; i < sigmas.size(); ++i) + for (Vector::Index i = 0; i < sigmas.size(); ++i) if (!std::isfinite(1. / sigmas[i])) { precisions[i] = 0.0; invsigmas[i] = 0.0; @@ -343,7 +360,7 @@ Vector Constrained::whiten(const Vector& v) const { assert (b.size()==a.size()); Vector c(n); for( size_t i = 0; i < n; i++ ) { - const double& ai = a(i), &bi = b(i); + const double& ai = a(i), bi = b(i); c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_() } return c; @@ -360,8 +377,11 @@ double Constrained::distance(const Vector& v) const { /* ************************************************************************* */ Matrix Constrained::Whiten(const Matrix& H) const { - // selective scaling - return vector_scale(invsigmas(), H, true); + Matrix A = H; + for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) + if (!constrained(i)) // if constrained, leave row of A as is + A.row(i) *= invsigmas_(i); + return A; } /* ************************************************************************* */ @@ -405,8 +425,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { list Rd; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector invsigmas = reciprocal(sigmas_); - Vector weights = emul(invsigmas,invsigmas); // calculate weights once + Vector invsigmas = sigmas_.array().inverse(); + Vector weights = invsigmas.array().square(); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding @@ -548,16 +568,6 @@ Vector Base::weight(const Vector &error) const { return w; } -/** square root version of the weight function */ -Vector Base::sqrtWeight(const Vector &error) const { - const size_t n = error.rows(); - Vector w(n); - for ( size_t i = 0 ; i < n ; ++i ) - w(i) = sqrtWeight(error(i)); - return w; -} - - /** The following three functions reweight block matrices and a vector * according to their weight implementation */ @@ -566,8 +576,7 @@ void Base::reweight(Vector& error) const { const double w = sqrtWeight(error.norm()); error *= w; } else { - const Vector w = sqrtWeight(error); - error.array() *= w.array(); + error.array() *= weight(error).cwiseSqrt().array(); } } @@ -585,7 +594,7 @@ void Base::reweight(vector &A, Vector &error) const { BOOST_FOREACH(Matrix& Aj, A) { vector_scale_inplace(W,Aj); } - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -599,7 +608,7 @@ void Base::reweight(Matrix &A, Vector &error) const { else { const Vector W = sqrtWeight(error); vector_scale_inplace(W,A); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -615,7 +624,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { const Vector W = sqrtWeight(error); vector_scale_inplace(W,A1); vector_scale_inplace(W,A2); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -633,7 +642,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { vector_scale_inplace(W,A1); vector_scale_inplace(W,A2); vector_scale_inplace(W,A3); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -647,7 +656,7 @@ void Null::print(const std::string &s="") const Null::shared_ptr Null::Create() { return shared_ptr(new Null()); } -Fair::Fair(const double c, const ReweightScheme reweight) +Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { if ( c_ <= 0 ) { cout << "mEstimator Fair takes only positive double in constructor. forced to 1.0" << endl; @@ -659,26 +668,26 @@ Fair::Fair(const double c, const ReweightScheme reweight) // Fair /* ************************************************************************* */ -double Fair::weight(const double &error) const +double Fair::weight(double error) const { return 1.0 / (1.0 + fabs(error)/c_); } void Fair::print(const std::string &s="") const { cout << s << "fair (" << c_ << ")" << endl; } -bool Fair::equals(const Base &expected, const double tol) const { +bool Fair::equals(const Base &expected, double tol) const { const Fair* p = dynamic_cast (&expected); if (p == NULL) return false; return fabs(c_ - p->c_ ) < tol; } -Fair::shared_ptr Fair::Create(const double c, const ReweightScheme reweight) +Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Fair(c, reweight)); } /* ************************************************************************* */ // Huber /* ************************************************************************* */ -Huber::Huber(const double k, const ReweightScheme reweight) +Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { if ( k_ <= 0 ) { cout << "mEstimator Huber takes only positive double in constructor. forced to 1.0" << endl; @@ -686,7 +695,7 @@ Huber::Huber(const double k, const ReweightScheme reweight) } } -double Huber::weight(const double &error) const { +double Huber::weight(double error) const { return (error < k_) ? (1.0) : (k_ / fabs(error)); } @@ -694,13 +703,13 @@ void Huber::print(const std::string &s="") const { cout << s << "huber (" << k_ << ")" << endl; } -bool Huber::equals(const Base &expected, const double tol) const { +bool Huber::equals(const Base &expected, double tol) const { const Huber* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(k_ - p->k_) < tol; } -Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { +Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Huber(c, reweight)); } @@ -708,7 +717,7 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { // Cauchy /* ************************************************************************* */ -Cauchy::Cauchy(const double k, const ReweightScheme reweight) +Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { if ( k_ <= 0 ) { cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl; @@ -716,7 +725,7 @@ Cauchy::Cauchy(const double k, const ReweightScheme reweight) } } -double Cauchy::weight(const double &error) const { +double Cauchy::weight(double error) const { return k_*k_ / (k_*k_ + error*error); } @@ -724,24 +733,24 @@ void Cauchy::print(const std::string &s="") const { cout << s << "cauchy (" << k_ << ")" << endl; } -bool Cauchy::equals(const Base &expected, const double tol) const { +bool Cauchy::equals(const Base &expected, double tol) const { const Cauchy* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(k_ - p->k_) < tol; } -Cauchy::shared_ptr Cauchy::Create(const double c, const ReweightScheme reweight) { +Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Cauchy(c, reweight)); } /* ************************************************************************* */ // Tukey /* ************************************************************************* */ -Tukey::Tukey(const double c, const ReweightScheme reweight) +Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double Tukey::weight(const double &error) const { +double Tukey::weight(double error) const { if (fabs(error) <= c_) { double xc2 = (error/c_)*(error/c_); double one_xc22 = (1.0-xc2)*(1.0-xc2); @@ -754,24 +763,24 @@ void Tukey::print(const std::string &s="") const { std::cout << s << ": Tukey (" << c_ << ")" << std::endl; } -bool Tukey::equals(const Base &expected, const double tol) const { +bool Tukey::equals(const Base &expected, double tol) const { const Tukey* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(c_ - p->c_) < tol; } -Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) { +Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Tukey(c, reweight)); } /* ************************************************************************* */ // Welsh /* ************************************************************************* */ -Welsh::Welsh(const double c, const ReweightScheme reweight) +Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double Welsh::weight(const double &error) const { +double Welsh::weight(double error) const { double xc2 = (error/c_)*(error/c_); return std::exp(-xc2); } @@ -780,13 +789,13 @@ void Welsh::print(const std::string &s="") const { std::cout << s << ": Welsh (" << c_ << ")" << std::endl; } -bool Welsh::equals(const Base &expected, const double tol) const { +bool Welsh::equals(const Base &expected, double tol) const { const Welsh* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(c_ - p->c_) < tol; } -Welsh::shared_ptr Welsh::Create(const double c, const ReweightScheme reweight) { +Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsh(c, reweight)); } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 94012c7c2..f2f8a01cf 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -26,7 +26,6 @@ #include #include #include -#include namespace gtsam { @@ -59,14 +58,15 @@ namespace gtsam { public: - /** primary constructor @param dim is the dimension of the model */ + /// primary constructor @param dim is the dimension of the model Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} - /// true if a constrained noise mode, saves slow/clumsy dynamic casting - virtual bool isConstrained() const { - return false; // default false - } + /// true if a constrained noise model, saves slow/clumsy dynamic casting + virtual bool isConstrained() const { return false; } // default false + + /// true if a unit noise model, saves slow/clumsy dynamic casting + virtual bool isUnit() const { return false; } // default false /// Dimensionality inline size_t dim() const { return dim_;} @@ -75,14 +75,16 @@ namespace gtsam { virtual bool equals(const Base& expected, double tol=1e-9) const = 0; - /** - * Whiten an error vector. - */ + /// Calculate standard deviations + virtual Vector sigmas() const; + + /// Whiten an error vector. virtual Vector whiten(const Vector& v) const = 0; - /** - * Unwhiten an error vector. - */ + /// Whiten a matrix. + virtual Matrix Whiten(const Matrix& H) const = 0; + + /// Unwhiten an error vector. virtual Vector unwhiten(const Vector& v) const = 0; virtual double distance(const Vector& v) const = 0; @@ -116,7 +118,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(dim_); } }; @@ -189,6 +191,7 @@ namespace gtsam { virtual void print(const std::string& name) const; virtual bool equals(const Base& expected, double tol=1e-9) const; + virtual Vector sigmas() const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; @@ -220,9 +223,9 @@ namespace gtsam { /** * Whiten a system, in place as well */ - virtual void WhitenSystem(std::vector& A, Vector& b) const ; - virtual void WhitenSystem(Matrix& A, Vector& b) const ; - virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; + virtual void WhitenSystem(std::vector& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; /** @@ -234,16 +237,20 @@ namespace gtsam { */ virtual boost::shared_ptr QR(Matrix& Ab) const; - /** - * Return R itself, but note that Whiten(H) is cheaper than R*H - */ + /// Return R itself, but note that Whiten(H) is cheaper than R*H virtual Matrix R() const { return thisR();} + /// Compute information matrix + virtual Matrix information() const { return thisR().transpose() * thisR(); } + + /// Compute covariance matrix + virtual Matrix covariance() const { return information().inverse(); } + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(sqrt_information_); } @@ -303,6 +310,7 @@ namespace gtsam { } virtual void print(const std::string& name) const; + virtual Vector sigmas() const { return sigmas_; } virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; @@ -312,7 +320,6 @@ namespace gtsam { /** * Return standard deviations (sqrt of diagonal) */ - inline const Vector& sigmas() const { return sigmas_; } inline double sigma(size_t i) const { return sigmas_(i); } /** @@ -338,7 +345,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian); ar & BOOST_SERIALIZATION_NVP(sigmas_); ar & BOOST_SERIALIZATION_NVP(invsigmas_); @@ -387,9 +394,7 @@ namespace gtsam { virtual ~Constrained() {} /// true if a constrained noise mode, saves slow/clumsy dynamic casting - virtual bool isConstrained() const { - return true; - } + virtual bool isConstrained() const { return true; } /// Return true if a particular dimension is free or constrained bool constrained(size_t i) const; @@ -489,7 +494,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal); ar & BOOST_SERIALIZATION_NVP(mu_); } @@ -557,7 +562,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal); ar & BOOST_SERIALIZATION_NVP(sigma_); ar & BOOST_SERIALIZATION_NVP(invsigma_); @@ -588,23 +593,26 @@ namespace gtsam { return shared_ptr(new Unit(dim)); } + /// true if a unit noise model, saves slow/clumsy dynamic casting + virtual bool isUnit() const { return true; } + virtual void print(const std::string& name) const; virtual double Mahalanobis(const Vector& v) const {return v.dot(v); } virtual Vector whiten(const Vector& v) const { return v; } virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } - virtual void WhitenInPlace(Matrix& H) const {} - virtual void WhitenInPlace(Eigen::Block H) const {} - virtual void whitenInPlace(Vector& v) const {} - virtual void unwhitenInPlace(Vector& v) const {} - virtual void whitenInPlace(Eigen::Block& v) const {} - virtual void unwhitenInPlace(Eigen::Block& v) const {} + virtual void WhitenInPlace(Matrix& /*H*/) const {} + virtual void WhitenInPlace(Eigen::Block /*H*/) const {} + virtual void whitenInPlace(Vector& /*v*/) const {} + virtual void unwhitenInPlace(Vector& /*v*/) const {} + virtual void whitenInPlace(Eigen::Block& /*v*/) const {} + virtual void unwhitenInPlace(Eigen::Block& /*v*/) const {} private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic); } }; @@ -630,20 +638,23 @@ namespace gtsam { virtual ~Base() {} /// robust error function to implement - virtual double weight(const double &error) const = 0; + virtual double weight(double error) const = 0; virtual void print(const std::string &s) const = 0; - virtual bool equals(const Base& expected, const double tol=1e-8) const = 0; + virtual bool equals(const Base& expected, double tol=1e-8) const = 0; - inline double sqrtWeight(const double &error) const - { return std::sqrt(weight(error)); } + double sqrtWeight(double error) const { + return std::sqrt(weight(error)); + } /** produce a weight vector according to an error vector and the implemented * robust function */ Vector weight(const Vector &error) const; /** square root version of the weight function */ - Vector sqrtWeight(const Vector &error) const; + Vector sqrtWeight(const Vector &error) const { + return weight(error).cwiseSqrt(); + } /** reweight block matrices and a vector according to their weight implementation */ void reweight(Vector &error) const; @@ -656,7 +667,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(reweight_); } }; @@ -668,16 +679,16 @@ namespace gtsam { Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} - virtual double weight(const double &error) const { return 1.0; } + virtual double weight(double /*error*/) const { return 1.0; } virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, const double tol=1e-8) const { return true; } + virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; @@ -687,12 +698,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Fair(const double c = 1.3998, const ReweightScheme reweight = Block); + Fair(double c = 1.3998, const ReweightScheme reweight = Block); virtual ~Fair() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double c, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; protected: double c_; @@ -701,7 +712,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(c_); } @@ -713,11 +724,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; virtual ~Huber() {} - Huber(const double k = 1.345, const ReweightScheme reweight = Block); - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + Huber(double k = 1.345, const ReweightScheme reweight = Block); + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double k_; @@ -726,7 +737,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(k_); } @@ -742,11 +753,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; virtual ~Cauchy() {} - Cauchy(const double k = 0.1, const ReweightScheme reweight = Block); - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + Cauchy(double k = 0.1, const ReweightScheme reweight = Block); + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double k_; @@ -755,7 +766,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(k_); } @@ -766,12 +777,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Tukey(const double c = 4.6851, const ReweightScheme reweight = Block); + Tukey(double c = 4.6851, const ReweightScheme reweight = Block); virtual ~Tukey() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double c_; @@ -780,7 +791,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(c_); } @@ -791,12 +802,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Welsh(const double c = 2.9846, const ReweightScheme reweight = Block); + Welsh(double c = 2.9846, const ReweightScheme reweight = Block); virtual ~Welsh() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double c_; @@ -805,7 +816,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(c_); } @@ -849,7 +860,9 @@ namespace gtsam { // TODO: functions below are dummy but necessary for the noiseModel::Base inline virtual Vector whiten(const Vector& v) const { Vector r = v; this->WhitenSystem(r); return r; } - inline virtual Vector unwhiten(const Vector& v) const + inline virtual Matrix Whiten(const Matrix& A) const + { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } + inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } inline virtual double distance(const Vector& v) const { return this->whiten(v).squaredNorm(); } @@ -868,7 +881,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & boost::serialization::make_nvp("robust_", const_cast(robust_)); ar & boost::serialization::make_nvp("noise_", const_cast(noise_)); diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h index 38e1407f0..f954cba88 100644 --- a/gtsam/linear/RegularJacobianFactor.h +++ b/gtsam/linear/RegularJacobianFactor.h @@ -68,6 +68,8 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } + using JacobianFactor::multiplyHessianAdd; + /** y += alpha * A'*A*x */ virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { @@ -182,50 +184,6 @@ public: return model_ ? model_->whiten(Ax) : Ax; } - /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x - * Note: this is not assuming a fixed dimension for the variables, - * but requires the vector accumulatedDims to tell the dimension of - * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, - * then accumulatedDims is [0 3 9 11 13] - * NOTE: size of accumulatedDims is size of keys + 1!! - * TODO Frank asks: why is this here if not regular ???? - */ - void multiplyHessianAdd(double alpha, const double* x, double* y, - const std::vector& accumulatedDims) const { - - /// Use Eigen magic to access raw memory - typedef Eigen::Map VectorMap; - typedef Eigen::Map ConstVectorMap; - - if (empty()) - return; - Vector Ax = zero(Ab_.rows()); - - /// Just iterate over all A matrices and multiply in correct config part (looping over keys) - /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' - /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) - for (size_t pos = 0; pos < size(); ++pos) { - size_t offset = accumulatedDims[keys_[pos]]; - size_t dim = accumulatedDims[keys_[pos] + 1] - offset; - Ax += Ab_(pos) * ConstVectorMap(x + offset, dim); - } - /// Deal with noise properly, need to Double* whiten as we are dividing by variance - if (model_) { - model_->whitenInPlace(Ax); - model_->whitenInPlace(Ax); - } - - /// multiply with alpha - Ax *= alpha; - - /// Again iterate over all A matrices and insert Ai^T into y - for (size_t pos = 0; pos < size(); ++pos) { - size_t offset = accumulatedDims[keys_[pos]]; - size_t dim = accumulatedDims[keys_[pos] + 1] - offset; - VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax; - } - } - }; // end class RegularJacobianFactor diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp new file mode 100644 index 000000000..942b42160 --- /dev/null +++ b/gtsam/linear/Scatter.cpp @@ -0,0 +1,81 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Scatter.cpp + * @author Richard Roberts + * @author Frank Dellaert + * @date June 2015 + */ + +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +string SlotEntry::toString() const { + ostringstream oss; + oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; + return oss.str(); +} + +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering) { + gttic(Scatter_Constructor); + static const DenseIndex none = std::numeric_limits::max(); + + // First do the set union. + BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { + if (factor) { + for (GaussianFactor::const_iterator variable = factor->begin(); + variable != factor->end(); ++variable) { + // TODO: Fix this hack to cope with zero-row Jacobians that come from + // BayesTreeOrphanWrappers + const JacobianFactor* asJacobian = + dynamic_cast(factor.get()); + if (!asJacobian || asJacobian->cols() > 1) + insert( + make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); + } + } + } + + // If we have an ordering, pre-fill the ordered variables first + size_t slot = 0; + if (ordering) { + BOOST_FOREACH (Key key, *ordering) { + const_iterator entry = find(key); + if (entry == end()) + throw std::invalid_argument( + "The ordering provided to the HessianFactor Scatter constructor\n" + "contained extra variables that did not appear in the factors to " + "combine."); + at(key).slot = (slot++); + } + } + + // Next fill in the slot indices (we can only get these after doing the set + // union. + BOOST_FOREACH (value_type& var_slot, *this) { + if (var_slot.second.slot == none) var_slot.second.slot = (slot++); + } +} + +/* ************************************************************************* */ + +} // gtsam diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h new file mode 100644 index 000000000..7a37ba1e0 --- /dev/null +++ b/gtsam/linear/Scatter.h @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * 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 Scatter.h + * @brief Maps global variable indices to slot indices + * @author Richard Roberts + * @author Frank Dellaert + * @date June 2015 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +class GaussianFactorGraph; +class Ordering; + +/// One SlotEntry stores the slot index for a variable, as well its dim. +struct GTSAM_EXPORT SlotEntry { + DenseIndex slot; + size_t dimension; + SlotEntry(DenseIndex _slot, size_t _dimension) + : slot(_slot), dimension(_dimension) {} + std::string toString() const; +}; + +/** + * Scatter is an intermediate data structure used when building a HessianFactor + * incrementally, to get the keys in the right order. The "scatter" is a map + * from global variable indices to slot indices in the union of involved + * variables. We also include the dimensionality of the variable. + */ +class Scatter : public FastMap { + public: + /// Constructor + Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering = boost::none); + + /// Get the slot corresponding to the given key + DenseIndex slot(Key key) const { return at(key).slot; } + + /// Get the dimension corresponding to the given key + DenseIndex dim(Key key) const { return at(key).dimension; } +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 86bee2188..350963bcf 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -44,7 +44,7 @@ namespace gtsam { private: friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(index_); ar & BOOST_SERIALIZATION_NVP(weight_); } @@ -70,7 +70,7 @@ namespace gtsam { Subgraph(const std::vector &indices) ; inline const Edges& edges() const { return edges_; } - inline const size_t size() const { return edges_.size(); } + inline size_t size() const { return edges_.size(); } EdgeIndices edgeIndices() const; iterator begin() { return edges_.begin(); } @@ -85,7 +85,7 @@ namespace gtsam { private: friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(edges_); } }; diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index ce33116ab..968fc1adb 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -376,7 +376,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(values_); } }; // VectorValues definition diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 3a2cd0fd4..10fb34988 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testCholeskyFactor.cpp + * @file testHessianFactor.cpp * @author Richard Roberts * @date Dec 15, 2010 */ @@ -38,6 +38,16 @@ using namespace gtsam; const double tol = 1e-5; +/* ************************************************************************* */ +TEST(HessianFactor, Slot) +{ + FastVector keys = list_of(2)(4)(1); + EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2)); + EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4)); + EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1)); + EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist +} + /* ************************************************************************* */ TEST(HessianFactor, emptyConstructor) { @@ -270,30 +280,69 @@ TEST(HessianFactor, ConstructorNWay) } /* ************************************************************************* */ -TEST(HessianFactor, CombineAndEliminate) -{ - Matrix A01 = (Matrix(3,3) << - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0).finished(); +TEST(HessianFactor, CombineAndEliminate1) { + Matrix3 A01 = 3.0 * I_3x3; + Vector3 b0(1, 0, 0); + + Matrix3 A21 = 4.0 * I_3x3; + Vector3 b2 = Vector3::Zero(); + + GaussianFactorGraph gfg; + gfg.add(1, A01, b0); + gfg.add(1, A21, b2); + + Matrix63 A1; + A1 << A01, A21; + Vector6 b; + b << b0, b2; + + // create a full, uneliminated version of the factor + JacobianFactor jacobian(1, A1, b); + + // Make sure combining works + HessianFactor hessian(gfg); + VectorValues v; + v.insert(1, Vector3(1, 0, 0)); + EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9); + EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6)); + EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9)); + EXPECT( + assert_equal(jacobian.augmentedInformation(), + hessian.augmentedInformation(), 1e-9)); + + // perform elimination on jacobian + Ordering ordering = list_of(1); + GaussianConditional::shared_ptr expectedConditional; + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + jacobian.eliminate(ordering); + + // Eliminate + GaussianConditional::shared_ptr actualConditional; + HessianFactor::shared_ptr actualHessian; + boost::tie(actualConditional, actualHessian) = // + EliminateCholesky(gfg, ordering); + + EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); + EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); + EXPECT( + assert_equal(expectedFactor->augmentedInformation(), + actualHessian->augmentedInformation(), 1e-9)); + EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); +} + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate2) { + Matrix A01 = I_3x3; Vector3 b0(1.5, 1.5, 1.5); Vector3 s0(1.6, 1.6, 1.6); - Matrix A10 = (Matrix(3,3) << - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0).finished(); - Matrix A11 = (Matrix(3,3) << - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0).finished(); + Matrix A10 = 2.0 * I_3x3; + Matrix A11 = -2.0 * I_3x3; Vector3 b1(2.5, 2.5, 2.5); Vector3 s1(2.6, 2.6, 2.6); - Matrix A21 = (Matrix(3,3) << - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0).finished(); + Matrix A21 = 3.0 * I_3x3; Vector3 b2(3.5, 3.5, 3.5); Vector3 s2(3.6, 3.6, 3.6); @@ -302,27 +351,45 @@ TEST(HessianFactor, CombineAndEliminate) gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector9 b; b << b1, b0, b2; - Vector9 sigmas; sigmas << s1, s0, s2; + Matrix93 A0, A1; + A0 << A10, Z_3x3, Z_3x3; + A1 << A11, A01, A21; + Vector9 b, sigmas; + b << b1, b0, b2; + sigmas << s1, s0, s2; // create a full, uneliminated version of the factor - JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + JacobianFactor jacobian(0, A0, 1, A1, b, + noiseModel::Diagonal::Sigmas(sigmas, true)); + + // Make sure combining works + HessianFactor hessian(gfg); + EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6)); + EXPECT( + assert_equal(jacobian.augmentedInformation(), + hessian.augmentedInformation(), 1e-9)); // perform elimination on jacobian + Ordering ordering = list_of(0); GaussianConditional::shared_ptr expectedConditional; - JacobianFactor::shared_ptr expectedRemainingFactor; - boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0))); + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + jacobian.eliminate(ordering); // Eliminate GaussianConditional::shared_ptr actualConditional; - HessianFactor::shared_ptr actualCholeskyFactor; - boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0))); + HessianFactor::shared_ptr actualHessian; + boost::tie(actualConditional, actualHessian) = // + EliminateCholesky(gfg, ordering); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); - EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6)); + VectorValues v; + v.insert(1, Vector3(1, 2, 3)); + EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); + EXPECT( + assert_equal(expectedFactor->augmentedInformation(), + actualHessian->augmentedInformation(), 1e-9)); + EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 7b2d59171..17ceaf5f0 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -540,9 +540,9 @@ TEST(JacobianFactor, EliminateQR) EXPECT(assert_equal(size_t(2), actualJF.keys().size())); EXPECT(assert_equal(Key(9), actualJF.keys()[0])); EXPECT(assert_equal(Key(11), actualJF.keys()[1])); - EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001)); - EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); - EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); + EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001)); EXPECT(!actualJF.get_model()); } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 5c6b2729d..b2afb1709 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -33,15 +33,16 @@ using namespace gtsam; using namespace noiseModel; using namespace boost::assign; -static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; -static Matrix R = (Matrix(3, 3) << +static const double kSigma = 2, s_1=1.0/kSigma, kVariance = kSigma*kSigma, prc = 1.0/kVariance; +static const Matrix R = (Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, 0.0, 0.0, s_1).finished(); -static Matrix Sigma = (Matrix(3, 3) << - var, 0.0, 0.0, - 0.0, var, 0.0, - 0.0, 0.0, var).finished(); +static const Matrix kCovariance = (Matrix(3, 3) << + kVariance, 0.0, 0.0, + 0.0, kVariance, 0.0, + 0.0, 0.0, kVariance).finished(); +static const Vector3 kSigmas(kSigma, kSigma, kSigma); //static double inf = numeric_limits::infinity(); @@ -53,15 +54,19 @@ TEST(NoiseModel, constructors) // Construct noise models vector m; - m.push_back(Gaussian::SqrtInformation(R)); - m.push_back(Gaussian::Covariance(Sigma)); - //m.push_back(Gaussian::Information(Q)); - m.push_back(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished())); - m.push_back(Diagonal::Variances((Vector(3) << var, var, var).finished())); - m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished())); - m.push_back(Isotropic::Sigma(3, sigma)); - m.push_back(Isotropic::Variance(3, var)); - m.push_back(Isotropic::Precision(3, prc)); + m.push_back(Gaussian::SqrtInformation(R,false)); + m.push_back(Gaussian::Covariance(kCovariance,false)); + m.push_back(Gaussian::Information(kCovariance.inverse(),false)); + m.push_back(Diagonal::Sigmas(kSigmas,false)); + m.push_back(Diagonal::Variances((Vector(3) << kVariance, kVariance, kVariance).finished(),false)); + m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished(),false)); + m.push_back(Isotropic::Sigma(3, kSigma,false)); + m.push_back(Isotropic::Variance(3, kVariance,false)); + m.push_back(Isotropic::Precision(3, prc,false)); + + // test kSigmas + BOOST_FOREACH(Gaussian::shared_ptr mi, m) + EXPECT(assert_equal(kSigmas,mi->sigmas())); // test whiten BOOST_FOREACH(Gaussian::shared_ptr mi, m) @@ -117,9 +122,9 @@ TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); - Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()), + Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << kSigma, kSigma, kSigma).finished()), d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); - Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), + Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma), i2 = Isotropic::Sigma(3, 0.7); EXPECT(assert_equal(*g1,*g1)); @@ -155,7 +160,7 @@ TEST(NoiseModel, ConstrainedConstructors ) Constrained::shared_ptr actual; size_t d = 3; double m = 100.0; - Vector sigmas = (Vector(3) << sigma, 0.0, 0.0).finished(); + Vector sigmas = (Vector(3) << kSigma, 0.0, 0.0).finished(); Vector mu = Vector3(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? @@ -182,7 +187,7 @@ TEST(NoiseModel, ConstrainedMixed ) { Vector feasible = Vector3(1.0, 0.0, 1.0), infeasible = Vector3(1.0, 1.0, 1.0); - Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma).finished()); + Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << kSigma, 0.0, kSigma).finished()); // NOTE: we catch constrained variables elsewhere, so whitening does nothing EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible))); EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible))); @@ -357,6 +362,44 @@ TEST(NoiseModel, robustNoise) DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8); } +/* ************************************************************************* */ +#define TEST_GAUSSIAN(gaussian)\ + EQUALITY(info, gaussian->information());\ + EQUALITY(cov, gaussian->covariance());\ + EXPECT(assert_equal(white, gaussian->whiten(e)));\ + EXPECT(assert_equal(e, gaussian->unwhiten(white)));\ + EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\ + Matrix A = R.inverse(); Vector b = e;\ + gaussian->WhitenSystem(A, b);\ + EXPECT(assert_equal(I, A));\ + EXPECT(assert_equal(white, b)); + +TEST(NoiseModel, NonDiagonalGaussian) +{ + Matrix3 R; + R << 6, 5, 4, 0, 3, 2, 0, 0, 1; + const Matrix3 info = R.transpose() * R; + const Matrix3 cov = info.inverse(); + const Vector3 e(1, 1, 1), white = R * e; + Matrix I = Matrix3::Identity(); + + + { + SharedGaussian gaussian = Gaussian::SqrtInformation(R); + TEST_GAUSSIAN(gaussian); + } + + { + SharedGaussian gaussian = Gaussian::Information(info); + TEST_GAUSSIAN(gaussian); + } + + { + SharedGaussian gaussian = Gaussian::Covariance(cov); + TEST_GAUSSIAN(gaussian); + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testScatter.cpp b/gtsam/linear/tests/testScatter.cpp new file mode 100644 index 000000000..19d099616 --- /dev/null +++ b/gtsam/linear/tests/testScatter.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 testScatter.cpp + * @author Frank Dellaert + * @date June, 2015 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate) { + static const size_t m = 3, n = 3; + Matrix A01 = + (Matrix(m, n) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(); + Vector3 b0(1.5, 1.5, 1.5); + Vector3 s0(1.6, 1.6, 1.6); + + Matrix A10 = + (Matrix(m, n) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0).finished(); + Matrix A11 = (Matrix(m, n) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0) + .finished(); + Vector3 b1(2.5, 2.5, 2.5); + Vector3 s1(2.6, 2.6, 2.6); + + Matrix A21 = + (Matrix(m, n) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished(); + Vector3 b2(3.5, 3.5, 3.5); + Vector3 s2(3.6, 3.6, 3.6); + + GaussianFactorGraph gfg; + gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + Scatter scatter(gfg); + EXPECT_LONGS_EQUAL(2, scatter.size()); + EXPECT_LONGS_EQUAL(0, scatter.at(0).slot); + EXPECT_LONGS_EQUAL(1, scatter.at(1).slot); + EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension); + EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 42bcb8027..fbf7d51a1 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -97,7 +97,7 @@ public: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); } @@ -179,7 +179,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 08b75c00d..b61a861d6 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -122,7 +122,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -206,7 +206,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1c22bab9a..3547719ac 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -33,19 +33,20 @@ using namespace std; //------------------------------------------------------------------------------ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, - const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration), - biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), - biasAccOmegaInit_(biasAccOmegaInit) -{ + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, + const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, + const bool use2ndOrderIntegration) : + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_( + biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_( + biasAccOmegaInit) { preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ +void CombinedImuFactor::CombinedPreintegratedMeasurements::print( + const string& s) const { PreintegrationBase::print(s); cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; @@ -54,31 +55,35 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s } //------------------------------------------------------------------------------ -bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ - return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol) - && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol) - &&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) +bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals( + const CombinedPreintegratedMeasurements& expected, double tol) const { + return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, + tol) + && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, + tol) + && equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ +void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, - double deltaT, boost::optional body_P_sensor, + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor, boost::optional F_test, boost::optional G_test) { // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + correctedAcc, correctedOmega, body_P_sensor); const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr @@ -86,7 +91,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, + deltaT); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we @@ -98,11 +104,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); // Single Jacobians to propagate covariance - Matrix3 H_vel_biasacc = - R_i * deltaT; - Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT; + Matrix3 H_vel_biasacc = -R_i * deltaT; + Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(15,15); + Matrix F(15, 15); // for documentation: // F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, // Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, @@ -110,45 +116,47 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; F.setZero(); - F.block<9,9>(0,0) = F_9x9; - F.block<6,6>(9,9) = I_6x6; - F.block<3,3>(3,9) = H_vel_biasacc; - F.block<3,3>(6,12) = H_angles_biasomega; + F.block<9, 9>(0, 0) = F_9x9; + F.block<6, 6>(9, 9) = I_6x6; + F.block<3, 3>(3, 9) = H_vel_biasacc; + F.block<3, 3>(6, 12) = H_angles_biasomega; // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Matrix G_measCov_Gt = Matrix::Zero(15,15); + Matrix G_measCov_Gt = Matrix::Zero(15, 15); // BLOCK DIAGONAL TERMS - G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance(); - G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * - (accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) * - (H_vel_biasacc.transpose()); - G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * - (gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) * - (H_angles_biasomega.transpose()); - G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_; - G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_; + G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance(); + G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc) + * (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0)) + * (H_vel_biasacc.transpose()); + G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega) + * (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3)) + * (H_angles_biasomega.transpose()); + G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_; + G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_; // OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose(); - G_measCov_Gt.block<3,3>(3,6) = block23; - G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); + Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0) + * H_angles_biasomega.transpose(); + G_measCov_Gt.block<3, 3>(3, 6) = block23; + G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; // F_test and G_test are used for testing purposes and are not needed by the factor - if(F_test){ - F_test->resize(15,15); + if (F_test) { + F_test->resize(15, 15); (*F_test) << F; } - if(G_test){ - G_test->resize(15,21); + if (G_test) { + G_test->resize(15, 21); // This is for testing & documentation ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, - Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; + (*G_test) << // + I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, // + Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, // + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; } } @@ -156,16 +164,22 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, Z_6x6) { +} //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, +CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, + Key vel_j, Key bias_i, Key bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), - _PIM_(preintegratedMeasurements) {} + Base( + noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, + vel_j, bias_i, bias_j), ImuFactorBase(gravity, omegaCoriolis, + body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -174,13 +188,11 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { } //------------------------------------------------------------------------------ -void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," +void CombinedImuFactor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "CombinedImuFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; ImuFactorBase::print(""); _PIM_.print(" preintegrated measurements:"); @@ -188,11 +200,11 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) } //------------------------------------------------------------------------------ -bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && _PIM_.equals(e->_PIM_, tol) - && ImuFactorBase::equals(*e, tol); +bool CombinedImuFactor::equals(const NonlinearFactor& expected, + double tol) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -220,39 +232,39 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, // if we need the jacobians if (H1) { H1->resize(15, 6); - H1->block < 9, 6 > (0, 0) = D_r_pose_i; + H1->block<9, 6>(0, 0) = D_r_pose_i; // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] - H1->block < 6, 6 > (9, 0) = Z_6x6; + H1->block<6, 6>(9, 0) = Z_6x6; } if (H2) { H2->resize(15, 3); - H2->block < 9, 3 > (0, 0) = D_r_vel_i; + H2->block<9, 3>(0, 0) = D_r_vel_i; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H2->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); + H2->block<6, 3>(9, 0) = Matrix::Zero(6, 3); } if (H3) { H3->resize(15, 6); - H3->block < 9, 6 > (0, 0) = D_r_pose_j; + H3->block<9, 6>(0, 0) = D_r_pose_j; // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] - H3->block < 6, 6 > (9, 0) = Z_6x6; + H3->block<6, 6>(9, 0) = Z_6x6; } if (H4) { H4->resize(15, 3); - H4->block < 9, 3 > (0, 0) = D_r_vel_j; + H4->block<9, 3>(0, 0) = D_r_vel_j; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H4->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); + H4->block<6, 3>(9, 0) = Matrix::Zero(6, 3); } if (H5) { H5->resize(15, 6); - H5->block < 9, 6 > (0, 0) = D_r_bias_i; + H5->block<9, 6>(0, 0) = D_r_bias_i; // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] - H5->block < 6, 6 > (9, 0) = Hbias_i; + H5->block<6, 6>(9, 0) = Hbias_i; } if (H6) { H6->resize(15, 6); - H6->block < 9, 6 > (0, 0) = Matrix::Zero(9, 6); + H6->block<9, 6>(0, 0) = Matrix::Zero(9, 6); // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] - H6->block < 6, 6 > (9, 0) = Hbias_j; + H6->block<6, 6>(9, 0) = Hbias_j; } // overall error diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index d2ad32122..a65a4d3f7 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -65,7 +65,8 @@ namespace gtsam { * the correlation between the bias uncertainty and the preintegrated * measurements uncertainty. */ -class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ +class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase { public: /** @@ -82,11 +83,11 @@ public: protected: - Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk + Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration + Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration - Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements + Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation ///< between the preintegrated measurements and the biases @@ -105,16 +106,20 @@ public: * @param use2ndOrderIntegration Controls the order of integration * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) */ - CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false); + const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = + false); /// print void print(const std::string& s = "Preintegrated Measurements:") const; /// equals - bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const; + bool equals(const CombinedPreintegratedMeasurements& expected, double tol = + 1e-9) const; /// Re-initialize CombinedPreintegratedMeasurements void resetIntegration(); @@ -126,19 +131,23 @@ public: * @param deltaT Time interval between two consecutive IMU measurements * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) */ - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor = boost::none, - boost::optional F_test = boost::none, boost::optional G_test = boost::none); + boost::optional F_test = boost::none, + boost::optional G_test = boost::none); /// methods to access class variables - Matrix preintMeasCov() const { return preintMeasCov_;} + Matrix preintMeasCov() const { + return preintMeasCov_; + } private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } @@ -147,7 +156,8 @@ public: private: typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; + typedef NoiseModelFactor6 Base; CombinedPreintegratedMeasurements _PIM_; @@ -177,12 +187,15 @@ public: * @param body_P_sensor Optional pose of the sensor frame in the body frame * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, + Key bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); - virtual ~CombinedImuFactor() {} + virtual ~CombinedImuFactor() { + } /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; @@ -190,54 +203,59 @@ public: /** implement functions needed for Testable */ /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** Access the preintegrated measurements. */ const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return _PIM_; } + return _PIM_; + } /** implement functions needed to derive from Factor */ /// vector of errors - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const; + boost::optional H1 = boost::none, boost::optional H2 = + boost::none, boost::optional H3 = boost::none, + boost::optional H4 = boost::none, boost::optional H5 = + boost::none, boost::optional H6 = boost::none) const; /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ - static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); - pose_j = PVB.pose; - vel_j = PVB.velocity; + PoseVelocityBias PVB( + PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected_out, + deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; } - private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; // class CombinedImuFactor +}; +// class CombinedImuFactor typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 6902f81f1..cd5fa71d2 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -103,7 +103,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index ce93bd740..571fb7249 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -17,8 +17,10 @@ #pragma once +#include +#include +#include #include -#include /* * NOTES: @@ -36,57 +38,63 @@ namespace gtsam { /// All bias models live in the imuBias namespace namespace imuBias { - class ConstantBias { - private: - Vector3 biasAcc_; - Vector3 biasGyro_; +class ConstantBias { +private: + Vector3 biasAcc_; + Vector3 biasGyro_; - public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 6; +public: + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 6; - ConstantBias(): - biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { - } + ConstantBias() : + biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { + } - ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro): + ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) : biasAcc_(biasAcc), biasGyro_(biasGyro) { - } + } - ConstantBias(const Vector6& v): + ConstantBias(const Vector6& v) : biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { + } + + /** return the accelerometer and gyro biases in a single vector */ + Vector6 vector() const { + Vector6 v; + v << biasAcc_, biasGyro_; + return v; + } + + /** get accelerometer bias */ + const Vector3& accelerometer() const { + return biasAcc_; + } + + /** get gyroscope bias */ + const Vector3& gyroscope() const { + return biasGyro_; + } + + /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ + Vector3 correctAccelerometer(const Vector3& measurement, + boost::optional H = boost::none) const { + if (H) { + H->resize(3, 6); + (*H) << -Matrix3::Identity(), Matrix3::Zero(); } + return measurement - biasAcc_; + } - /** return the accelerometer and gyro biases in a single vector */ - Vector6 vector() const { - Vector6 v; - v << biasAcc_, biasGyro_; - return v; - } - - /** get accelerometer bias */ - const Vector3& accelerometer() const { return biasAcc_; } - - /** get gyroscope bias */ - const Vector3& gyroscope() const { return biasGyro_; } - - /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ - Vector3 correctAccelerometer(const Vector3& measurement, boost::optional H=boost::none) const { - if (H) { - H->resize(3, 6); - (*H) << -Matrix3::Identity(), Matrix3::Zero(); - } - return measurement - biasAcc_; - } - - /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ - Vector3 correctGyroscope(const Vector3& measurement, boost::optional H=boost::none) const { - if (H) { - H->resize(3, 6); - (*H) << Matrix3::Zero(), -Matrix3::Identity(); - } - return measurement - biasGyro_; + /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ + Vector3 correctGyroscope(const Vector3& measurement, + boost::optional H = boost::none) const { + if (H) { + H->resize(3, 6); + (*H) << Matrix3::Zero(), -Matrix3::Identity(); } + return measurement - biasGyro_; + } // // H1: Jacobian w.r.t. IMUBias // // H2: Jacobian w.r.t. pose @@ -118,76 +126,91 @@ namespace imuBias { //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; // } - /// @} - /// @name Testable - /// @{ +/// @} +/// @name Testable +/// @{ - /// print with optional string - void print(const std::string& s = "") const { - // explicit printing for now. - std::cout << s + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl; - std::cout << s + ".biasGyro [" << biasGyro_.transpose() << "]" << std::endl; - } +/// print with optional string + void print(const std::string& s = "") const { + // explicit printing for now. + std::cout << s + ".Acc [" << biasAcc_.transpose() << "]" << std::endl; + std::cout << s + ".Gyro [" << biasGyro_.transpose() << "]" << std::endl; + } - /** equality up to tolerance */ - inline bool equals(const ConstantBias& expected, double tol=1e-5) const { - return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) - && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); - } + /** equality up to tolerance */ + inline bool equals(const ConstantBias& expected, double tol = 1e-5) const { + return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) + && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); + } - /// @} - /// @name Group - /// @{ + /// @} + /// @name Group + /// @{ - /** identity for group operation */ - static ConstantBias identity() { return ConstantBias(); } + /** identity for group operation */ + static ConstantBias identity() { + return ConstantBias(); + } - /** inverse */ - inline ConstantBias operator-() const { - return ConstantBias(-biasAcc_, -biasGyro_); - } + /** inverse */ + inline ConstantBias operator-() const { + return ConstantBias(-biasAcc_, -biasGyro_); + } - /** addition */ - ConstantBias operator+(const ConstantBias& b) const { - return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); - } + /** addition */ + ConstantBias operator+(const ConstantBias& b) const { + return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); + } - /** subtraction */ - ConstantBias operator-(const ConstantBias& b) const { - return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_); - } + /** subtraction */ + ConstantBias operator-(const ConstantBias& b) const { + return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_); + } - /// @} + /// @} - /// @name Deprecated - /// @{ - ConstantBias inverse() { return -(*this);} - ConstantBias compose(const ConstantBias& q) { return (*this)+q;} - ConstantBias between(const ConstantBias& q) { return q-(*this);} - Vector6 localCoordinates(const ConstantBias& q) { return between(q).vector();} - ConstantBias retract(const Vector6& v) {return compose(ConstantBias(v));} - static Vector6 Logmap(const ConstantBias& p) {return p.vector();} - static ConstantBias Expmap(const Vector6& v) { return ConstantBias(v);} - /// @} + /// @name Deprecated + /// @{ + ConstantBias inverse() { + return -(*this); + } + ConstantBias compose(const ConstantBias& q) { + return (*this) + q; + } + ConstantBias between(const ConstantBias& q) { + return q - (*this); + } + Vector6 localCoordinates(const ConstantBias& q) { + return between(q).vector(); + } + ConstantBias retract(const Vector6& v) { + return compose(ConstantBias(v)); + } + static Vector6 Logmap(const ConstantBias& p) { + return p.vector(); + } + static ConstantBias Expmap(const Vector6& v) { + return ConstantBias(v); + } + /// @} - private: +private: - /// @name Advanced Interface - /// @{ + /// @name Advanced Interface + /// @{ - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this); - ar & BOOST_SERIALIZATION_NVP(biasAcc_); - ar & BOOST_SERIALIZATION_NVP(biasGyro_); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this); + ar & BOOST_SERIALIZATION_NVP(biasAcc_); + ar & BOOST_SERIALIZATION_NVP(biasGyro_); + } - /// @} + /// @} - }; // ConstantBias class +}; // ConstantBias class } // namespace imuBias template<> @@ -197,4 +220,3 @@ struct traits : public internal::VectorSpace< } // namespace gtsam - diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 81120b7c6..01de5a8f3 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -33,29 +33,28 @@ using namespace std; //------------------------------------------------------------------------------ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, - measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration) -{ + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration) { preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { PreintegrationBase::print(s); - cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ -bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { +bool ImuFactor::PreintegratedMeasurements::equals( + const PreintegratedMeasurements& expected, double tol) const { return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) - && PreintegrationBase::equals(expected, tol); + && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::resetIntegration(){ +void ImuFactor::PreintegratedMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } @@ -63,11 +62,12 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor, - OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { + boost::optional body_P_sensor, OptionalJacobian<9, 9> F_test, + OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + correctedAcc, correctedOmega, body_P_sensor); const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr @@ -87,34 +87,37 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise, - // as G and measurementCovariance are blockdiagonal matrices + // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise, + // as G and measurementCovariance are block-diagonal matrices preintMeasCov_ = F * preintMeasCov_ * F.transpose(); - preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT; - preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT; - preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; + preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT; + preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance() + * R_i.transpose() * deltaT; + preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega + * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; Matrix3 F_pos_noiseacc; - if(use2ndOrderIntegration()){ + if (use2ndOrderIntegration()) { F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT; - preintMeasCov_.block<3,3>(0,0) += (1/deltaT) * F_pos_noiseacc * accelerometerCovariance() * F_pos_noiseacc.transpose(); + preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc + * accelerometerCovariance() * F_pos_noiseacc.transpose(); Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT - preintMeasCov_.block<3,3>(0,3) += temp; - preintMeasCov_.block<3,3>(3,0) += temp.transpose(); + preintMeasCov_.block<3, 3>(0, 3) += temp; + preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); } // F_test and G_test are given as output for testing purposes and are not needed by the factor - if(F_test){ // This in only for testing + if (F_test) { // This in only for testing (*F_test) << F; } - if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise - if(!use2ndOrderIntegration()) + if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise + if (!use2ndOrderIntegration()) F_pos_noiseacc = Z_3x3; // intNoise accNoise omegaNoise - (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos - Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle + (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos + Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle } } @@ -122,19 +125,20 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) { +} //------------------------------------------------------------------------------ -ImuFactor::ImuFactor( - Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, +ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor, - const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), - pose_i, vel_i, pose_j, vel_j, bias), - ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), - _PIM_(preintegratedMeasurements) {} + boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : + Base( + noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, + vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, + use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { @@ -144,23 +148,21 @@ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { //------------------------------------------------------------------------------ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "ImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << ")\n"; + cout << s << "ImuFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) + << ")\n"; ImuFactorBase::print(""); _PIM_.print(" preintegrated measurements:"); - this->noiseModel_->print(" noise model: "); + // Print standard deviations on covariance only + cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl; } //------------------------------------------------------------------------------ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && _PIM_.equals(e->_PIM_, tol) - && ImuFactorBase::equals(*e, tol); + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 984d62d08..50e6c835f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -58,7 +58,8 @@ namespace gtsam { * (which are usually slowly varying quantities), which is up to the caller. * See also CombinedImuFactor for a class that does this for you. */ -class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { +class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { public: /** @@ -75,7 +76,7 @@ public: protected: - Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). public: @@ -90,14 +91,17 @@ public: * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) */ PreintegratedMeasurements(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false); + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const bool use2ndOrderIntegration = false); /// print void print(const std::string& s = "Preintegrated Measurements:") const; /// equals - bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const; + bool equals(const PreintegratedMeasurements& expected, + double tol = 1e-9) const; /// Re-initialize PreintegratedMeasurements void resetIntegration(); @@ -106,36 +110,41 @@ public: * Add a single IMU measurement to the preintegration. * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) - * @param deltaT Time interval between two consecutive IMU measurements + * @param deltaT Time interval between this and the last IMU measurement * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) * @param Fout, Gout Jacobians used internally (only needed for testing) */ - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor = boost::none, - OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); + OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = + boost::none); /// methods to access class variables - Matrix preintMeasCov() const { return preintMeasCov_;} + Matrix preintMeasCov() const { + return preintMeasCov_; + } private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; - private: +private: typedef ImuFactor This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactor5 Base; PreintegratedMeasurements _PIM_; - public: +public: /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 @@ -163,9 +172,11 @@ public: ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); - virtual ~ImuFactor() {} + virtual ~ImuFactor() { + } /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; @@ -173,52 +184,58 @@ public: /** implement functions needed for Testable */ /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** Access the preintegrated measurements. */ const PreintegratedMeasurements& preintegratedMeasurements() const { - return _PIM_; } + return _PIM_; + } /** implement functions needed to derive from Factor */ /// vector of errors - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const; + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, + boost::optional H3 = boost::none, boost::optional H4 = + boost::none, boost::optional H5 = boost::none) const; /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ - static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, const Vector3& gravity, + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const PreintegratedMeasurements& PIM, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); - pose_j = PVB.pose; - vel_j = PVB.velocity; + PoseVelocityBias PVB( + PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected_out, + deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; } - private: +private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor5", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; // class ImuFactor +}; +// class ImuFactor typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 1b7919a82..1e4e51220 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -33,15 +33,16 @@ protected: Vector3 gravity_; Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect public: /** Default constructor - only use for serialization */ ImuFactorBase() : - gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), - body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_( + boost::none), use2ndOrderCoriolis_(false) { + } /** * Default constructor, stores basic quantities required by the Imu factors @@ -51,21 +52,29 @@ public: * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect */ ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : - gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) : + gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_( + body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) { + } /// Methods to access class variables - const Vector3& gravity() const { return gravity_; } - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + const Vector3& gravity() const { + return gravity_; + } + const Vector3& omegaCoriolis() const { + return omegaCoriolis_; + } /// Needed for testable //------------------------------------------------------------------------------ - void print(const std::string& s) const { + void print(const std::string& /*s*/) const { std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl; - if(this->body_P_sensor_) + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" + << std::endl; + std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" + << std::endl; + if (this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); } @@ -73,10 +82,11 @@ public: //------------------------------------------------------------------------------ bool equals(const ImuFactorBase& expected, double tol) const { return equal_with_abs_tol(gravity_, expected.gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) - && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) - && ((!body_P_sensor_ && !expected.body_P_sensor_) || - (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); + && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) + && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) + && ((!body_P_sensor_ && !expected.body_P_sensor_) + || (body_P_sensor_ && expected.body_P_sensor_ + && body_P_sensor_->equals(*expected.body_P_sensor_))); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 67deb2d99..2379f58af 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -32,12 +32,12 @@ namespace gtsam { */ class PreintegratedRotation { - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j /// Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delRdelBiasOmega_; - Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements + Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements public: @@ -45,35 +45,46 @@ public: * Default constructor, initializes the variables in the base class */ PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : - deltaRij_(Rot3()), deltaTij_(0.0), - delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {} + deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_( + measuredOmegaCovariance) { + } /// methods to access class variables - Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive - Vector3 thetaRij(OptionalJacobian<3,3> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive - const double& deltaTij() const{return deltaTij_;} - const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} - const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;} + Matrix3 deltaRij() const { + return deltaRij_.matrix(); + } // expensive + Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const { + return Rot3::Logmap(deltaRij_, H); + } // super-expensive + const double& deltaTij() const { + return deltaTij_; + } + const Matrix3& delRdelBiasOmega() const { + return delRdelBiasOmega_; + } + const Matrix3& gyroscopeCovariance() const { + return gyroscopeCovariance_; + } /// Needed for testable void print(const std::string& s) const { std::cout << s << std::endl; - std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl; - std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl; + std::cout << " deltaTij [" << deltaTij_ << "]" << std::endl; + std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl; } /// Needed for testable bool equals(const PreintegratedRotation& expected, double tol) const { return deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol) - && equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol); + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, + tol) + && equal_with_abs_tol(gyroscopeCovariance_, + expected.gyroscopeCovariance_, tol); } /// Re-initialize PreintegratedMeasurements - void resetIntegration(){ + void resetIntegration() { deltaRij_ = Rot3(); deltaTij_ = 0.0; delRdelBiasOmega_ = Z_3x3; @@ -81,7 +92,7 @@ public: /// Update preintegrated measurements void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, - OptionalJacobian<3, 3> H = boost::none){ + OptionalJacobian<3, 3> H = boost::none) { deltaRij_ = deltaRij_.compose(incrR, H, boost::none); deltaTij_ += deltaT; } @@ -90,15 +101,16 @@ public: * Update Jacobians to be used during preintegration * TODO: explain arguments */ - void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, - double deltaT) { + void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, + const Rot3& incrR, double deltaT) { const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - D_Rincr_integratedOmega * deltaT; } /// Return a bias corrected version of the integrated rotation - expensive Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { - return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); + return deltaRij_ * Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); } /// Get so<3> version of bias corrected rotation, with optional Jacobian @@ -111,7 +123,8 @@ public: if (H) { Matrix3 Jrinv_theta_bc; // This was done via an expmap, now we go *back* to so<3> - const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, + Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = // Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; @@ -131,7 +144,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp new file mode 100644 index 000000000..496569599 --- /dev/null +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -0,0 +1,357 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "PreintegrationBase.h" + +namespace gtsam { + +PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, + const bool use2ndOrderIntegration) + : PreintegratedRotation(measuredOmegaCovariance), + biasHat_(bias), + use2ndOrderIntegration_(use2ndOrderIntegration), + deltaPij_(Vector3::Zero()), + deltaVij_(Vector3::Zero()), + delPdelBiasAcc_(Z_3x3), + delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), + delVdelBiasOmega_(Z_3x3), + accelerometerCovariance_(measuredAccCovariance), + integrationCovariance_(integrationErrorCovariance) { +} + +/// Needed for testable +void PreintegrationBase::print(const std::string& s) const { + PreintegratedRotation::print(s); + std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; + biasHat_.print(" biasHat"); +} + +/// Needed for testable +bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { + return PreintegratedRotation::equals(other, tol) && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol) + && equal_with_abs_tol(accelerometerCovariance_, other.accelerometerCovariance_, tol) + && equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol); +} + +/// Re-initialize PreintegratedMeasurements +void PreintegrationBase::resetIntegration() { + PreintegratedRotation::resetIntegration(); + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; +} + +/// Update preintegrated measurements +void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc, + const Rot3& incrR, const double deltaT, + OptionalJacobian<9, 9> F) { + + const Matrix3 dRij = deltaRij(); // expensive + const Vector3 temp = dRij * correctedAcc * deltaT; + if (!use2ndOrderIntegration_) { + deltaPij_ += deltaVij_ * deltaT; + } else { + deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; + } + deltaVij_ += temp; + + Matrix3 R_i, F_angles_angles; + if (F) + R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij + updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0); + + if (F) { + const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; + Matrix3 F_pos_angles; + if (use2ndOrderIntegration_) + F_pos_angles = 0.5 * F_vel_angles * deltaT; + else + F_pos_angles = Z_3x3; + + // pos vel angle + *F << // + I_3x3, I_3x3 * deltaT, F_pos_angles, // pos + Z_3x3, I_3x3, F_vel_angles, // vel + Z_3x3, Z_3x3, F_angles_angles; // angle + } +} + +/// Update Jacobians to be used during preintegration +void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc, + const Matrix3& D_Rincr_integratedOmega, + const Rot3& incrR, double deltaT) { + const Matrix3 dRij = deltaRij(); // expensive + const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); + if (!use2ndOrderIntegration_) { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + } else { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; + delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5); + } + delVdelBiasAcc_ += -dRij * deltaT; + delVdelBiasOmega_ += temp; + update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); +} + +void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( + const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, boost::optional body_P_sensor) { + correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame + if (body_P_sensor) { + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc + - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } +} + +/// Predict state at time j +//------------------------------------------------------------------------------ +PoseVelocityBias PreintegrationBase::predict( + const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, + boost::optional deltaPij_biascorrected_out, + boost::optional deltaVij_biascorrected_out) const { + + const imuBias::ConstantBias biasIncr = bias_i - biasHat(); + const Vector3& biasAccIncr = biasIncr.accelerometer(); + const Vector3& biasOmegaIncr = biasIncr.gyroscope(); + + const Rot3& Rot_i = pose_i.rotation(); + const Matrix3 Rot_i_matrix = Rot_i.matrix(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + + delPdelBiasOmega() * biasOmegaIncr; + if (deltaPij_biascorrected_out) // if desired, store this + *deltaPij_biascorrected_out = deltaPij_biascorrected; + + const double dt = deltaTij(), dt2 = dt * dt; + Vector3 pos_j = pos_i + Rot_i_matrix * deltaPij_biascorrected + vel_i * dt + - omegaCoriolis.cross(vel_i) * dt2 // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * dt2; + + const Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + + delVdelBiasOmega() * biasOmegaIncr; + if (deltaVij_biascorrected_out) // if desired, store this + *deltaVij_biascorrected_out = deltaVij_biascorrected; + + Vector3 vel_j = Vector3( + vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term + + gravity * dt); + + if (use2ndOrderCoriolis) { + pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // 2nd order coriolis term for position + vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) + + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); + const Vector3 correctedOmega = biascorrectedOmega + - Rot_i_matrix.transpose() * omegaCoriolis * dt; // Coriolis term + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); + const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij); + + const Pose3 pose_j = Pose3(Rot_j, Point3(pos_j)); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant +} + +/// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const Vector3& gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis, + OptionalJacobian<9, 6> H1, + OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4, + OptionalJacobian<9, 6> H5) const { + + // We need the mismatch w.r.t. the biases used for preintegration + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3& Ri = pose_i.rotation(); + const Matrix3 Ri_transpose_matrix = Ri.transpose(); + + const Rot3& Rj = pose_j.rotation(); + const Vector3 pos_j = pose_j.translation().vector(); + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 deltaPij_biascorrected, deltaVij_biascorrected; + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected, + deltaVij_biascorrected); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fp = Ri_transpose_matrix * (pos_j - predictedState_j.pose.translation().vector()); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fv = Ri_transpose_matrix * (vel_j - predictedState_j.velocity); + + // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) + + /* ---------------------------------------------------------------------------------------------------- */ + // Get Get so<3> version of bias corrected rotation + // If H5 is asked for, we will need the Jacobian, which we store in H5 + // H5 will then be corrected below to take into account the Coriolis effect + Matrix3 D_cThetaRij_biasOmegaIncr; + const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, + H5 ? &D_cThetaRij_biasOmegaIncr : 0); + + // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration + const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; + + // Calculate Jacobians, matrices below needed only for some Jacobians... + Vector3 fR; + Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, Ritranspose_omegaCoriolisHat; + + if (H1 || H2) + Ritranspose_omegaCoriolisHat = Ri_transpose_matrix * skewSymmetric(omegaCoriolis); + + // This is done to save computation: we ask for the jacobians only when they are needed + const double dt = deltaTij(), dt2 = dt*dt; + Rot3 fRrot; + const Rot3 RiBetweenRj = Rot3(Ri_transpose_matrix) * Rj; + if (H1 || H2 || H3 || H4 || H5) { + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(RiBetweenRj); + fR = Rot3::Logmap(fRrot, D_fR_fRrot); + D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); + } else { + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(RiBetweenRj); + fR = Rot3::Logmap(fRrot); + } + if (H1) { + Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3; + if (use2ndOrderCoriolis) { + // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() + const Matrix3 temp = Ritranspose_omegaCoriolisHat + * (-Ritranspose_omegaCoriolisHat.transpose()); + dfPdPi += 0.5 * temp * dt2; + dfVdPi += temp * dt; + } + (*H1) << + // dfP/dRi + skewSymmetric(fp + deltaPij_biascorrected), + // dfP/dPi + dfPdPi, + // dfV/dRi + skewSymmetric(fv + deltaVij_biascorrected), + // dfV/dPi + dfVdPi, + // dfR/dRi + D_fR_fRrot * (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), + // dfR/dPi + Z_3x3; + } + if (H2) { + (*H2) << + // dfP/dVi + -Ri_transpose_matrix * dt + Ritranspose_omegaCoriolisHat * dt2, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + -Ri_transpose_matrix + 2 * Ritranspose_omegaCoriolisHat * dt, // Coriolis term + // dfR/dVi + Z_3x3; + } + if (H3) { + (*H3) << + // dfP/dPosej + Z_3x3, Ri_transpose_matrix * Rj.matrix(), + // dfV/dPosej + Matrix::Zero(3, 6), + // dfR/dPosej + D_fR_fRrot, Z_3x3; + } + if (H4) { + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + Ri_transpose_matrix, + // dfR/dVj + Z_3x3; + } + if (H5) { + // H5 by this point already contains 3*3 biascorrectedThetaRij derivative + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; + (*H5) << + // dfP/dBias + -delPdelBiasAcc(), -delPdelBiasOmega(), + // dfV/dBias + -delVdelBiasAcc(), -delVdelBiasOmega(), + // dfR/dBias + Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega); + } + Vector9 r; + r << fp, fv, fR; + return r; +} + +ImuBase::ImuBase() + : gravity_(Vector3(0.0, 0.0, 9.81)), + omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), + body_P_sensor_(boost::none), + use2ndOrderCoriolis_(false) { +} + +ImuBase::ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor, const bool use2ndOrderCoriolis) + : gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis) { +} + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 29b9b6e66..f6b24e752 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -23,6 +23,9 @@ #include #include +#include +#include +#include namespace gtsam { @@ -34,9 +37,10 @@ struct PoseVelocityBias { Vector3 velocity; imuBias::ConstantBias bias; - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias) + : pose(_pose), + velocity(_velocity), + bias(_bias) { } }; @@ -48,22 +52,22 @@ struct PoseVelocityBias { */ class PreintegrationBase : public PreintegratedRotation { - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - bool use2ndOrderIntegration_; ///< Controls the order of integration + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + bool use2ndOrderIntegration_; ///< Controls the order of integration - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements - Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty + const Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements + const Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty /// (to compensate errors in Euler integration) -public: + public: /** * Default constructor, initializes the variables in the base class @@ -71,328 +75,92 @@ public: * @param use2ndOrderIntegration Controls the order of integration * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) */ - PreintegrationBase(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, - const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) : - PreintegratedRotation(measuredOmegaCovariance), - biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), - deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - accelerometerCovariance_(measuredAccCovariance), - integrationCovariance_(integrationErrorCovariance) {} + PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration); /// methods to access class variables - bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;} - const Vector3& deltaPij() const {return deltaPij_;} - const Vector3& deltaVij() const {return deltaVij_;} - const imuBias::ConstantBias& biasHat() const { return biasHat_;} - Vector6 biasHatVector() const { return biasHat_.vector();} // expensive - const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} - const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} - const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} - const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;} - - const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;} - const Matrix3& integrationCovariance() const { return integrationCovariance_;} - - /// Needed for testable - void print(const std::string& s) const { - PreintegratedRotation::print(s); - std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl; - std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - biasHat_.print(" biasHat"); + bool use2ndOrderIntegration() const { + return use2ndOrderIntegration_; + } + const Vector3& deltaPij() const { + return deltaPij_; + } + const Vector3& deltaVij() const { + return deltaVij_; + } + const imuBias::ConstantBias& biasHat() const { + return biasHat_; + } + Vector6 biasHatVector() const { + return biasHat_.vector(); + } // expensive + const Matrix3& delPdelBiasAcc() const { + return delPdelBiasAcc_; + } + const Matrix3& delPdelBiasOmega() const { + return delPdelBiasOmega_; + } + const Matrix3& delVdelBiasAcc() const { + return delVdelBiasAcc_; + } + const Matrix3& delVdelBiasOmega() const { + return delVdelBiasOmega_; } - /// Needed for testable - bool equals(const PreintegrationBase& expected, double tol) const { - return PreintegratedRotation::equals(expected, tol) - && biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol) - && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol); + const Matrix3& accelerometerCovariance() const { + return accelerometerCovariance_; } + const Matrix3& integrationCovariance() const { + return integrationCovariance_; + } + + /// print + void print(const std::string& s) const; + + /// check equality + bool equals(const PreintegrationBase& other, double tol) const; /// Re-initialize PreintegratedMeasurements - void resetIntegration(){ - PreintegratedRotation::resetIntegration(); - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; - } + void resetIntegration(); /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { - - Matrix3 dRij = deltaRij(); // expensive - Vector3 temp = dRij * correctedAcc * deltaT; - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; - } - deltaVij_ += temp; - - Matrix3 R_i, F_angles_angles; - if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij - updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0); - - if(F){ - Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; - Matrix3 F_pos_angles; - if(use2ndOrderIntegration_) - F_pos_angles = 0.5 * F_vel_angles * deltaT; - else - F_pos_angles = Z_3x3; - - // pos vel angle - *F << I_3x3, I_3x3 * deltaT, F_pos_angles, // pos - Z_3x3, I_3x3, F_vel_angles, // vel - Z_3x3, Z_3x3, F_angles_angles;// angle - } - } + void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR, + const double deltaT, OptionalJacobian<9, 9> F); /// Update Jacobians to be used during preintegration void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){ - Matrix3 dRij = deltaRij(); // expensive - Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); - if (!use2ndOrderIntegration_) { - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - } else { - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; - delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5); - } - delVdelBiasAcc_ += -dRij * deltaT; - delVdelBiasOmega_ += temp; - update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); - } + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, + double deltaT); void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, boost::optional body_P_sensor) { - correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities - // (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - } + const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, + boost::optional body_P_sensor); /// Predict state at time j - //------------------------------------------------------------------------------ - PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + PoseVelocityBias predict( + const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, - boost::optional deltaVij_biascorrected_out = boost::none) const { - - const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); - - const Rot3& Rot_i = pose_i.rotation(); - const Vector3& pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; - if(deltaPij_biascorrected_out)// if desired, store this - *deltaPij_biascorrected_out = deltaPij_biascorrected; - - Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected - + vel_i * deltaTij() - - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij()*deltaTij(); - - Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr; - if(deltaVij_biascorrected_out)// if desired, store this - *deltaVij_biascorrected_out = deltaVij_biascorrected; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected - - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term - + gravity * deltaTij()); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position - vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); - Vector3 correctedOmega = biascorrectedOmega - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term - const Rot3 correctedDeltaRij = - Rot3::Expmap( correctedOmega ); - const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant - } + boost::optional deltaVij_biascorrected_out = boost::none) const; /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j - //------------------------------------------------------------------------------ - Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, - const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, - OptionalJacobian<9, 6> H1 = boost::none, - OptionalJacobian<9, 3> H2 = boost::none, - OptionalJacobian<9, 6> H3 = boost::none, - OptionalJacobian<9, 3> H4 = boost::none, - OptionalJacobian<9, 6> H5 = boost::none) const { + Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, + const Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis, OptionalJacobian<9, 6> H1 = + boost::none, + OptionalJacobian<9, 3> H2 = boost::none, + OptionalJacobian<9, 6> H3 = boost::none, + OptionalJacobian<9, 3> H4 = boost::none, + OptionalJacobian<9, 6> H5 = boost::none) const; - // We need the mismatch w.r.t. the biases used for preintegration - // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary - const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3& Ri = pose_i.rotation(); - const Rot3& Rj = pose_j.rotation(); - const Vector3& pos_j = pose_j.translation().vector(); - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected, deltaVij_biascorrected; - PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, - omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); - - // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) - - // Jacobian computation - /* ---------------------------------------------------------------------------------------------------- */ - // Get Get so<3> version of bias corrected rotation - // If H5 is asked for, we will need the Jacobian, which we store in H5 - // H5 will then be corrected below to take into account the Coriolis effect - Matrix3 D_cThetaRij_biasOmegaIncr; - Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0); - - // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration - const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); - const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - Vector3 correctedOmega = biascorrectedOmega - coriolis; - - Rot3 correctedDeltaRij, fRrot; - Vector3 fR; - - // Accessory matrix, used to build the jacobians - Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; - - // This is done to save computation: we ask for the jacobians only when they are needed - if(H1 || H2 || H3 || H4 || H5){ - correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRrot, D_fR_fRrot); - D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); - }else{ - correctedDeltaRij = Rot3::Expmap( correctedOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRrot); - } - if(H1) { - H1->resize(9,6); - Matrix3 dfPdPi = -I_3x3; - Matrix3 dfVdPi = Z_3x3; - if(use2ndOrderCoriolis){ - // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() - Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose()); - dfPdPi += 0.5 * temp * deltaTij()*deltaTij(); - dfVdPi += temp * deltaTij(); - } - (*H1) << - // dfP/dRi - skewSymmetric(fp + deltaPij_biascorrected), - // dfP/dPi - dfPdPi, - // dfV/dRi - skewSymmetric(fv + deltaVij_biascorrected), - // dfV/dPi - dfVdPi, - // dfR/dRi - D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), - // dfR/dPi - Z_3x3; - } - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - Ri.transpose() * deltaTij() - + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Ri.transpose() - + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term - // dfR/dVi - Z_3x3; - } - if(H3) { - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Z_3x3, Ri.transpose() * Rj.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - D_fR_fRrot, Z_3x3; - } - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - Ri.transpose(), - // dfR/dVj - Z_3x3; - } - if(H5) { - // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; - H5->resize(9,6); - (*H5) << - // dfP/dBias - - delPdelBiasAcc(), - delPdelBiasOmega(), - // dfV/dBias - - delVdelBiasAcc(), - delVdelBiasOmega(), - // dfR/dBias - Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); - } - Vector9 r; r << fp, fv, fR; - return r; - } - -private: + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(deltaPij_); @@ -406,27 +174,30 @@ private: class ImuBase { -protected: + protected: Vector3 gravity_; Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect -public: + public: - ImuBase() : - gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), - body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + /// Default constructor, with decent gravity and zero coriolis + ImuBase(); + /// Fully fledge constructor ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : - gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); - const Vector3& gravity() const { return gravity_; } - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + const Vector3& gravity() const { + return gravity_; + } + const Vector3& omegaCoriolis() const { + return omegaCoriolis_; + } }; -} /// namespace gtsam +} /// namespace gtsam diff --git a/gtsam/navigation/tests/CMakeLists.txt b/gtsam/navigation/tests/CMakeLists.txt index b03b8167c..0beca7769 100644 --- a/gtsam/navigation/tests/CMakeLists.txt +++ b/gtsam/navigation/tests/CMakeLists.txt @@ -14,15 +14,10 @@ if(GTSAM_INSTALL_GEOGRAPHICLIB) endif() else() - if(GEOGRAPHICLIB_FOUND) + if(GeographicLib_LIBRARIES) # If we're not installing, but it's already installed, use the installed one include_directories(${GeographicLib_INCLUDE_DIRS}) - list(APPEND test_link_libraries ) - #if(MSVC) - # list(APPEND test_link_libraries GeographicLib_STATIC) - #else() - list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) - #endif() + list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) else() # We don't have GeographicLib set(tests_excluded testGeographicLib.cpp testGPSFactor.cpp testMagFactor.cpp) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 25b3518bc..928c0f74f 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -132,7 +132,7 @@ TEST(AHRSFactor, Error) { pre_int_data.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, false); + AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, boost::none); Vector3 errorActual = factor.evaluateError(x1, x2, bias); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index c6eb1396e..9fb0f939b 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -44,36 +44,39 @@ namespace { // Auxiliary functions to test Jacobians F and G used for // covariance propagation during preintegration /* ************************************************************************* */ -Vector updatePreintegratedMeasurementsTest( - const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const imuBias::ConstantBias& bias_old, - const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, +Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old, + const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, + const Vector3& correctedOmega, const double deltaT, const bool use2ndOrderIntegration) { Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; Vector3 deltaPij_new; - if(!use2ndOrderIntegration){ + if (!use2ndOrderIntegration) { deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - }else{ + } else { deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; } Vector3 deltaVij_new = deltaVij_old + temp; - Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT); + Rot3 deltaRij_new = deltaRij_old + * Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT); Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more imuBias::ConstantBias bias_new(bias_old); - Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); + Vector result(15); + result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); return result; } -Rot3 updatePreintegratedMeasurementsRot( - const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const imuBias::ConstantBias& bias_old, - const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, - const bool use2ndOrderIntegration){ +Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old, + const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, + const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration) { - Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old, - bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration); + Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega, + deltaT, use2ndOrderIntegration); return Rot3::Expmap(result.segment<3>(6)); } @@ -82,60 +85,53 @@ Rot3 updatePreintegratedMeasurementsRot( // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); - for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } return result; } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaPij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaVij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaVij(); } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return Rot3(evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaRij()); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaRij()); } } /* ************************************************************************* */ -TEST( CombinedImuFactor, PreintegratedMeasurements ) -{ +TEST( CombinedImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; double tol = 1e-6; @@ -145,58 +141,75 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix::Zero(6, 6)); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol)); - EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol)); - EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); + EXPECT( + assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), + tol)); + EXPECT( + assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), + tol)); + EXPECT( + assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), + tol)); DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } /* ************************************************************************* */ -TEST( CombinedImuFactor, ErrorWithBiases ) -{ +TEST( CombinedImuFactor, ErrorWithBiases ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(0.5, 0.0, 0.0); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(0.5, 0.0, 0.0); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; double tol = 1e-6; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6); - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, + deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr Combinedmodel = + noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), + Combined_pre_int_data, gravity, omegaCoriolis); Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); - Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, + bias2); EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); @@ -206,7 +219,8 @@ TEST( CombinedImuFactor, ErrorWithBiases ) // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; - (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); + (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, + H3a, H4a, H5a, H6a); EXPECT(assert_equal(H1e, H1a.topRows(9))); EXPECT(assert_equal(H2e, H2a.topRows(9))); @@ -216,101 +230,128 @@ TEST( CombinedImuFactor, ErrorWithBiases ) } /* ************************************************************************* */ -TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) -{ +TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } // Actual preintegrated values CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + EXPECT( + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT( + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT( + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), + 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST(CombinedImuFactor, PredictPositionAndVelocity){ - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) +TEST(CombinedImuFactor, PredictPositionAndVelocity) { + imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,1,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 1.1, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); + bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); - for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i < 1000; ++i) + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, + deltaT); // Create factor - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr Combinedmodel = + noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), + Combined_pre_int_data, gravity, omegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, + bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; + Vector3 expectedVelocity; + expectedVelocity << 0, 1, 0; EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); + EXPECT( + assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); } /* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - Matrix I6x6(6,6); + Matrix I6x6(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); - Vector3 measuredAcc; measuredAcc << 0, 0, -9.81; - Vector3 gravity; gravity<<0,0,9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0,0,0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; + bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); + Vector3 measuredAcc; + measuredAcc << 0, 0, -9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0; double deltaT = 0.001; double tol = 1e-4; for (int i = 0; i < 1000; ++i) @@ -320,16 +361,16 @@ TEST(CombinedImuFactor, PredictRotation) { Combined_pre_int_data, gravity, omegaCoriolis); // Predict - Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)), x2; - Vector3 v(0,0,0), v2; - CombinedImuFactor::Predict(x, v, x2, v2, bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); + Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; + Vector3 v(0, 0, 0), v2; + CombinedImuFactor::Predict(x, v, x2, v2, bias, + Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); + Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); EXPECT(assert_equal(expectedPose, x2, tol)); } /* ************************************************************************* */ -TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) -{ +TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { // Linearization point imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases Pose3 body_P_sensor = Pose3(); @@ -338,35 +379,36 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } // Actual preintegrated values CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs); + evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, + deltaTs); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement Matrix oldPreintCovariance = preintegrated.preintMeasCov(); Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, - body_P_sensor, Factual, Gactual); + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); bool use2ndOrderIntegration = false; @@ -374,44 +416,51 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) // COMPUTE NUMERICAL DERIVATIVES FOR F ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected F wrt positions (15,3) - Matrix df_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - _1, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + Matrix df_dpos = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaPij_old); // rotation part has to be done properly, on manifold - df_dpos.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - _1, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + df_dpos.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaPij_old); // Compute expected F wrt velocities (15,3) - Matrix df_dvel = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, _1, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + Matrix df_dvel = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaVij_old); // rotation part has to be done properly, on manifold - df_dvel.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, _1, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + df_dvel.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaVij_old); // Compute expected F wrt angles (15,3) - Matrix df_dangle = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, _1, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix df_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), deltaRij_old); // rotation part has to be done properly, on manifold - df_dangle.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, _1, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + df_dangle.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), deltaRij_old); // Compute expected F wrt biases (15,6) - Matrix df_dbias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + Matrix df_dbias = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); // rotation part has to be done properly, on manifold - df_dbias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + df_dbias.block<3, 6>(6, 0) = + numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); - Matrix Fexpected(15,15); + Matrix Fexpected(15, 15); Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; EXPECT(assert_equal(Fexpected, Factual)); @@ -419,55 +468,65 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) // COMPUTE NUMERICAL DERIVATIVES FOR G ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected G wrt integration noise - Matrix df_dintNoise(15,3); + Matrix df_dintNoise(15, 3); df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; // Compute expected G wrt acc noise (15,3) - Matrix df_daccNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + Matrix df_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // rotation part has to be done properly, on manifold - df_daccNoise.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // Compute expected G wrt gyro noise (15,3) - Matrix df_domegaNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + Matrix df_domegaNoise = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, + use2ndOrderIntegration), newMeasuredOmega); // rotation part has to be done properly, on manifold - df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, + use2ndOrderIntegration), newMeasuredOmega); // Compute expected G wrt bias random walk noise (15,6) - Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries + Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries df_rwBias.setZero(); - df_rwBias.block<6,6>(9,0) = eye(6); + df_rwBias.block<6, 6>(9, 0) = eye(6); // Compute expected G wrt gyro noise (15,6) - Matrix df_dinitBias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + Matrix df_dinitBias = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); // rotation part has to be done properly, on manifold - df_dinitBias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); - df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows + df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); + df_dinitBias.block<6, 6>(9, 0) = Matrix::Zero(6, 6); // only has to influence first 9 rows - Matrix Gexpected(15,21); + Matrix Gexpected(15, 21); Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + - (1/newDeltaT) * Gactual * Gactual.transpose(); + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index bd321d51d..4d5df3f05 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -21,23 +21,111 @@ using namespace std; using namespace gtsam; +Vector biasAcc1(Vector3(0.2, -0.1, 0)); +Vector biasGyro1(Vector3(0.1, -0.3, -0.2)); +imuBias::ConstantBias bias1(biasAcc1, biasGyro1); + +Vector biasAcc2(Vector3(0.1, 0.2, 0.04)); +Vector biasGyro2(Vector3(-0.002, 0.005, 0.03)); +imuBias::ConstantBias bias2(biasAcc2, biasGyro2); /* ************************************************************************* */ -TEST( ImuBias, Constructor) -{ - Vector bias_acc(Vector3(0.1,0.2,0.4)); - Vector bias_gyro(Vector3(-0.2, 0.5, 0.03)); - +TEST( ImuBias, Constructor) { // Default Constructor - gtsam::imuBias::ConstantBias bias1; + imuBias::ConstantBias bias1; // Acc + Gyro Constructor - gtsam::imuBias::ConstantBias bias2(bias_acc, bias_gyro); + imuBias::ConstantBias bias2(biasAcc2, biasGyro2); // Copy Constructor - gtsam::imuBias::ConstantBias bias3(bias2); + imuBias::ConstantBias bias3(bias2); } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +TEST( ImuBias, inverse) { + imuBias::ConstantBias biasActual = bias1.inverse(); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, + -biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, compose) { + imuBias::ConstantBias biasActual = bias2.compose(bias1); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, between) { + // p.between(q) == q - p + imuBias::ConstantBias biasActual = bias2.between(bias1); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, localCoordinates) { + Vector deltaActual = Vector(bias2.localCoordinates(bias1)); + Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2, + biasGyro1 - biasGyro2)).vector(); + EXPECT(assert_equal(deltaExpected, deltaActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, retract) { + Vector6 delta; + delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; + imuBias::ConstantBias biasActual = bias2.retract(delta); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0)); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, Logmap) { + Vector deltaActual = bias2.Logmap(bias1); + Vector deltaExpected = bias1.vector(); + EXPECT(assert_equal(deltaExpected, deltaActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, Expmap) { + Vector6 delta; + delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; + imuBias::ConstantBias biasActual = bias2.Expmap(delta); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, operatorSub) { + imuBias::ConstantBias biasActual = -bias1; + imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, operatorAdd) { + imuBias::ConstantBias biasActual = bias2 + bias1; + imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, + biasGyro2 + biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, operatorSubB) { + imuBias::ConstantBias biasActual = bias2 - bias1; + imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, + biasGyro2 - biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 62f496894..c27921fc9 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -39,37 +40,39 @@ using symbol_shorthand::B; namespace { // Auxiliary functions to test evaluate error in ImuFactor /* ************************************************************************* */ -Vector callEvaluateError(const ImuFactor& factor, - const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias){ +Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias) { return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); } -Rot3 evaluateRotationError(const ImuFactor& factor, - const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias){ - return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; +Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias) { + return Rot3::Expmap( + factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3)); } // Auxiliary functions to test Jacobians F and G used for // covariance propagation during preintegration /* ************************************************************************* */ -Vector updatePreintegratedPosVel( - const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, - const bool use2ndOrderIntegration_) { +Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, + const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, + const double deltaT, const bool use2ndOrderIntegration_) { Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * correctedAcc * deltaT; Vector3 deltaPij_new; - if(!use2ndOrderIntegration_){ + if (!use2ndOrderIntegration_) { deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - }else{ + } else { deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; } Vector3 deltaVij_new = deltaVij_old + temp; - Vector result(6); result << deltaPij_new, deltaVij_new; + Vector result(6); + result << deltaPij_new, deltaVij_new; return result; } @@ -79,138 +82,160 @@ Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, return deltaRij_new; } -// Auxiliary functions to test preintegrated Jacobians -// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +// Define covariance matrices /* ************************************************************************* */ double accNoiseVar = 0.01; double omegaNoiseVar = 0.03; double intNoiseVar = 0.0001; +const Matrix3 kMeasuredAccCovariance = accNoiseVar * Matrix3::Identity(); +const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * Matrix3::Identity(); +const Matrix3 kIntegrationErrorCovariance = intNoiseVar * Matrix3::Identity(); + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const bool use2ndOrderIntegration = false){ - ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(), - omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity(),use2ndOrderIntegration); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs, + const bool use2ndOrderIntegration = false) { + ImuFactor::PreintegratedMeasurements result(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); - for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } return result; } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaPij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaVij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaVij(); } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return Rot3(evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaRij()); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaRij()); } -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){ +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, + const double deltaT) { return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); } -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { + return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); } } /* ************************************************************************* */ -TEST( ImuFactor, PreintegratedMeasurements ) -{ +TEST( ImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; // Expected preintegrated values - Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5*0.1*0.5*0.5, 0, 0; + Vector3 expectedDeltaP1; + expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0; Vector3 expectedDeltaV1(0.05, 0.0, 0.0); - Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); + Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT1(0.5); bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements actual1(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); - EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); // Integrate again - Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0; - Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5; - Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); + Vector3 expectedDeltaP2; + expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; + Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + + expectedDeltaR1.matrix() * measuredAcc * 0.5; + Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT2(1); // Actual preintegrated values ImuFactor::PreintegratedMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); - EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobians ) -{ +TEST( ImuFactor, ErrorAndJacobians ) { // Linearization point imuBias::ConstantBias bias; // Bias - Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << M_PI / 100, 0, 0; Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); double deltaT = 1.0; bool use2ndOrderIntegration = true; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements pre_int_data(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); // Expected error - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + Vector errorExpected(9); + errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Actual Jacobians @@ -219,61 +244,102 @@ TEST( ImuFactor, ErrorAndJacobians ) // Expected Jacobians /////////////////// H1 /////////////////////////// - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); // Jacobians are around zero, so the rotation part is the same as: - Matrix H1Rot3 = numericalDerivative11( + Matrix H1Rot3 = numericalDerivative11( boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3))); EXPECT(assert_equal(H1e, H1a)); /////////////////// H2 /////////////////////////// - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); EXPECT(assert_equal(H2e, H2a)); /////////////////// H3 /////////////////////////// - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); // Jacobians are around zero, so the rotation part is the same as: - Matrix H3Rot3 = numericalDerivative11( + Matrix H3Rot3 = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3))); EXPECT(assert_equal(H3e, H3a)); /////////////////// H4 /////////////////////////// - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); EXPECT(assert_equal(H4e, H4a)); /////////////////// H5 /////////////////////////// - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); EXPECT(assert_equal(H5e, H5a)); + + //////////////////////////////////////////////////////////////////////////// + // Evaluate error with wrong values + Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); + errorActual = factor.evaluateError(x1, v1, x2, v2_wrong, bias); + errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; + EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2_wrong); + values.insert(B(1), bias); + errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; + EXPECT(assert_equal(factor.unwhitenedError(values), errorActual, 1e-6)); + + // Make sure the whitening is done correctly + Matrix cov = pre_int_data.preintMeasCov(); + Matrix R = RtR(cov.inverse()); + Vector whitened = R * errorActual; + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6)); + + /////////////////////////////////////////////////////////////////////////////// + // Make sure linearization is correct + // Create expected value by numerical differentiation + JacobianFactor expected = linearizeNumerically(factor, values, 1e-8); + + // Create actual value by linearize + GaussianFactor::shared_ptr linearized = factor.linearize(values); + JacobianFactor* actual = dynamic_cast(linearized.get()); + + // Check cast result and then equality + EXPECT(assert_equal(expected, *actual, 1e-4)); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWithBiases ) -{ +TEST( ImuFactor, ErrorAndJacobianWithBiases ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0), + Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); SETDEBUG("ImuFactor evaluateError", false); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -285,23 +351,23 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( + Matrix RH1e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( + Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( + Matrix RH5e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); // Actual Jacobians @@ -316,29 +382,36 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) -{ +TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0), + Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor Pose3 bodyPsensor = Pose3(); bool use2ndOrderCoriolis = true; - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); SETDEBUG("ImuFactor evaluateError", false); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -350,23 +423,23 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( + Matrix RH1e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( + Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( + Matrix RH5e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); // Actual Jacobians @@ -381,39 +454,43 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivative_wrt_Bias ) -{ +TEST( ImuFactor, PartialDerivative_wrt_Bias ) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega; + biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0.1, 0, 0; double deltaT = 0.5; // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( - &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); + Matrix expectedDelRdelBiasOmega = numericalDerivative11( + boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), + Vector3(biasOmega)); - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); - Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeLogmap ) -{ +TEST( ImuFactor, PartialDerivativeLogmap ) { // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + Vector3 thetahat; + thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 deltatheta; deltatheta << 0, 0, 0; + Vector3 deltatheta; + deltatheta << 0, 0, 0; // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( - &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); + Matrix expectedDelFdeltheta = numericalDerivative11( + boost::bind(&evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); @@ -422,28 +499,33 @@ TEST( ImuFactor, PartialDerivativeLogmap ) } /* ************************************************************************* */ -TEST( ImuFactor, fistOrderExponential ) -{ +TEST( ImuFactor, fistOrderExponential ) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega; + biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0.1, 0, 0; double deltaT = 1.0; // change w.r.t. linearization point double alpha = 0.0; - Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; + Vector3 deltabiasOmega; + deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); - Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign - const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + const Matrix expectedRot = Rot3::Expmap( + (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); - const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); - const Matrix3 actualRot = - hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + const Matrix3 hatRot = + Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 actualRot = hatRot + * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); // This is a first order expansion so the equality is only an approximation @@ -451,61 +533,70 @@ TEST( ImuFactor, fistOrderExponential ) } /* ************************************************************************* */ -TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) -{ +TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } // Actual preintegrated values ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + EXPECT( + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT( + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT( + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), + 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) -{ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); @@ -514,68 +605,71 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } bool use2ndOrderIntegration = false; // Actual preintegrated values ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs, use2ndOrderIntegration); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement Matrix oldPreintCovariance = preintegrated.preintMeasCov(); Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, - body_P_sensor, Factual, Gactual); + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR F ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected f_pos_vel wrt positions - Matrix dfpv_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + Matrix dfpv_dpos = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaPij_old); // Compute expected f_pos_vel wrt velocities - Matrix dfpv_dvel = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + Matrix dfpv_dvel = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaVij_old); // Compute expected f_pos_vel wrt angles - Matrix dfpv_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix dfpv_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaRij_old); - Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; + Matrix FexpectedTop6(6, 9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; // Compute expected f_rot wrt angles - Matrix dfr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - _1, newMeasuredOmega, newDeltaT), deltaRij_old); + Matrix dfr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), + deltaRij_old); - Matrix FexpectedBottom3(3,9); + Matrix FexpectedBottom3(3, 9); FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; - Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3; + Matrix Fexpected(9, 9); + Fexpected << FexpectedTop6, FexpectedBottom3; EXPECT(assert_equal(Fexpected, Factual)); @@ -583,50 +677,50 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) // COMPUTE NUMERICAL DERIVATIVES FOR G ////////////////////////////////////////////////////////////////////////////////////////////// // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6,3); + Matrix dgpv_dintNoise(6, 3); dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; // Compute jacobian wrt acc noise - Matrix dgpv_daccNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + Matrix dgpv_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // Compute expected F wrt gyro noise - Matrix dgpv_domegaNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); - Matrix GexpectedTop6(6,9); + Matrix dgpv_domegaNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), + newMeasuredOmega); + Matrix GexpectedTop6(6, 9); GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; // Compute expected f_rot wrt gyro noise - Matrix dgr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - deltaRij_old, _1, newDeltaT), newMeasuredOmega); + Matrix dgr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), + newMeasuredOmega); - Matrix GexpectedBottom3(3,9); + Matrix GexpectedBottom3(3, 9); GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; - Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3; + Matrix Gexpected(9, 9); + Gexpected << GexpectedTop6, GexpectedBottom3; EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation Matrix9 measurementCovariance; - measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3, - Z_3x3, accNoiseVar*I_3x3, Z_3x3, - Z_3x3, Z_3x3, omegaNoiseVar*I_3x3; + measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar + * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + - (1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); } /* ************************************************************************* */ -TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) -{ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); @@ -635,68 +729,71 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } bool use2ndOrderIntegration = true; // Actual preintegrated values ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs, use2ndOrderIntegration); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement Matrix oldPreintCovariance = preintegrated.preintMeasCov(); Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, - body_P_sensor, Factual, Gactual); + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR F ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected f_pos_vel wrt positions - Matrix dfpv_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + Matrix dfpv_dpos = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaPij_old); // Compute expected f_pos_vel wrt velocities - Matrix dfpv_dvel = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + Matrix dfpv_dvel = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaVij_old); // Compute expected f_pos_vel wrt angles - Matrix dfpv_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix dfpv_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaRij_old); - Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; + Matrix FexpectedTop6(6, 9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; // Compute expected f_rot wrt angles - Matrix dfr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - _1, newMeasuredOmega, newDeltaT), deltaRij_old); + Matrix dfr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), + deltaRij_old); - Matrix FexpectedBottom3(3,9); + Matrix FexpectedBottom3(3, 9); FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; - Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3; + Matrix Fexpected(9, 9); + Fexpected << FexpectedTop6, FexpectedBottom3; EXPECT(assert_equal(Fexpected, Factual)); @@ -704,42 +801,43 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) // COMPUTE NUMERICAL DERIVATIVES FOR G ////////////////////////////////////////////////////////////////////////////////////////////// // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6,3); + Matrix dgpv_dintNoise(6, 3); dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; // Compute jacobian wrt acc noise - Matrix dgpv_daccNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + Matrix dgpv_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // Compute expected F wrt gyro noise - Matrix dgpv_domegaNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); - Matrix GexpectedTop6(6,9); + Matrix dgpv_domegaNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), + newMeasuredOmega); + Matrix GexpectedTop6(6, 9); GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; // Compute expected f_rot wrt gyro noise - Matrix dgr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - deltaRij_old, _1, newDeltaT), newMeasuredOmega); + Matrix dgr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), + newMeasuredOmega); - Matrix GexpectedBottom3(3,9); + Matrix GexpectedBottom3(3, 9); GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; - Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3; + Matrix Gexpected(9, 9); + Gexpected << GexpectedTop6, GexpectedBottom3; EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation Matrix9 measurementCovariance; - measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3, - Z_3x3, accNoiseVar*I_3x3, Z_3x3, - Z_3x3, Z_3x3, omegaNoiseVar*I_3x3; + measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar + * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + - (1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); @@ -801,93 +899,109 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) //} /* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) -{ +TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); + const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), + Point3(1, 0, 0)); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ -TEST(ImuFactor, PredictPositionAndVelocity){ +TEST(ImuFactor, PredictPositionAndVelocity) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,1,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, 0; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 1, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i < 1000; ++i) + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, + omegaCoriolis); + Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + Vector3 expectedVelocity; + expectedVelocity << 0, 1, 0; + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); } /* ************************************************************************* */ @@ -895,34 +1009,46 @@ TEST(ImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,0,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 0, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i < 1000; ++i) + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Predict - Pose3 x1, x2; - Vector3 v1 = Vector3(0, 0.0, 0.0); - Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,0,0; - EXPECT(assert_equal(expectedPose, x2)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); + // Predict + Pose3 x1, x2; + Vector3 v1 = Vector3(0, 0.0, 0.0); + Vector3 v2; + ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), + gravity, omegaCoriolis); + Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + Vector3 expectedVelocity; + expectedVelocity << 0, 0, 0; + EXPECT(assert_equal(expectedPose, x2)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt index 5bb2bd7d2..ad4824817 100644 --- a/gtsam/nonlinear/CMakeLists.txt +++ b/gtsam/nonlinear/CMakeLists.txt @@ -2,5 +2,8 @@ file(GLOB nonlinear_headers "*.h") install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear) +file(GLOB nonlinear_headers_internal "internal/*.h") +install(FILES ${nonlinear_headers_internal} DESTINATION include/gtsam/nonlinear/internal) + # Build tests add_subdirectory(tests) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index a86e7312a..6c6c155c7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,773 +19,244 @@ #pragma once -#include -#include -#include +#include -#include #include -#include -#include - -// template meta-programming headers -#include -namespace MPL = boost::mpl::placeholders; - -#include // operator typeid -#include - -class ExpressionFactorBinaryTest; -// Forward declare for testing +#include +#include namespace gtsam { -const unsigned TraceAlignment = 16; - template -T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { - // right now only word sized types are supported. - // Easy to extend if needed, - // by somehow inferring the unsigned integer of same size - BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); - size_t & uiValue = reinterpret_cast(value); - size_t misAlignment = uiValue % requiredAlignment; - if (misAlignment) { - uiValue += requiredAlignment - misAlignment; - } - return value; -} -template -T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { - return upAlign(value, requiredAlignment); +void Expression::print(const std::string& s) const { + std::cout << s << *root_ << std::endl; } template -class Expression; - -/** - * Expressions are designed to write their derivatives into an already allocated - * Jacobian of the correct size, of type VerticalBlockMatrix. - * The JacobianMap provides a mapping from keys to the underlying blocks. - */ -class JacobianMap { - const FastVector& keys_; - VerticalBlockMatrix& Ab_; -public: - JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : - keys_(keys), Ab_(Ab) { - } - /// Access via key - VerticalBlockMatrix::Block operator()(Key key) { - FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), - key); - DenseIndex block = it - keys_.begin(); - return Ab_(block); - } -}; - -//----------------------------------------------------------------------------- - -namespace internal { - -template -struct UseBlockIf { - static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - // block makes HUGE difference - jacobians(key).block( - 0, 0) += dTdA; - } - ; -}; -/// Handle Leaf Case for Dynamic Matrix type (slower) -template -struct UseBlockIf { - static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - jacobians(key) += dTdA; - } -}; +Expression::Expression(const T& value) : + root_(new internal::ConstantExpression(value)) { } -/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians -template -void handleLeafCase(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - internal::UseBlockIf< - Derived::RowsAtCompileTime != Eigen::Dynamic - && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( - dTdA, jacobians, key); +template +Expression::Expression(const Key& key) : + root_(new internal::LeafExpression(key)) { } -//----------------------------------------------------------------------------- -/** - * The ExecutionTrace class records a tree-structured expression's execution. - * - * The class looks a bit complicated but it is so for performance. - * It is a tagged union that obviates the need to create - * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead - * the key for the leaf is stored in the space normally used to store a - * CallRecord*. Nothing is stored for a Constant. - * - * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: - * Trace(Function) -> - * BinaryRecord with two traces in it - * trace1(Function) -> - * UnaryRecord with one trace in it - * trace1(Function) -> - * BinaryRecord with two traces in it - * trace1(Leaf) - * trace2(Constant) - * trace2(Leaf) - * Hence, there are three Record structs, written to memory by traceExecution - */ -template -class ExecutionTrace { +template +Expression::Expression(const Symbol& symbol) : + root_(new internal::LeafExpression(symbol)) { +} + +template +Expression::Expression(unsigned char c, size_t j) : + root_(new internal::LeafExpression(Symbol(c, j))) { +} + +/// Construct a unary function expression +template +template +Expression::Expression(typename UnaryFunction::type function, + const Expression& expression) : + root_(new internal::UnaryExpression(function, expression)) { +} + +/// Construct a binary function expression +template +template +Expression::Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2) : + root_( + new internal::BinaryExpression(function, expression1, + expression2)) { +} + +/// Construct a ternary function expression +template +template +Expression::Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3) : + root_( + new internal::TernaryExpression(function, expression1, + expression2, expression3)) { +} + +/// Construct a nullary method expression +template +template +Expression::Expression(const Expression& expression, + T (A::*method)(typename MakeOptionalJacobian::type) const) : + root_( + new internal::UnaryExpression(boost::bind(method, _1, _2), + expression)) { +} + +/// Construct a unary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2) : + root_( + new internal::BinaryExpression( + boost::bind(method, _1, _2, _3, _4), expression1, expression2)) { +} + +/// Construct a binary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3) : + root_( + new internal::TernaryExpression( + boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, + expression2, expression3)) { +} + +template +std::set Expression::keys() const { + return root_->keys(); +} + +template +void Expression::dims(std::map& map) const { + root_->dims(map); +} + +template +T Expression::value(const Values& values, + boost::optional&> H) const { + + if (H) { + // Call private version that returns derivatives in H + KeysAndDims pair = keysAndDims(); + return valueAndDerivatives(values, pair.first, pair.second, *H); + } else + // no derivatives needed, just return value + return root_->value(values); +} + +template +const boost::shared_ptr >& Expression::root() const { + return root_; +} + +template +size_t Expression::traceSize() const { + return root_->traceSize(); +} + +// Private methods: + +template +T Expression::valueAndDerivatives(const Values& values, + const FastVector& keys, const FastVector& dims, + std::vector& H) const { + + // H should be pre-allocated + assert(H.size()==keys.size()); + + // Pre-allocate and zero VerticalBlockMatrix static const int Dim = traits::dimension; - enum { - Constant, Leaf, Function - } kind; - union { - Key key; - CallRecord* ptr; - } content; -public: - /// Pointer always starts out as a Constant - ExecutionTrace() : - kind(Constant) { - } - /// Change pointer to a Leaf Record - void setLeaf(Key key) { - kind = Leaf; - content.key = key; - } - /// Take ownership of pointer to a Function Record - void setFunction(CallRecord* record) { - kind = Function; - content.ptr = record; - } - /// Print - void print(const std::string& indent = "") const { - if (kind == Constant) - std::cout << indent << "Constant" << std::endl; - else if (kind == Leaf) - std::cout << indent << "Leaf, key = " << content.key << std::endl; - else if (kind == Function) { - std::cout << indent << "Function" << std::endl; - content.ptr->print(indent + " "); - } - } - /// Return record pointer, quite unsafe, used only for testing - template - boost::optional record() { - if (kind != Function) - return boost::none; - else { - Record* p = dynamic_cast(content.ptr); - return p ? boost::optional(p) : boost::none; - } - } - /** - * *** This is the main entry point for reverse AD, called from Expression *** - * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) - */ - typedef Eigen::Matrix JacobianTT; - void startReverseAD1(JacobianMap& jacobians) const { - if (kind == Leaf) { - // This branch will only be called on trivial Leaf expressions, i.e. Priors - static const JacobianTT I = JacobianTT::Identity(); - handleLeafCase(I, jacobians, content.key); - } else if (kind == Function) - // This is the more typical entry point, starting the AD pipeline - // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. - content.ptr->startReverseAD2(jacobians); - } - // Either add to Jacobians (Leaf) or propagate (Function) - template - void reverseAD1(const Eigen::MatrixBase & dTdA, - JacobianMap& jacobians) const { - if (kind == Leaf) - handleLeafCase(dTdA, jacobians, content.key); - else if (kind == Function) - content.ptr->reverseAD2(dTdA, jacobians); - } + VerticalBlockMatrix Ab(dims, Dim); + Ab.matrix().setZero(); + internal::JacobianMap jacobianMap(keys, Ab); - /// Define type so we can apply it as a meta-function - typedef ExecutionTrace type; -}; + // Call unsafe version + T result = valueAndJacobianMap(values, jacobianMap); -/// Storage type for the execution trace. -/// It enforces the proper alignment in a portable way. -/// Provide a traceSize() sized array of this type to traceExecution as traceStorage. -typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < static_cast(keys.size()); i++) + H[i] = Ab(i); -//----------------------------------------------------------------------------- -/** - * Expression node. The superclass for objects that do the heavy lifting - * An Expression has a pointer to an ExpressionNode underneath - * allowing Expressions to have polymorphic behaviour even though they - * are passed by value. This is the same way boost::function works. - * http://loki-lib.sourceforge.net/html/a00652.html - */ -template -class ExpressionNode { + return result; +} -protected: - - size_t traceSize_; - - /// Constructor, traceSize is size of the execution trace of expression rooted here - ExpressionNode(size_t traceSize = 0) : - traceSize_(traceSize) { - } - -public: - - /// Destructor - virtual ~ExpressionNode() { - } - - /// Streaming - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, - const ExpressionNode& node) { - os << "Expression of type " << typeid(T).name(); - if (node.traceSize_>0) os << ", trace size = " << node.traceSize_; - os << "\n"; - return os; - } - - /// Return keys that play in this expression as a set - virtual std::set keys() const { - std::set keys; - return keys; - } - - /// Return dimensions for each argument, as a map - virtual void dims(std::map& map) const { - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return traceSize_; - } - - /// Return value - virtual T value(const Values& values) const = 0; - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const = 0; -}; - -//----------------------------------------------------------------------------- -/// Constant Expression -template -class ConstantExpression: public ExpressionNode { - - /// The constant value - T constant_; - - /// Constructor with a value, yielding a constant - ConstantExpression(const T& value) : - constant_(value) { - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - return constant_; - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return constant_; - } -}; - - -//----------------------------------------------------------------------------- -/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value template -class LeafExpression : public ExpressionNode { - typedef T value_type; +T Expression::traceExecution(const Values& values, + internal::ExecutionTrace& trace, void* traceStorage) const { + return root_->traceExecution(values, trace, + static_cast(traceStorage)); +} - /// The key into values - Key key_; +template +T Expression::valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const { + // The following piece of code is absolutely crucial for performance. + // We allocate a block of memory on the stack, which can be done at runtime + // with modern C++ compilers. The traceExecution then fills this memory + // with an execution trace, made up entirely of "Record" structs, see + // the FunctionalNode class in expression-inl.h + size_t size = traceSize(); - /// Constructor with a single key - LeafExpression(Key key) : - key_(key) { - } - // todo: do we need a virtual destructor here too? - - friend class Expression ; - -public: - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys; - keys.insert(key_); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - map[key_] = traits::dimension; - } - - /// Return value - virtual T value(const Values& values) const { - return values.at(key_); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - trace.setLeaf(key_); - return values.at(key_); - } - -}; - -//----------------------------------------------------------------------------- -// Below we use the "Class Composition" technique described in the book -// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost -// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. -// to recursively generate a class, that will be the base for function nodes. -// -// The class generated, for three arguments A1, A2, and A3 will be -// -// struct Base1 : Argument, FunctionalBase { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Base2 : Argument, Base1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Base3 : Argument, Base2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct FunctionalNode : Base3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// -// All this magic happens when we generate the Base3 base class of FunctionalNode -// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode -// -// Similarly, the inner Record struct will be -// -// struct Record1 : JacobianTrace, CallRecord::value> { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Record2 : JacobianTrace, Record1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Record3 : JacobianTrace, Record2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct Record : Record3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// - -//----------------------------------------------------------------------------- - -/// meta-function to generate fixed-size JacobianTA type -template -struct Jacobian { - typedef Eigen::Matrix::dimension, - traits::dimension> type; -}; - -/// meta-function to generate JacobianTA optional reference -template -struct MakeOptionalJacobian { - typedef OptionalJacobian::dimension, - traits::dimension> type; -}; - -/** - * Base case for recursive FunctionalNode class - */ -template -struct FunctionalBase: ExpressionNode { - static size_t const N = 0; // number of arguments - - struct Record { - void print(const std::string& indent) const { - } - void startReverseAD4(JacobianMap& jacobians) const { - } - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { - } - }; - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - // base case: does not do anything - } -}; - -/** - * Building block for recursive FunctionalNode class - * The integer argument N is to guarantee a unique type signature, - * so we are guaranteed to be able to extract their values by static cast. - */ -template -struct Argument { - /// Expression that will generate value/derivatives for argument - boost::shared_ptr > expression; -}; - -/** - * Building block for Recursive Record Class - * Records the evaluation of a single argument in a functional expression - */ -template -struct JacobianTrace { - A value; - ExecutionTrace trace; - typename Jacobian::type dTdA; -}; - -// Recursive Definition of Functional ExpressionNode -// The reason we inherit from Argument is because we can then -// case to this unique signature to retrieve the expression at any level -template -struct GenerateFunctionalNode: Argument, Base { - - static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy - typedef Argument This; ///< The storage we have direct access to - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys = Base::keys(); - std::set myKeys = This::expression->keys(); - keys.insert(myKeys.begin(), myKeys.end()); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - Base::dims(map); - This::expression->dims(map); - } - - // Recursive Record Class for Functional Expressions - // The reason we inherit from JacobianTrace is because we can then - // case to this unique signature to retrieve the value/trace at any level - struct Record: JacobianTrace, Base::Record { - - typedef T return_type; - typedef JacobianTrace This; - - /// Print to std::cout - void print(const std::string& indent) const { - Base::Record::print(indent); - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); - } - - /// Start the reverse AD process - void startReverseAD4(JacobianMap& jacobians) const { - Base::Record::startReverseAD4(jacobians); - // This is the crucial point where the size of the AD pipeline is selected. - // One pipeline is started for each argument, but the number of rows in each - // pipeline is the same, namely the dimension of the output argument T. - // For example, if the entire expression is rooted by a binary function - // yielding a 2D result, then the matrix dTdA will have 2 rows. - // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 - // which calls the correctly sized CallRecord::reverseAD3, which in turn - // calls reverseAD4 below. - This::trace.reverseAD1(This::dTdA, jacobians); - } - - /// Given df/dT, multiply in dT/dA and continue reverse AD process - // Cols is always known at compile time - template - void reverseAD4(const SomeMatrix & dFdT, - JacobianMap& jacobians) const { - Base::Record::reverseAD4(dFdT, jacobians); - This::trace.reverseAD1(dFdT * This::dTdA, jacobians); - } - }; - - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - Base::trace(values, record, traceStorage); // recurse - // Write an Expression execution trace in record->trace - // Iff Constant or Leaf, this will not write to traceStorage, only to trace. - // Iff the expression is functional, write all Records in traceStorage buffer - // Return value of type T is recorded in record->value - record->Record::This::value = This::expression->traceExecution(values, - record->Record::This::trace, traceStorage); - // traceStorage is never modified by traceExecution, but if traceExecution has - // written in the buffer, the next caller expects we advance the pointer - traceStorage += This::expression->traceSize(); - } -}; - -/** - * Recursive GenerateFunctionalNode class Generator - */ -template -struct FunctionalNode { - - /// The following typedef generates the recursively defined Base class - typedef typename boost::mpl::fold, - GenerateFunctionalNode >::type Base; - - /** - * The type generated by this meta-function derives from Base - * and adds access functions as well as the crucial [trace] function - */ - struct type: public Base { - - // Argument types and derived, note these are base 0 ! - // These are currently not used - useful for Phoenix in future -#ifdef EXPRESSIONS_PHOENIX - typedef TYPES Arguments; - typedef typename boost::mpl::transform >::type Jacobians; - typedef typename boost::mpl::transform >::type Optionals; + // Windows does not support variable length arrays, so memory must be dynamically + // allocated on Visual Studio. For more information see the issue below + // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio +#ifdef _MSC_VER + internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size]; +#else + internal::ExecutionTraceStorage traceStorage[size]; #endif - /// Reset expression shared pointer - template - void reset(const boost::shared_ptr >& ptr) { - static_cast &>(*this).expression = ptr; - } + internal::ExecutionTrace trace; + T value(this->traceExecution(values, trace, traceStorage)); + trace.startReverseAD1(jacobians); - /// Access Expression shared pointer - template - boost::shared_ptr > expression() const { - return static_cast const &>(*this).expression; - } +#ifdef _MSC_VER + delete[] traceStorage; +#endif - /// Provide convenience access to Record storage and implement - /// the virtual function based interface of CallRecord using the CallRecordImplementor - struct Record: public internal::CallRecordImplementor::dimension>, public Base::Record { - using Base::Record::print; - using Base::Record::startReverseAD4; - using Base::Record::reverseAD4; - - virtual ~Record() { - } - - /// Access Value - template - const A& value() const { - return static_cast const &>(*this).value; - } - - /// Access Jacobian - template - typename Jacobian::type& jacobian() { - return static_cast&>(*this).dTdA; - } - }; - - /// Construct an execution trace for reverse AD - Record* trace(const Values& values, - ExecutionTraceStorage* traceStorage) const { - assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); - - // Create the record and advance the pointer - Record* record = new (traceStorage) Record(); - traceStorage += upAligned(sizeof(Record)); - - // Record the traces for all arguments - // After this, the traceStorage pointer is set to after what was written - Base::trace(values, record, traceStorage); - - // Return the record for this function evaluation - return record; - } - }; -}; -//----------------------------------------------------------------------------- - -/// Unary Function Expression -template -class UnaryExpression: public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - -public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a unary function f, and input argument e - UnaryExpression(Function f, const Expression& e1) : - function_(f) { - this->template reset(e1.root()); - ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - return function_(this->template expression()->value(values), boost::none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_(record->template value(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Binary Expression - -template -class BinaryExpression: - public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; - -public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a ternary function f, and three input arguments - BinaryExpression(Function f, const Expression& e1, - const Expression& e2) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); - } - - friend class Expression ; - friend class ::ExpressionFactorBinaryTest; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_(record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Ternary Expression - -template -class TernaryExpression: - public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; - typedef typename MakeOptionalJacobian::type OJ3; - -public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a ternary function f, and three input arguments - TernaryExpression(Function f, const Expression& e1, - const Expression& e2, const Expression& e3) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - this->template reset(e3.root()); - ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() - + e3.traceSize(); - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - this->template expression()->value(values), - none, none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_( - record->template value(), record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian(), record->template jacobian()); - } - -}; -//----------------------------------------------------------------------------- + return value; } + +template +typename Expression::KeysAndDims Expression::keysAndDims() const { + std::map map; + dims(map); + size_t n = map.size(); + KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); + boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); + boost::copy(map | boost::adaptors::map_values, pair.second.begin()); + return pair; +} + +namespace internal { +// http://stackoverflow.com/questions/16260445/boost-bind-to-operator +template +struct apply_compose { + typedef T result_type; + static const int Dim = traits::dimension; + T operator()(const T& x, const T& y, OptionalJacobian H1 = + boost::none, OptionalJacobian H2 = boost::none) const { + return x.compose(y, H1, H2); + } +}; +} + +// Global methods: + +/// Construct a product expression, assumes T::compose(T) -> T +template +Expression operator*(const Expression& expression1, + const Expression& expression2) { + return Expression( + boost::bind(internal::apply_compose(), _1, _2, _3, _4), expression1, + expression2); +} + +/// Construct an array of leaves +template +std::vector > createUnknowns(size_t n, char c, size_t start) { + std::vector > unknowns; + unknowns.reserve(n); + for (size_t i = start; i < start + n; i++) + unknowns.push_back(Expression(c, i)); + return unknowns; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index a39b1557c..d24a568f5 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -19,21 +19,28 @@ #pragma once -#include +#include #include -#include +#include #include -#include -#include +#include +#include +// Forward declare tests class ExpressionFactorShallowTest; namespace gtsam { -// Forward declare +// Forward declares +class Values; template class ExpressionFactor; +namespace internal { +template class ExecutionTrace; +template class ExpressionNode; +} + /** * Expression class that supports automatic differentiation */ @@ -48,110 +55,97 @@ public: private: // Paul's trick shared pointer, polymorphic root of entire expression tree - boost::shared_ptr > root_; + boost::shared_ptr > root_; public: + // Expressions wrap trees of functions that can evaluate their own derivatives. + // The meta-functions below are useful to specify the type of those functions. + // Example, a function taking a camera and a 3D point and yielding a 2D point: + // Expression::BinaryFunction::type + template + struct UnaryFunction { + typedef boost::function< + T(const A1&, typename MakeOptionalJacobian::type)> type; + }; + + template + struct BinaryFunction { + typedef boost::function< + T(const A1&, const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> type; + }; + + template + struct TernaryFunction { + typedef boost::function< + T(const A1&, const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> type; + }; + /// Print - void print(const std::string& s) const { - std::cout << s << *root_ << std::endl; - } + void print(const std::string& s) const; - // Construct a constant expression - Expression(const T& value) : - root_(new ConstantExpression(value)) { - } + /// Construct a constant expression + Expression(const T& value); - // Construct a leaf expression, with Key - Expression(const Key& key) : - root_(new LeafExpression(key)) { - } + /// Construct a leaf expression, with Key + Expression(const Key& key); - // Construct a leaf expression, with Symbol - Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)) { - } + /// Construct a leaf expression, with Symbol + Expression(const Symbol& symbol); - // Construct a leaf expression, creating Symbol - Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { - } + /// Construct a leaf expression, creating Symbol + Expression(unsigned char c, size_t j); + + /// Construct a unary function expression + template + Expression(typename UnaryFunction::type function, + const Expression& expression); + + /// Construct a binary function expression + template + Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2); + + /// Construct a ternary function expression + template + Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3); /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename MakeOptionalJacobian::type) const) : - root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { - } - - /// Construct a unary function expression - template - Expression(typename UnaryExpression::Function function, - const Expression& expression) : - root_(new UnaryExpression(function, expression)) { - } + T (A::*method)(typename MakeOptionalJacobian::type) const); /// Construct a unary method expression template Expression(const Expression& expression1, T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type) const, - const Expression& expression2) : - root_( - new BinaryExpression(boost::bind(method, _1, _2, _3, _4), - expression1, expression2)) { - } - - /// Construct a binary function expression - template - Expression(typename BinaryExpression::Function function, - const Expression& expression1, const Expression& expression2) : - root_(new BinaryExpression(function, expression1, expression2)) { - } + const Expression& expression2); /// Construct a binary method expression template Expression(const Expression& expression1, T (A1::*method)(const A2&, const A3&, - typename TernaryExpression::OJ1, - typename TernaryExpression::OJ2, - typename TernaryExpression::OJ3) const, - const Expression& expression2, const Expression& expression3) : - root_( - new TernaryExpression( - boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, - expression2, expression3)) { - } + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3); - /// Construct a ternary function expression - template - Expression(typename TernaryExpression::Function function, - const Expression& expression1, const Expression& expression2, - const Expression& expression3) : - root_( - new TernaryExpression(function, expression1, - expression2, expression3)) { - } - - /// Return root - const boost::shared_ptr >& root() const { - return root_; - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return root_->traceSize(); + /// Destructor + virtual ~Expression() { } /// Return keys that play in this expression - std::set keys() const { - return root_->keys(); - } + std::set keys() const; /// Return dimensions for each argument, as a map - void dims(std::map& map) const { - root_->dims(map); - } + void dims(std::map& map) const; /** * @brief Return value and optional derivatives, reverse AD version @@ -159,16 +153,7 @@ public: * The order of the Jacobians is same as keys in either keys() or dims() */ T value(const Values& values, boost::optional&> H = - boost::none) const { - - if (H) { - // Call private version that returns derivatives in H - KeysAndDims pair = keysAndDims(); - return value(values, pair.first, pair.second, *H); - } else - // no derivatives needed, just return value - return root_->value(values); - } + boost::none) const; /** * @return a "deep" copy of this Expression @@ -179,116 +164,55 @@ public: return boost::make_shared(*this); } + /// Return root + const boost::shared_ptr >& root() const; + + /// Return size needed for memory buffer in traceExecution + size_t traceSize() const; + private: - /// Vaguely unsafe keys and dimensions in same order + /// Keys and dimensions in same order typedef std::pair, FastVector > KeysAndDims; - KeysAndDims keysAndDims() const { - std::map map; - dims(map); - size_t n = map.size(); - KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); - boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); - boost::copy(map | boost::adaptors::map_values, pair.second.begin()); - return pair; - } + KeysAndDims keysAndDims() const; /// private version that takes keys and dimensions, returns derivatives - T value(const Values& values, const FastVector& keys, - const FastVector& dims, std::vector& H) const { - - // H should be pre-allocated - assert(H.size()==keys.size()); - - // Pre-allocate and zero VerticalBlockMatrix - static const int Dim = traits::dimension; - VerticalBlockMatrix Ab(dims, Dim); - Ab.matrix().setZero(); - JacobianMap jacobianMap(keys, Ab); - - // Call unsafe version - T result = value(values, jacobianMap); - - // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < static_cast(keys.size()); i++) - H[i] = Ab(i); - - return result; - } + T valueAndDerivatives(const Values& values, const FastVector& keys, + const FastVector& dims, std::vector& H) const; /// trace execution, very unsafe - T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return root_->traceExecution(values, trace, traceStorage); - } + T traceExecution(const Values& values, internal::ExecutionTrace& trace, + void* traceStorage) const; - /** - * @brief Return value and derivatives, reverse AD version - * This very unsafe method needs a JacobianMap with correctly allocated - * and initialized VerticalBlockMatrix, hence is declared private. - */ - T value(const Values& values, JacobianMap& jacobians) const { - // The following piece of code is absolutely crucial for performance. - // We allocate a block of memory on the stack, which can be done at runtime - // with modern C++ compilers. The traceExecution then fills this memory - // with an execution trace, made up entirely of "Record" structs, see - // the FunctionalNode class in expression-inl.h - size_t size = traceSize(); - - // Windows does not support variable length arrays, so memory must be dynamically - // allocated on Visual Studio. For more information see the issue below - // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio -#ifdef _MSC_VER - ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size]; -#else - ExecutionTraceStorage traceStorage[size]; -#endif - - ExecutionTrace trace; - T value(traceExecution(values, trace, traceStorage)); - trace.startReverseAD1(jacobians); - -#ifdef _MSC_VER - delete[] traceStorage; -#endif - - return value; - } + /// brief Return value and derivatives, reverse AD version + T valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const; // be very selective on who can access these private methods: friend class ExpressionFactor ; + friend class internal::ExpressionNode; + + // and add tests friend class ::ExpressionFactorShallowTest; - }; -// http://stackoverflow.com/questions/16260445/boost-bind-to-operator -template -struct apply_compose { - typedef T result_type; - static const int Dim = traits::dimension; - T operator()(const T& x, const T& y, OptionalJacobian H1 = - boost::none, OptionalJacobian H2 = boost::none) const { - return x.compose(y, H1, H2); - } -}; - -/// Construct a product expression, assumes T::compose(T) -> T +/** + * Construct a product expression, assumes T::compose(T) -> T + * Example: + * Expression a(0), b(1), c = a*b; + */ template -Expression operator*(const Expression& expression1, - const Expression& expression2) { - return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), - expression1, expression2); -} +Expression operator*(const Expression& e1, const Expression& e2); -/// Construct an array of leaves +/** + * Construct an array of unknown expressions with successive symbol keys + * Example: + * createUnknowns(3,'x') creates unknown expressions for x0,x1,x2 + */ template -std::vector > createUnknowns(size_t n, char c, size_t start = 0) { - std::vector > unknowns; - unknowns.reserve(n); - for (size_t i = start; i < start + n; i++) - unknowns.push_back(Expression(c, i)); - return unknowns; -} +std::vector > createUnknowns(size_t n, char c, size_t start = 0); -} +} // namespace gtsam + +#include diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 6440afbef..b32b84df3 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -32,7 +32,9 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { -protected: + protected: + + typedef ExpressionFactor This; T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled @@ -40,7 +42,9 @@ protected: static const int Dim = traits::dimension; -public: + public: + + typedef boost::shared_ptr > shared_ptr; /// Constructor ExpressionFactor(const SharedNoiseModel& noiseModel, // @@ -65,24 +69,21 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if (H) { - const T hx = expression_.value(x, keys_, dims_, *H); // h(x) - return traits::Local(measurement_, hx); // h(x) - z + if (H) { + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); + return traits::Local(measurement_, value); } else { - const T hx = expression_.value(x); // h(x) - return traits::Local(measurement_, hx); // h(x) - z + const T value = expression_.value(x); + return traits::Local(measurement_, value); } } - /** - * Linearize the factor into a JacobianFactor - */ - virtual boost::shared_ptr linearize(const Values& x_bar) const { + virtual boost::shared_ptr linearize(const Values& x) const { // Only linearize if the factor is active - if (!active(x_bar)) + if (!active(x)) return boost::shared_ptr(); - // Create a writable JacobianFactor in advance + // Create a writeable JacobianFactor in advance // In case noise model is constrained, we need to provide a noise model bool constrained = noiseModel_->isConstrained(); boost::shared_ptr factor( @@ -91,19 +92,17 @@ public: new JacobianFactor(keys_, dims_, Dim)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ - VerticalBlockMatrix& Ab = factor->matrixObject(); // reference, no malloc ! - JacobianMap jacobianMap(keys_, Ab); + VerticalBlockMatrix& Ab = factor->matrixObject(); + internal::JacobianMap jacobianMap(keys_, Ab); // Zero out Jacobian so we can simply add to it Ab.matrix().setZero(); // Get value and Jacobians, writing directly into JacobianFactor - T hx = expression_.value(x_bar, jacobianMap); // <<< Reverse AD happens here ! + T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here ! - // Evaluate error and set RHS vector b = - (h(x_bar) - z) = z-h(x_bar) - // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z| - // = |A*dx - (z-h(x_bar))| - Ab(size()).col(0) = - traits::Local(measurement_, hx); // - unwhitenedError(x_bar) + // Evaluate error and set RHS vector b + Ab(size()).col(0) = -traits::Local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models @@ -111,8 +110,13 @@ public: return factor; } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } }; // ExpressionFactor -} // \ namespace gtsam +}// \ namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 3b3d76285..00ffdccef 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -98,7 +98,7 @@ DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationMode } /* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) const { +ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) { std::string s = str; boost::algorithm::to_upper(s); if (s == "QR") return ISAM2Params::QR; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; @@ -108,7 +108,7 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::strin } /* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) const { +std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) { std::string s; switch (value) { case ISAM2Params::QR: s = "QR"; break; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 9adceee6a..5f5554e91 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -186,6 +186,7 @@ struct GTSAM_EXPORT ISAM2Params { enableDetailedResults(false), enablePartialRelinearizationCheck(false), findUnusedFactorSlots(false) {} + /// print iSAM2 parameters void print(const std::string& str = "") const { std::cout << str << "\n"; if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) @@ -214,7 +215,9 @@ struct GTSAM_EXPORT ISAM2Params { std::cout.flush(); } - /** Getters and Setters for all properties */ + /// @name Getters and Setters for all properties + /// @{ + OptimizationParams getOptimizationParams() const { return this->optimizationParams; } RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; } int getRelinearizeSkip() const { return relinearizeSkip; } @@ -237,14 +240,21 @@ struct GTSAM_EXPORT ISAM2Params { void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; } void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; } - Factorization factorizationTranslator(const std::string& str) const; - std::string factorizationTranslator(const Factorization& value) const; - GaussianFactorGraph::Eliminate getEliminationFunction() const { return factorization == CHOLESKY ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky : (GaussianFactorGraph::Eliminate)EliminateQR; } + + /// @} + + /// @name Some utilities + /// @{ + + static Factorization factorizationTranslator(const std::string& str); + static std::string factorizationTranslator(const Factorization& value); + + /// @} }; @@ -404,7 +414,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(cachedFactor_); ar & BOOST_SERIALIZATION_NVP(gradientContribution_); @@ -544,8 +554,15 @@ public: boost::optional&> marginalFactorsIndices = boost::none, boost::optional&> deletedFactorsIndices = boost::none); - /** Access the current linearization point */ - const Values& getLinearizationPoint() const { return theta_; } + /// Access the current linearization point + const Values& getLinearizationPoint() const { + return theta_; + } + + /// Check whether variable with given key exists in linearization point + bool valueExists(Key key) const { + return theta_.exists(key); + } /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 2f8fcfcee..5fb51a243 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -25,9 +25,10 @@ #include #include +#include +#include #include #include -#include using namespace std; @@ -178,7 +179,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); damped += boost::make_shared(key_vector.first, A, b, model); - } catch (std::exception e) { + } catch (const std::exception& e) { // Don't attempt any damping if no key found in diagonal continue; } @@ -247,7 +248,7 @@ void LevenbergMarquardtOptimizer::iterate() { double modelFidelity = 0.0; bool step_is_successful = false; bool stopSearchingLambda = false; - double newError; + double newError = numeric_limits::infinity(); Values newValues; VectorValues delta; @@ -255,7 +256,7 @@ void LevenbergMarquardtOptimizer::iterate() { try { delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - } catch (IndeterminantLinearSystemException) { + } catch (const IndeterminantLinearSystemException& e) { systemSolvedSuccessfully = false; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 009b480f2..e5561af48 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -54,65 +54,45 @@ public: double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] bool diagonalDamping; ///< if true, use diagonal of Hessian - bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (related to efficiency) + bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (TODO: should be in state?) bool useFixedLambdaFactor_; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor double min_diagonal_; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) - LevenbergMarquardtParams() : - lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound( - 0.0), verbosityLM(SILENT), minModelFidelity(1e-3), - diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true), - min_diagonal_(1e-6), max_diagonal_(1e32) { - } - virtual ~LevenbergMarquardtParams() { - } + LevenbergMarquardtParams() + : lambdaInitial(1e-5), + lambdaFactor(10.0), + lambdaUpperBound(1e5), + lambdaLowerBound(0.0), + verbosityLM(SILENT), + minModelFidelity(1e-3), + diagonalDamping(false), + reuse_diagonal_(false), + useFixedLambdaFactor_(true), + min_diagonal_(1e-6), + max_diagonal_(1e32) {} + virtual ~LevenbergMarquardtParams() {} virtual void print(const std::string& str = "") const; - - inline double getlambdaInitial() const { - return lambdaInitial; - } - inline double getlambdaFactor() const { - return lambdaFactor; - } - inline double getlambdaUpperBound() const { - return lambdaUpperBound; - } - inline double getlambdaLowerBound() const { - return lambdaLowerBound; - } + inline double getlambdaInitial() const { return lambdaInitial; } + inline double getlambdaFactor() const { return lambdaFactor; } + inline double getlambdaUpperBound() const { return lambdaUpperBound; } + inline double getlambdaLowerBound() const { return lambdaLowerBound; } inline std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM); } - inline std::string getLogFile() const { - return logFile; - } - inline bool getDiagonalDamping() const { - return diagonalDamping; - } + inline std::string getLogFile() const { return logFile; } + inline bool getDiagonalDamping() const { return diagonalDamping; } - inline void setlambdaInitial(double value) { - lambdaInitial = value; - } - inline void setlambdaFactor(double value) { - lambdaFactor = value; - } - inline void setlambdaUpperBound(double value) { - lambdaUpperBound = value; - } - inline void setlambdaLowerBound(double value) { - lambdaLowerBound = value; - } - inline void setVerbosityLM(const std::string &s) { + inline void setlambdaInitial(double value) { lambdaInitial = value; } + inline void setlambdaFactor(double value) { lambdaFactor = value; } + inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; } + inline void setlambdaLowerBound(double value) { lambdaLowerBound = value; } + inline void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s); } - inline void setLogFile(const std::string &s) { - logFile = s; - } - inline void setDiagonalDamping(bool flag) { - diagonalDamping = flag; - } + inline void setLogFile(const std::string& s) { logFile = s; } + inline void setDiagonalDamping(bool flag) { diagonalDamping = flag; } inline void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor_ = flag; } diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 676b3fb85..3ae8d4c98 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -146,7 +146,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(factor_); diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 6e348fb58..86fb34fe0 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -21,21 +21,14 @@ #include #include +#include + #include #include #include namespace gtsam { -/** - * Template default compare function that assumes a testable T - */ -template -bool compare(const T& a, const T& b) { - GTSAM_CONCEPT_TESTABLE_TYPE(T); - return a.equals(b); -} - /** * An equality factor that forces either one variable to a constant, * or a set of variables to be equal to each other. @@ -76,7 +69,9 @@ public: /** * Function that compares two values */ - bool (*compare_)(const T& a, const T& b); + typedef boost::function CompareFunction; + CompareFunction compare_; +// bool (*compare_)(const T& a, const T& b); /** default constructor - only for serialization */ NonlinearEquality() { @@ -92,7 +87,7 @@ public: * Constructor - forces exact evaluation */ NonlinearEquality(Key j, const T& feasible, - bool (*_compare)(const T&, const T&) = compare) : + const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { @@ -102,7 +97,7 @@ public: * Constructor - allows inexact evaluation */ NonlinearEquality(Key j, const T& feasible, double error_gain, - bool (*_compare)(const T&, const T&) = compare) : + const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { @@ -122,7 +117,7 @@ public: /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This* e = dynamic_cast(&f); - return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) + return e && Base::equals(f) && traits::Equals(feasible_,e->feasible_, tol) && std::abs(error_gain_ - e->error_gain_) < tol; } @@ -185,7 +180,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -273,7 +268,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -337,7 +332,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 5d9f176b3..5b46742e7 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -112,7 +112,7 @@ public: * when the constraint is *NOT* fulfilled. * @return true if the constraint is active */ - virtual bool active(const Values& c) const { return true; } + virtual bool active(const Values& /*c*/) const { return true; } /** linearize to a GaussianFactor */ virtual boost::shared_ptr @@ -247,7 +247,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(noiseModel_); @@ -325,7 +325,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -401,7 +401,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -478,7 +478,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -559,7 +559,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -644,7 +644,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -733,7 +733,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 238d3bc56..f23418d2a 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -47,11 +47,12 @@ double NonlinearFactorGraph::probPrime(const Values& c) const { /* ************************************************************************* */ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const { - cout << str << "size: " << size() << endl; + cout << str << "size: " << size() << endl << endl; for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; - ss << "factor " << i << ": "; + ss << "Factor " << i << ": "; if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); + cout << endl; } } @@ -62,12 +63,12 @@ bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol) /* ************************************************************************* */ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, - const GraphvizFormatting& graphvizFormatting, + const GraphvizFormatting& formatting, const KeyFormatter& keyFormatter) const { stm << "graph {\n"; - stm << " size=\"" << graphvizFormatting.figureWidthInches << "," << - graphvizFormatting.figureHeightInches << "\";\n\n"; + stm << " size=\"" << formatting.figureWidthInches << "," << + formatting.figureHeightInches << "\";\n\n"; FastSet keys = this->keys(); @@ -75,72 +76,38 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, struct { boost::optional operator()( const Value& value, const GraphvizFormatting& graphvizFormatting) { - if(const Pose2* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = 0.0; break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = 0.0; break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = 0.0; break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = 0.0; break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); - } else if(const Pose3* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = p->z(); break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = p->z(); break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); - } else if(const Point3* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = p->z(); break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = p->z(); break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); + Vector3 t; + if (const GenericValue* p = dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t = p->value().translation().vector(); + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t = p->value().vector(); } else { return boost::none; } + double x, y; + switch (graphvizFormatting.paperHorizontalAxis) { + case GraphvizFormatting::X: x = t.x(); break; + case GraphvizFormatting::Y: x = t.y(); break; + case GraphvizFormatting::Z: x = t.z(); break; + case GraphvizFormatting::NEGX: x = -t.x(); break; + case GraphvizFormatting::NEGY: x = -t.y(); break; + case GraphvizFormatting::NEGZ: x = -t.z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + switch (graphvizFormatting.paperVerticalAxis) { + case GraphvizFormatting::X: y = t.x(); break; + case GraphvizFormatting::Y: y = t.y(); break; + case GraphvizFormatting::Z: y = t.z(); break; + case GraphvizFormatting::NEGX: y = -t.x(); break; + case GraphvizFormatting::NEGY: y = -t.y(); break; + case GraphvizFormatting::NEGZ: y = -t.z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + return Point2(x,y); }} getXY; // Find bounds @@ -148,7 +115,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, double minY = numeric_limits::infinity(), maxY = -numeric_limits::infinity(); BOOST_FOREACH(Key key, keys) { if(values.exists(key)) { - boost::optional xy = getXY(values.at(key), graphvizFormatting); + boost::optional xy = getXY(values.at(key), formatting); if(xy) { if(xy->x() < minX) minX = xy->x(); @@ -163,33 +130,22 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } // Create nodes for each variable in the graph - bool firstTimePoses = true; - Key lastKey; - BOOST_FOREACH(Key key, keys) { + BOOST_FOREACH(Key key, keys){ // Label the node with the label from the KeyFormatter stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if(values.exists(key)) { - boost::optional xy = getXY(values.at(key), graphvizFormatting); + boost::optional xy = getXY(values.at(key), formatting); if(xy) - stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\""; + stm << ", pos=\"" << formatting.scale*(xy->x() - minX) << "," << formatting.scale*(xy->y() - minY) << "!\""; } stm << "];\n"; - - if (firstTimePoses) { - lastKey = key; - firstTimePoses = false; - } else { - stm << " var" << key << "--" << "var" << lastKey << ";\n"; - lastKey = key; - } } stm << "\n"; - - if(graphvizFormatting.mergeSimilarFactors) { + if (formatting.mergeSimilarFactors) { // Remove duplicate factors FastSet > structure; - BOOST_FOREACH(const sharedFactor& factor, *this) { + BOOST_FOREACH(const sharedFactor& factor, *this){ if(factor) { vector factorKeys = factor->keys(); std::sort(factorKeys.begin(), factorKeys.end()); @@ -199,57 +155,65 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections size_t i = 0; - BOOST_FOREACH(const vector& factorKeys, structure) { + BOOST_FOREACH(const vector& factorKeys, structure){ // Make each factor a dot stm << " factor" << i << "[label=\"\", shape=point"; { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; + map::const_iterator pos = formatting.factorPositions.find(i); + if(pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," + << formatting.scale*(pos->second.y() - minY) << "!\""; } stm << "];\n"; // Make factor-variable connections BOOST_FOREACH(Key key, factorKeys) { - stm << " var" << key << "--" << "factor" << i << ";\n"; } + stm << " var" << key << "--" << "factor" << i << ";\n"; + } ++ i; } } else { // Create factors and variable connections for(size_t i = 0; i < this->size(); ++i) { - if(graphvizFormatting.plotFactorPoints){ - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point"; - { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; - } - stm << "];\n"; + const NonlinearFactor::shared_ptr& factor = this->at(i); + if(formatting.plotFactorPoints) { + const FastVector& keys = factor->keys(); + if (formatting.binaryEdges && keys.size()==2) { + stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n"; + } else { + // Make each factor a dot + stm << " factor" << i << "[label=\"\", shape=point"; + { + map::const_iterator pos = formatting.factorPositions.find(i); + if(pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," + << formatting.scale*(pos->second.y() - minY) << "!\""; + } + stm << "];\n"; - // Make factor-variable connections - if(graphvizFormatting.connectKeysToFactor && this->at(i)) { - BOOST_FOREACH(Key key, *this->at(i)) { - stm << " var" << key << "--" << "factor" << i << ";\n"; + // Make factor-variable connections + if(formatting.connectKeysToFactor && factor) { + BOOST_FOREACH(Key key, *factor) { + stm << " var" << key << "--" << "factor" << i << ";\n"; + } + } + } } - } - - } else { - if(this->at(i)) { - Key k; - bool firstTime = true; - BOOST_FOREACH(Key key, *this->at(i)) { - if(firstTime){ - k = key; - firstTime = false; - continue; + if(factor) { + Key k; + bool firstTime = true; + BOOST_FOREACH(Key key, *this->at(i)) { + if(firstTime) { + k = key; + firstTime = false; + continue; + } + stm << " var" << key << "--" << "var" << k << ";\n"; + k = key; + } } - stm << " var" << key << "--" << "var" << k << ";\n"; - k = key; - } - } } } } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 28f72d57d..169d12455 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -32,6 +32,10 @@ namespace gtsam { class Ordering; class GaussianFactorGraph; class SymbolicFactorGraph; + template + class Expression; + template + class ExpressionFactor; /** * Formatting options when saving in GraphViz format using @@ -47,6 +51,7 @@ namespace gtsam { bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity bool plotFactorPoints; ///< Plots each factor as a dot between the variables bool connectKeysToFactor; ///< Draw a line from each key within a factor to the dot of the factor + bool binaryEdges; ///< just use non-dotted edges for binary factors std::map factorPositions; ///< (optional for each factor) Manually specify factor "dot" positions. /// Default constructor sets up robot coordinates. Paper horizontal is robot Y, /// paper vertical is robot X. Default figure size of 5x5 in. @@ -54,7 +59,7 @@ namespace gtsam { paperHorizontalAxis(Y), paperVerticalAxis(X), figureWidthInches(5), figureHeightInches(5), scale(1), mergeSimilarFactors(false), plotFactorPoints(true), - connectKeysToFactor(true) {} + connectKeysToFactor(true), binaryEdges(true) {} }; @@ -150,12 +155,24 @@ namespace gtsam { */ NonlinearFactorGraph rekey(const std::map& rekey_mapping) const; + /** + * Directly add ExpressionFactor that implements |h(x)-z|^2_R + * @param h expression that implements measurement function + * @param z measurement + * @param R model + */ + template + void addExpressionFactor(const SharedNoiseModel& R, const T& z, + const Expression& h) { + push_back(boost::make_shared >(R, z, h)); + } + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactorGraph", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 00746d9b7..77d26d361 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -68,6 +68,7 @@ void NonlinearOptimizer::defaultOptimize() { // Do next iteration currentError = this->error(); this->iterate(); + tictoc_finishedIteration(); // Maybe show output if(params.verbosity >= NonlinearOptimizerParams::VALUES) this->values().print("newValues"); diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index ad727f45e..4eb89b084 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -398,7 +398,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(values_); } diff --git a/gtsam/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h index ab6703f3a..47f61b8b1 100644 --- a/gtsam/nonlinear/expressionTesting.h +++ b/gtsam/nonlinear/expressionTesting.h @@ -21,9 +21,9 @@ #include #include +#include #include #include -#include #include #include @@ -32,47 +32,6 @@ namespace gtsam { -/** - * Linearize a nonlinear factor using numerical differentiation - * The benefit of this method is that it does not need to know what types are - * involved to evaluate the factor. If all the machinery of gtsam is working - * correctly, we should get the correct numerical derivatives out the other side. - */ -JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, - const Values& values, double delta = 1e-5) { - - // We will fill a map of Jacobians - std::map jacobians; - - // Get size - Eigen::VectorXd e = factor.whitenedError(values); - const size_t rows = e.size(); - - // Loop over all variables - VectorValues dX = values.zeroVectors(); - BOOST_FOREACH(Key key, factor) { - // Compute central differences using the values struct. - const size_t cols = dX.dim(key); - Matrix J = Matrix::Zero(rows, cols); - for (size_t col = 0; col < cols; ++col) { - Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = delta; - dX[key] = dx; - Values eval_values = values.retract(dX); - Eigen::VectorXd left = factor.whitenedError(eval_values); - dx[col] = -delta; - dX[key] = dx; - eval_values = values.retract(dX); - Eigen::VectorXd right = factor.whitenedError(eval_values); - J.col(col) = (left - right) * (1.0 / (2.0 * delta)); - } - jacobians[key] = J; - } - - // Next step...return JacobianFactor - return JacobianFactor(jacobians, -e); -} - namespace internal { // CPPUnitLite-style test for linearization of a factor void testFactorJacobians(TestResult& result_, const std::string& name_, @@ -88,7 +47,7 @@ void testFactorJacobians(TestResult& result_, const std::string& name_, // Check cast result and then equality CHECK(actual); - EXPECT( assert_equal(expected, *actual, tolerance)); + EXPECT(assert_equal(expected, *actual, tolerance)); } } @@ -112,7 +71,7 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_, expression.value(values), expression); testFactorJacobians(result_, name_, f, values, nd_step, tolerance); } -} +} // namespace internal } // namespace gtsam /// \brief Check the Jacobians produced by an expression against finite differences. diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index a549517e5..2f46d6d74 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -18,6 +18,12 @@ Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } +// Generics +template +Expression compose(const Expression& t1, const Expression& t2) { + return Expression(traits::Compose, t1, t2); +} + typedef Expression double_; typedef Expression Vector3_; diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h new file mode 100644 index 000000000..442b29382 --- /dev/null +++ b/gtsam/nonlinear/factorTesting.h @@ -0,0 +1,68 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file factorTesting.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Evaluate derivatives of a nonlinear factor numerically + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Linearize a nonlinear factor using numerical differentiation + * The benefit of this method is that it does not need to know what types are + * involved to evaluate the factor. If all the machinery of gtsam is working + * correctly, we should get the correct numerical derivatives out the other side. + */ +JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, double delta = 1e-5) { + + // We will fill a vector of key/Jacobians pairs (a map would sort) + std::vector > jacobians; + + // Get size + Eigen::VectorXd e = factor.whitenedError(values); + const size_t rows = e.size(); + + // Loop over all variables + VectorValues dX = values.zeroVectors(); + BOOST_FOREACH(Key key, factor) { + // Compute central differences using the values struct. + const size_t cols = dX.dim(key); + Matrix J = Matrix::Zero(rows, cols); + for (size_t col = 0; col < cols; ++col) { + Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + dx[col] = delta; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = factor.whitenedError(eval_values); + dx[col] = -delta; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = factor.whitenedError(eval_values); + J.col(col) = (left - right) * (1.0 / (2.0 * delta)); + } + jacobians.push_back(std::make_pair(key,J)); + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h similarity index 92% rename from gtsam/nonlinear/CallRecord.h rename to gtsam/nonlinear/internal/CallRecord.h index 159a841e5..90aa8a8be 100644 --- a/gtsam/nonlinear/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -20,18 +20,9 @@ #pragma once -#include -#include -#include - -#include +#include namespace gtsam { - -class JacobianMap; -// forward declaration - -//----------------------------------------------------------------------------- namespace internal { /** @@ -64,8 +55,6 @@ struct ConvertToVirtualFunctionSupportedMatrixType { } }; -} // namespace internal - /** * The CallRecord is an abstract base class for the any class that stores * the Jacobians of applying a function with respect to each of its arguments, @@ -94,9 +83,8 @@ struct CallRecord { inline void reverseAD2(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { _reverseAD3( - internal::ConvertToVirtualFunctionSupportedMatrixType< - (Derived::RowsAtCompileTime > 5)>::convert(dFdT), - jacobians); + ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); } // This overload supports matrices with both rows and columns dynamically sized. @@ -140,7 +128,6 @@ private: */ const int CallRecordMaxVirtualStaticRows = 5; -namespace internal { /** * The CallRecordImplementor implements the CallRecord interface for a Derived class by * delegating to its corresponding (templated) non-virtual methods. @@ -193,5 +180,4 @@ private: }; } // namespace internal - } // gtsam diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h new file mode 100644 index 000000000..37ce4dfd5 --- /dev/null +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -0,0 +1,166 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExecutionTrace.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief Execution trace for expressions + */ + +#pragma once + +#include +#include +#include + +#include + +#include + +namespace gtsam { +namespace internal { + +template struct CallRecord; + +/// Storage type for the execution trace. +/// It enforces the proper alignment in a portable way. +/// Provide a traceSize() sized array of this type to traceExecution as traceStorage. +static const unsigned TraceAlignment = 16; +typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + // block makes HUGE difference + jacobians(key).block( + 0, 0) += dTdA; + } +}; + +/// Handle Leaf Case for Dynamic Matrix type (slower) +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + jacobians(key) += dTdA; + } +}; + +/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians +template +void handleLeafCase(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + UseBlockIf< + Derived::RowsAtCompileTime != Eigen::Dynamic + && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( + dTdA, jacobians, key); +} + +/** + * The ExecutionTrace class records a tree-structured expression's execution. + * + * The class looks a bit complicated but it is so for performance. + * It is a tagged union that obviates the need to create + * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead + * the key for the leaf is stored in the space normally used to store a + * CallRecord*. Nothing is stored for a Constant. + * + * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: + * Trace(Function) -> + * BinaryRecord with two traces in it + * trace1(Function) -> + * UnaryRecord with one trace in it + * trace1(Function) -> + * BinaryRecord with two traces in it + * trace1(Leaf) + * trace2(Constant) + * trace2(Leaf) + * Hence, there are three Record structs, written to memory by traceExecution + */ +template +class ExecutionTrace { + static const int Dim = traits::dimension; + enum { + Constant, Leaf, Function + } kind; + union { + Key key; + CallRecord* ptr; + } content; +public: + /// Pointer always starts out as a Constant + ExecutionTrace() : + kind(Constant) { + } + /// Change pointer to a Leaf Record + void setLeaf(Key key) { + kind = Leaf; + content.key = key; + } + /// Take ownership of pointer to a Function Record + void setFunction(CallRecord* record) { + kind = Function; + content.ptr = record; + } + /// Print + void print(const std::string& indent = "") const { + if (kind == Constant) + std::cout << indent << "Constant" << std::endl; + else if (kind == Leaf) + std::cout << indent << "Leaf, key = " << content.key << std::endl; + else if (kind == Function) { + std::cout << indent << "Function" << std::endl; + content.ptr->print(indent + " "); + } + } + /// Return record pointer, quite unsafe, used only for testing + template + boost::optional record() { + if (kind != Function) + return boost::none; + else { + Record* p = dynamic_cast(content.ptr); + return p ? boost::optional(p) : boost::none; + } + } + /** + * *** This is the main entry point for reverse AD, called from Expression *** + * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) + */ + typedef Eigen::Matrix JacobianTT; + void startReverseAD1(JacobianMap& jacobians) const { + if (kind == Leaf) { + // This branch will only be called on trivial Leaf expressions, i.e. Priors + static const JacobianTT I = JacobianTT::Identity(); + handleLeafCase(I, jacobians, content.key); + } else if (kind == Function) + // This is the more typical entry point, starting the AD pipeline + // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. + content.ptr->startReverseAD2(jacobians); + } + // Either add to Jacobians (Leaf) or propagate (Function) + template + void reverseAD1(const Eigen::MatrixBase & dTdA, + JacobianMap& jacobians) const { + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) + content.ptr->reverseAD2(dTdA, jacobians); + } + + /// Define type so we can apply it as a meta-function + typedef ExecutionTrace type; +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h new file mode 100644 index 000000000..e7aa34db0 --- /dev/null +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -0,0 +1,516 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExpressionNode.h + * @date May 10, 2015 + * @author Frank Dellaert + * @author Paul Furgale + * @brief ExpressionNode class + */ + +#pragma once + +#include +#include +#include + +#include // operator typeid +#include +#include + +class ExpressionFactorBinaryTest; +// Forward declare for testing + +namespace gtsam { +namespace internal { + +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { + // right now only word sized types are supported. + // Easy to extend if needed, + // by somehow inferring the unsigned integer of same size + BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); + size_t & uiValue = reinterpret_cast(value); + size_t misAlignment = uiValue % requiredAlignment; + if (misAlignment) { + uiValue += requiredAlignment - misAlignment; + } + return value; +} +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { + return upAlign(value, requiredAlignment); +} + +//----------------------------------------------------------------------------- + +/** + * Expression node. The superclass for objects that do the heavy lifting + * An Expression has a pointer to an ExpressionNode underneath + * allowing Expressions to have polymorphic behaviour even though they + * are passed by value. This is the same way boost::function works. + * http://loki-lib.sourceforge.net/html/a00652.html + */ +template +class ExpressionNode { + +protected: + + size_t traceSize_; + + /// Constructor, traceSize is size of the execution trace of expression rooted here + ExpressionNode(size_t traceSize = 0) : + traceSize_(traceSize) { + } + +public: + + /// Destructor + virtual ~ExpressionNode() { + } + + /// Streaming + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, + const ExpressionNode& node) { + os << "Expression of type " << typeid(T).name(); + if (node.traceSize_ > 0) + os << ", trace size = " << node.traceSize_; + os << "\n"; + return os; + } + + /// Return keys that play in this expression as a set + virtual std::set keys() const { + std::set keys; + return keys; + } + + /// Return dimensions for each argument, as a map + virtual void dims(std::map& map) const { + } + + // Return size needed for memory buffer in traceExecution + size_t traceSize() const { + return traceSize_; + } + + /// Return value + virtual T value(const Values& values) const = 0; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const = 0; +}; + +//----------------------------------------------------------------------------- +/// Constant Expression +template +class ConstantExpression: public ExpressionNode { + + /// The constant value + T constant_; + + /// Constructor with a value, yielding a constant + ConstantExpression(const T& value) : + constant_(value) { + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~ConstantExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + return constant_; + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + return constant_; + } +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value +template +class LeafExpression: public ExpressionNode { + typedef T value_type; + + /// The key into values + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~LeafExpression() { + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + map[key_] = traits::dimension; + } + + /// Return value + virtual T value(const Values& values) const { + return values.at(key_); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + trace.setLeaf(key_); + return values.at(key_); + } + +}; + +//----------------------------------------------------------------------------- +/// meta-function to generate fixed-size JacobianTA type +template +struct Jacobian { + typedef Eigen::Matrix::dimension, traits::dimension> type; +}; + +// Eigen format for printing Jacobians +static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); + +//----------------------------------------------------------------------------- +/// Unary Function Expression +template +class UnaryExpression: public ExpressionNode { + + typedef typename Expression::template UnaryFunction::type Function; + boost::shared_ptr > expression1_; + Function function_; + + /// Constructor with a unary function f, and input argument e1 + UnaryExpression(Function f, const Expression& e1) : + expression1_(e1.root()), function_(f) { + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~UnaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + return function_(expression1_->value(values), boost::none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + return expression1_->keys(); + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "UnaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process + void startReverseAD4(JacobianMap& jacobians) const { + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 + // which calls the correctly sized CallRecord::reverseAD3, which in turn + // calls reverseAD4 below. + trace1.reverseAD1(dTdA1, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + } + }; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + + // Create the record at the start of the traceStorage and advance the pointer + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + + // Record the traces for all arguments + // After this, the traceStorage pointer is set to after what was written + // Write an Expression execution trace in record->trace + // Iff Constant or Leaf, this will not write to traceStorage, only to trace. + // Iff the expression is functional, write all Records in traceStorage buffer + // Return value of type T is recorded in record->value + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + + // ptr is never modified by traceExecution, but if traceExecution has + // written in the buffer, the next caller expects we advance the pointer + ptr += expression1_->traceSize(); + trace.setFunction(record); + + return function_(record->value1, record->dTdA1); + } +}; + +//----------------------------------------------------------------------------- +/// Binary Expression +template +class BinaryExpression: public ExpressionNode { + + typedef typename Expression::template BinaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + Function function_; + + /// Constructor with a binary function f, and two input arguments + BinaryExpression(Function f, const Expression& e1, + const Expression& e2) : + expression1_(e1.root()), expression2_(e2.root()), function_(f) { + ExpressionNode::traceSize_ = // + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); + } + + friend class Expression ; + friend class ::ExpressionFactorBinaryTest; + +public: + + /// Destructor + virtual ~BinaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(expression1_->value(values), expression2_->value(values), + none, none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "BinaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize(); + trace.setFunction(record); + return function_(record->value1, record->value2, record->dTdA1, + record->dTdA2); + } +}; + +//----------------------------------------------------------------------------- +/// Ternary Expression +template +class TernaryExpression: public ExpressionNode { + + typedef typename Expression::template TernaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + boost::shared_ptr > expression3_; + Function function_; + + /// Constructor with a ternary function f, and two input arguments + TernaryExpression(Function f, const Expression& e1, + const Expression& e2, const Expression& e3) : + expression1_(e1.root()), expression2_(e2.root()), expression3_(e3.root()), // + function_(f) { + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + // + e1.traceSize() + e2.traceSize() + e3.traceSize(); + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~TernaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(expression1_->value(values), expression2_->value(values), + expression3_->value(values), none, none, none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + myKeys = expression3_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + expression3_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + A3 value3; + ExecutionTrace trace3; + typename Jacobian::type dTdA3; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "TernaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << dTdA3.format(kMatlabFormat) << std::endl; + trace3.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + trace3.reverseAD1(dTdA3, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + trace3.reverseAD1(dFdT * dTdA3, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + record->value3 = expression3_->traceExecution(values, record->trace3, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize() + + expression3_->traceSize(); + trace.setFunction(record); + return function_(record->value1, record->value2, record->value3, + record->dTdA1, record->dTdA2, record->dTdA3); + } +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/JacobianMap.h b/gtsam/nonlinear/internal/JacobianMap.h new file mode 100644 index 000000000..f4d2e9565 --- /dev/null +++ b/gtsam/nonlinear/internal/JacobianMap.h @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianMap.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief JacobianMap for returning derivatives from expressions + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { +namespace internal { + +// A JacobianMap is the primary mechanism by which derivatives are returned. +// Expressions are designed to write their derivatives into an already allocated +// Jacobian of the correct size, of type VerticalBlockMatrix. +// The JacobianMap provides a mapping from keys to the underlying blocks. +class JacobianMap { +private: + typedef FastVector Keys; + const FastVector& keys_; + VerticalBlockMatrix& Ab_; + +public: + /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab + JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { + } + + /// Access blocks of via key + VerticalBlockMatrix::Block operator()(Key key) { + Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key); + DenseIndex block = it - keys_.begin(); + return Ab_(block); + } +}; + +} // namespace internal +} // namespace gtsam + diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index f3858c818..3f477098a 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -265,9 +265,10 @@ TEST(AdaptAutoDiff, Snavely) { typedef AdaptAutoDiff Adaptor; Expression expression(Adaptor(), P, X); #ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero #else - EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression::Record), + expression.traceSize()); // Todo, should be zero #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 483b5ffa9..208f0b284 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -18,7 +18,7 @@ * @brief unit tests for CallRecord class */ -#include +#include #include #include @@ -32,7 +32,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > CallRecordMaxVirtualStaticRows){ + if(i > internal::CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; @@ -80,13 +80,13 @@ struct Record: public internal::CallRecordImplementor { } void print(const std::string& indent) const { } - void startReverseAD4(JacobianMap& jacobians) const { + void startReverseAD4(internal::JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, internal::JacobianMap& jacobians) const { cc.compTimeRows = SomeMatrix::RowsAtCompileTime; cc.compTimeCols = SomeMatrix::ColsAtCompileTime; cc.runTimeRows = dFdT.rows(); @@ -97,7 +97,7 @@ struct Record: public internal::CallRecordImplementor { friend struct internal::CallRecordImplementor; }; -JacobianMap & NJM= *static_cast(NULL); +internal::JacobianMap & NJM= *static_cast(NULL); /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp new file mode 100644 index 000000000..731f69816 --- /dev/null +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExecutionTrace.cpp + * @date May 11, 2015 + * @author Frank Dellaert + * @brief unit tests for Expression internals + */ + +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Constant +TEST(ExecutionTrace, construct) { + internal::ExecutionTrace trace; +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 1c97394e1..75af5f634 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -42,7 +42,7 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ // Constant -TEST(Expression, constant) { +TEST(Expression, Constant) { Expression R(someR); Values values; Rot3 actual = R.value(values); @@ -68,24 +68,27 @@ TEST(Expression, Leaves) { Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); std::vector > points = createUnknowns(10, 'p', 1); - EXPECT(assert_equal(somePoint,points.back().value(values))); + EXPECT(assert_equal(somePoint, points.back().value(values))); } /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { +Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) { return Point2(); } double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } +Vector f3(const Point3& p, OptionalJacobian H) { + return p.vector(); +} Expression p(1); set expected = list_of(1); } -TEST(Expression, Unary0) { +TEST(Expression, Unary1) { using namespace unary; - Expression e(f0, p); + Expression e(f1, p); EXPECT(expected == e.keys()); } TEST(Expression, Unary2) { @@ -94,6 +97,12 @@ TEST(Expression, Unary2) { EXPECT(expected == e.keys()); } /* ************************************************************************* */ +// Unary(Leaf), dynamic +TEST(Expression, Unary3) { + using namespace unary; +// Expression e(f3, p); +} +/* ************************************************************************* */ //Nullary Method TEST(Expression, NullaryMethod) { @@ -118,7 +127,7 @@ TEST(Expression, NullaryMethod) { EXPECT(actual == sqrt(50)); Matrix expected(1, 3); expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); - EXPECT(assert_equal(expected,H[0])); + EXPECT(assert_equal(expected, H[0])); } /* ************************************************************************* */ // Binary(Leaf,Leaf) @@ -149,12 +158,12 @@ TEST(Expression, BinaryKeys) { TEST(Expression, BinaryDimensions) { map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // dimensions TEST(Expression, BinaryTraceSize) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Binary::Record); EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize()); } @@ -181,17 +190,26 @@ TEST(Expression, TreeKeys) { TEST(Expression, TreeDimensions) { map actual, expected = map_list_of(1, 6)(2, 3)(3, 5); tree::uv_hat.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // TraceSize TEST(Expression, TreeTraceSize) { - typedef UnaryExpression Unary; - typedef BinaryExpression Binary1; - typedef BinaryExpression Binary2; - size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record) - + sizeof(Binary2::Record); - EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize()); + typedef internal::BinaryExpression Binary1; + EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), + tree::p_cam.traceSize()); + + typedef internal::UnaryExpression Unary; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(), + tree::projection.traceSize()); + + EXPECT_LONGS_EQUAL(0, tree::K.traceSize()); + + typedef internal::BinaryExpression Binary2; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() + + tree::projection.traceSize(), tree::uv_hat.traceSize()); } /* ************************************************************************* */ @@ -235,7 +253,8 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, + OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index ae794db6a..b3fd76c67 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -109,7 +109,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("AntiFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(factor_); diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index d5740a6ab..c78b5a3bf 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -91,7 +91,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index ef02b5cb1..1b51224d2 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -113,7 +113,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measuredBearing_); diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 319c74cd5..bfd7d4e34 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -117,7 +117,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); @@ -149,7 +149,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("BetweenFactor", boost::serialization::base_object >(*this)); } diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 9cb1e3017..44a5ee5f2 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -84,7 +84,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); @@ -157,7 +157,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 0e4fb48cf..92a78279b 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -97,7 +97,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 2f8dd601f..d2b042fed 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -20,218 +20,280 @@ #pragma once -#include -#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include #include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { - /** - * Non-linear factor for a constraint derived from a 2D measurement. - * The calibration is unknown here compared to GenericProjectionFactor - * @addtogroup SLAM - */ - template - class GeneralSFMFactor: public NoiseModelFactor2 { +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * The calibration is unknown here compared to GenericProjectionFactor + * @addtogroup SLAM + */ +template +class GeneralSFMFactor: public NoiseModelFactor2 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) - GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA); + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK); - static const int DimC = FixedDimension::value; - static const int DimL = FixedDimension::value; + static const int DimC = FixedDimension::value; + static const int DimL = FixedDimension::value; + typedef Eigen::Matrix JacobianC; + typedef Eigen::Matrix JacobianL; - protected: +protected: - Point2 measured_; ///< the 2D measurement + Point2 measured_; ///< the 2D measurement - public: +public: - typedef GeneralSFMFactor This; ///< typedef for this object - typedef NoiseModelFactor2 Base; ///< typedef for the base class + typedef GeneralSFMFactor This;///< typedef for this object + typedef NoiseModelFactor2 Base;///< typedef for the base class - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /** - * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation of the measurements - * @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():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 - - virtual ~GeneralSFMFactor() {} ///< destructor - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter for printing out Symbols - */ - void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); - } - - /** - * equals - */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { - const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; - } - - /** h(x)-z */ - Vector evaluateError(const CAMERA& camera, const LANDMARK& point, - boost::optional H1=boost::none, boost::optional H2=boost::none) const { - - try { - Point2 reprojError(camera.project2(point,H1,H2) - measured_); - return reprojError.vector(); - } - catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, DimC); - if (H2) *H2 = zeros(2, DimL); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; - return zero(2); - } - } - - /** return the measured */ - inline const Point2 measured() const { - return measured_; - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } - }; - - template - struct traits > : Testable< - GeneralSFMFactor > { - }; + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; /** - * Non-linear factor for a constraint derived from a 2D measurement. - * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. + * Constructor + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation of the measurements + * @param cameraKey is the index of the camera + * @param landmarkKey is the index of the landmark */ - template - class GeneralSFMFactor2: public NoiseModelFactor3 { + GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : + Base(model, cameraKey, landmarkKey), measured_(measured) {} - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) - static const int DimK = FixedDimension::value; + 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 - protected: + virtual ~GeneralSFMFactor() {} ///< destructor - Point2 measured_; ///< the 2D measurement + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this)));} - public: + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter for printing out Symbols + */ + void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } - typedef GeneralSFMFactor2 This; - typedef PinholeCamera Camera; ///< typedef for camera type - typedef NoiseModelFactor3 Base; ///< typedef for the base class + /** + * equals + */ + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + } - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /** - * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation of the measurements - * @param poseKey is the index of the camera - * @param landmarkKey is the index of the landmark - * @param calibKey is the index of the calibration - */ - GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : - Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} - GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor - - virtual ~GeneralSFMFactor2() {} ///< destructor - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); + /** h(x)-z */ + Vector evaluateError(const CAMERA& camera, const LANDMARK& point, + boost::optional H1=boost::none, boost::optional H2=boost::none) const { + try { + Point2 reprojError(camera.project2(point,H1,H2) - measured_); + return reprojError.vector(); } - - /** - * equals - */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { - const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; - } - - /** h(x)-z */ - Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const - { - try { - Camera camera(pose3,calib); - Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); - return reprojError.vector(); - } - catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, 6); - if (H2) *H2 = zeros(2, 3); - if (H3) *H3 = zeros(2, DimK); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; - } + catch( CheiralityException& e) { + if (H1) *H1 = JacobianC::Zero(); + if (H2) *H2 = JacobianL::Zero(); + // TODO warn if verbose output asked for return zero(2); } + } - /** return the measured */ - inline const Point2 measured() const { - return measured_; + /// Linearize using fixed-size matrices + boost::shared_ptr linearize(const Values& values) const { + // Only linearize if the factor is active + if (!this->active(values)) return boost::shared_ptr(); + + const Key key1 = this->key1(), key2 = this->key2(); + JacobianC H1; + JacobianL H2; + Vector2 b; + try { + const CAMERA& camera = values.at(key1); + const LANDMARK& point = values.at(key2); + Point2 reprojError(camera.project2(point, H1, H2) - measured()); + b = -reprojError.vector(); + } catch (CheiralityException& e) { + H1.setZero(); + H2.setZero(); + b.setZero(); + // TODO warn if verbose output asked for } - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor3", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); + // Whiten the system if needed + const SharedNoiseModel& noiseModel = this->get_noiseModel(); + if (noiseModel && !noiseModel->isUnit()) { + // TODO: implement WhitenSystem for fixed size matrices and include + // above + H1 = noiseModel->Whiten(H1); + H2 = noiseModel->Whiten(H2); + b = noiseModel->Whiten(b); } - }; - template - struct traits > : Testable< - GeneralSFMFactor2 > { - }; + // Create new (unit) noiseModel, preserving constraints if applicable + SharedDiagonal model; + if (noiseModel && noiseModel->isConstrained()) { + model = boost::static_pointer_cast(noiseModel)->unit(); + } + + return boost::make_shared >(key1, H1, key2, H2, b, model); + } + + /** return the measured */ + inline const Point2 measured() const { + return measured_; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; + +template +struct traits > : Testable< + GeneralSFMFactor > { +}; + +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. + */ +template +class GeneralSFMFactor2: public NoiseModelFactor3 { + + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + static const int DimK = FixedDimension::value; + +protected: + + Point2 measured_; ///< the 2D measurement + +public: + + typedef GeneralSFMFactor2 This; + typedef PinholeCamera Camera;///< typedef for camera type + typedef NoiseModelFactor3 Base;///< typedef for the base class + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation of the measurements + * @param poseKey is the index of the camera + * @param landmarkKey is the index of the landmark + * @param calibKey is the index of the calibration + */ + GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : + Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} + GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor + + virtual ~GeneralSFMFactor2() {} ///< destructor + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this)));} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /** + * equals + */ + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + } + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const + { + try { + Camera camera(pose3,calib); + Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); + return reprojError.vector(); + } + catch( CheiralityException& e) { + if (H1) *H1 = zeros(2, 6); + if (H2) *H2 = zeros(2, 3); + if (H3) *H3 = zeros(2, DimK); + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) + << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; + } + return zero(2); + } + + /** return the measured */ + inline const Point2 measured() const { + return measured_; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; + +template +struct traits > : Testable< + GeneralSFMFactor2 > { +}; } //namespace diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index f7d42497e..94c19a9d0 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -88,7 +88,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 6689653aa..e9914955c 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -90,7 +90,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index eee14f9f2..a738d4cf0 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -57,6 +57,11 @@ namespace gtsam { Base(model, key), prior_(prior) { } + /** Convenience constructor that takes a full covariance argument */ + PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : + Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { + } + /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( @@ -94,7 +99,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 52e28beaf..1056527d1 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -178,7 +178,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 216b9320e..d45dd5ce0 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -90,7 +90,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); @@ -181,7 +181,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 6a70d58b4..90ceeafc8 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -122,7 +122,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor3", boost::serialization::base_object(*this)); } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 0e52d9ba7..c713eff72 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -109,6 +109,11 @@ public: return D; } + virtual void updateHessian(const FastVector& keys, + SymmetricBlockMatrix* info) const { + throw std::runtime_error( + "RegularImplicitSchurFactor::updateHessian non implemented"); + } virtual Matrix augmentedJacobian() const { throw std::runtime_error( "RegularImplicitSchurFactor::augmentedJacobian non implemented"); diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index ba38a65d9..b147c2721 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -406,7 +406,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index e089e6f4f..b428d7b05 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -143,7 +143,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 7b21e044f..578422eaf 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -169,7 +169,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 8001357c9..b94eafba7 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -183,7 +183,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(camera_); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index d3a7c1e84..5ad1ff2c0 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -15,19 +15,34 @@ * @brief utility functions for loading datasets */ -#include -#include #include +#include +#include +#include #include -#include -#include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include +#include +#include +#include +#include +#include #include -#include -#include +#include +#include using namespace std; namespace fs = boost::filesystem; @@ -64,8 +79,8 @@ string findExampleDataFile(const string& name) { throw invalid_argument( "gtsam::findExampleDataFile could not find a matching file in\n" - SOURCE_TREE_DATASET_DIR " or\n" - INSTALLED_DATASET_DIR " named\n" + + GTSAM_SOURCE_TREE_DATASET_DIR " or\n" + GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name + ".graph, or " + name + ".txt"); } @@ -670,6 +685,7 @@ bool readBundler(const string& filename, SfM_data &data) { float u, v; is >> cam_idx >> point_idx >> u >> v; track.measurements.push_back(make_pair(cam_idx, Point2(u, -v))); + track.siftIndices.push_back(make_pair(cam_idx, point_idx)); } data.tracks.push_back(track); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 3a0f8edb7..54e27229c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,16 +18,21 @@ #pragma once -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include // for pair +#include #include +#include // for pair +#include namespace gtsam { @@ -137,11 +142,15 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfM_Measurement; +/// SfM_Track +typedef std::pair SIFT_Index; + /// Define the structure for the 3D points struct SfM_Track { Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) + std::vector siftIndices; size_t number_measurements() const { return measurements.size(); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 240b19a46..a8f85301e 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -19,18 +19,20 @@ #include #include #include +#include +#include +#include #include -#include #include #include #include -#include -#include +#include #include +#include #include #include -using namespace boost; +using namespace boost::assign; #include #include @@ -49,7 +51,8 @@ typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: - void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { + void addMeasurement(int i, int j, const Point2& z, + const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, X(i), L(j))); } @@ -65,98 +68,99 @@ public: }; -static double getGaussian() -{ - double S,V1,V2; - // Use Box-Muller method to create gauss noise from uniform noise - do - { - double U1 = rand() / (double)(RAND_MAX); - double U2 = rand() / (double)(RAND_MAX); - V1 = 2 * U1 - 1; /* V1=[-1,1] */ - V2 = 2 * U2 - 1; /* V2=[-1,1] */ - S = V1 * V1 + V2 * V2; - } while(S>=1); - return sqrt(-2.0f * (double)log(S) / S) * V1; +static double getGaussian() { + double S, V1, V2; + // Use Box-Muller method to create gauss noise from uniform noise + do { + double U1 = rand() / (double) (RAND_MAX); + double U2 = rand() / (double) (RAND_MAX); + V1 = 2 * U1 - 1; /* V1=[-1,1] */ + V2 = 2 * U2 - 1; /* V2=[-1,1] */ + S = V1 * V1 + V2 * V2; + } while (S >= 1); + return sqrt(-2.f * (double) log(S) / S) * V1; } static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); +static const double baseline = 5.; /* ************************************************************************* */ -TEST( GeneralSFMFactor, equals ) -{ - // Create two identical factors and make sure they're equal - Point2 z(323.,240.); - const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); - const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); +static vector genPoint3() { + const double z = 5; + vector landmarks; + landmarks.push_back(Point3(-1., -1., z)); + landmarks.push_back(Point3(-1., 1., z)); + landmarks.push_back(Point3(1., 1., z)); + landmarks.push_back(Point3(1., -1., z)); + landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-2., -2., 2 * z)); + landmarks.push_back(Point3(-2., 2., 2 * z)); + landmarks.push_back(Point3(2., 2., 2 * z)); + landmarks.push_back(Point3(2., -2., 2 * z)); + return landmarks; +} - boost::shared_ptr - factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); +static vector genCameraDefaultCalibration() { + vector X; + X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)))); + return X; +} + +static vector genCameraVariableCalibration() { + const Cal3_S2 K(640, 480, 0.1, 320, 240); + vector X; + X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K)); + return X; +} + +static boost::shared_ptr getOrdering( + const vector& cameras, const vector& landmarks) { + boost::shared_ptr ordering(new Ordering); + for (size_t i = 0; i < landmarks.size(); ++i) + ordering->push_back(L(i)); + for (size_t i = 0; i < cameras.size(); ++i) + ordering->push_back(X(i)); + return ordering; +} + +/* ************************************************************************* */ +TEST( GeneralSFMFactor, equals ) { + // Create two identical factors and make sure they're equal + Point2 z(323., 240.); + const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); + const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); + boost::shared_ptr factor1( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + + boost::shared_ptr factor2( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { - Point2 z(3.,0.); - const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); + Point2 z(3., 0.); + const SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + Projection factor(z, sigma, X(1), L(1)); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; - Point3 t1(0,0,-6); - Pose3 x1(R,t1); + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor->unwhitenedError(values))); + Point3 l1; + values.insert(L(1), l1); + EXPECT( + assert_equal(((Vector ) Vector2(-3., 0.)), + factor.unwhitenedError(values))); } -static const double baseline = 5.0 ; - -/* ************************************************************************* */ -static vector genPoint3() { - const double z = 5; - vector landmarks ; - landmarks.push_back(Point3 (-1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); - landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); - return landmarks ; -} - -static vector genCameraDefaultCalibration() { - vector X ; - X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); - X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); - return X ; -} - -static vector genCameraVariableCalibration() { - const Cal3_S2 K(640,480,0.01,320,240); - vector X ; - X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); - X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); - return X ; -} - -static boost::shared_ptr getOrdering(const vector& cameras, const vector& landmarks) { - boost::shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; - return ordering ; -} - - /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_defaultK ) { @@ -165,32 +169,32 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -202,38 +206,37 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - if ( i == 0 ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; - } - else { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + if (i == 0) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); + } else { + values.insert(L(i), landmarks[i]); } } graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -246,35 +249,34 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; - graph.addMeasurement(j, i, pt, sigma1); + for (size_t i = 0; i < cameras.size(); ++i) { + for (size_t j = 0; j < landmarks.size(); ++j) { + Point2 z = cameras[i].project(landmarks[j]); + graph.addMeasurement(i, j, z, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); - values.insert(L(i), pt) ; + for (size_t j = 0; j < landmarks.size(); ++j) { + Point3 pt(landmarks[j].x() + noise * getGaussian(), + landmarks[j].y() + noise * getGaussian(), + landmarks[j].z() + noise * getGaussian()); + values.insert(L(j), pt); } - for ( size_t i = 0 ; i < cameras.size() ; ++i ) + for (size_t i = 0; i < cameras.size(); ++i) graph.addCameraConstraint(i, cameras[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -288,50 +290,45 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) { - const double - rot_noise = 1e-5, - trans_noise = 1e-3, - focal_noise = 1, - skew_noise = 1e-5; - if ( i == 0 ) { - values.insert(X(i), cameras[i]) ; - } - else { + for (size_t i = 0; i < cameras.size(); ++i) { + const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, + skew_noise = 1e-5; + if (i == 0) { + values.insert(X(i), cameras[i]); + } else { - Vector delta = (Vector(11) << - rot_noise, rot_noise, rot_noise, // rotation - trans_noise, trans_noise, trans_noise, // translation - focal_noise, focal_noise, // f_x, f_y - skew_noise, // s - trans_noise, trans_noise // ux, uy + Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation + trans_noise, trans_noise, trans_noise, // translation + focal_noise, focal_noise, // f_x, f_y + skew_noise, // s + trans_noise, trans_noise // ux, uy ).finished(); - values.insert(X(i), cameras[i].retract(delta)) ; + values.insert(X(i), cameras[i].retract(delta)); } } - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + values.insert(L(i), landmarks[i]); } // fix X0 and all landmarks, allow only the cameras[1] to move graph.addCameraConstraint(0, cameras[0]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + for (size_t i = 0; i < landmarks.size(); ++i) graph.addPoint3Constraint(i, landmarks[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -344,38 +341,40 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } // Constrain position of system with the first camera constrained to the origin graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.))); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -386,17 +385,21 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.))); + graph.push_back( + PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.))); Values init; init.insert(X(0), GeneralCamera()); - init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.))); // The optimal value between the 2m range factor and 1m prior is 1.5m Values expected; expected.insert(X(0), GeneralCamera()); - expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0))); + expected.insert(X(1), Pose3(Rot3(), Point3(1.5, 0., 0.))); LevenbergMarquardtParams params; params.absoluteErrorTol = 1e-9; @@ -410,16 +413,23 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.push_back(PriorFactor(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0))); - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back( + PriorFactor(X(0), CalibratedCamera(), + noiseModel::Isotropic::Sigma(6, 1.))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.))); + graph.push_back( + PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.))); Values init; init.insert(X(0), CalibratedCamera()); - init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.))); Values expected; - expected.insert(X(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); + expected.insert(X(0), + CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0))); LevenbergMarquardtParams params; @@ -431,5 +441,95 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +// Frank created these tests after switching to a custom LinearizedFactor +TEST(GeneralSFMFactor, BinaryJacobianFactor) { + Point2 measurement(3., -1.); + + // Create Values + Values values; + Rot3 R; + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); + values.insert(X(1), GeneralCamera(x1)); + Point3 l1; + values.insert(L(1), l1); + + vector models; + { + // Create various noise-models to test all cases + using namespace noiseModel; + Rot2 R = Rot2::fromAngle(0.3); + Matrix2 cov = R.matrix() * R.matrix().transpose(); + models += SharedNoiseModel(), Unit::Create(2), // + Isotropic::Sigma(2, 0.5), Constrained::All(2), Gaussian::Covariance(cov); + } + + // Now loop over all these noise models + BOOST_FOREACH(SharedNoiseModel model, models) { + Projection factor(measurement, model, X(1), L(1)); + + // Test linearize + GaussianFactor::shared_ptr expected = // + factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = factor.linearize(values); + EXPECT(assert_equal(*expected, *actual, 1e-9)); + + // Test methods that rely on updateHessian + if (model && !model->isConstrained()) { + // Construct HessianFactor from single JacobianFactor + HessianFactor expectedHessian(*expected), actualHessian(*actual); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-9)); + + // Convert back + JacobianFactor actualJacobian(actualHessian); + // Note we do not expect the actualJacobian to match *expected + // Just that they have the same information on the variable. + EXPECT( + assert_equal(expected->augmentedInformation(), + actualJacobian.augmentedInformation(), 1e-9)); + + // Construct from GaussianFactorGraph + GaussianFactorGraph gfg1; + gfg1 += expected; + GaussianFactorGraph gfg2; + gfg2 += actual; + HessianFactor hessian1(gfg1), hessian2(gfg2); + EXPECT(assert_equal(hessian1, hessian2, 1e-9)); + } + } +} + +/* ************************************************************************* */ +// Do a thorough test of BinaryJacobianFactor +TEST( GeneralSFMFactor, BinaryJacobianFactor2 ) { + + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); + + Values values; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); + for (size_t j = 0; j < landmarks.size(); ++j) + values.insert(L(j), landmarks[j]); + + for (size_t i = 0; i < cameras.size(); ++i) { + for (size_t j = 0; j < landmarks.size(); ++j) { + Point2 z = cameras[i].project(landmarks[j]); + Projection::shared_ptr nonlinear = // + boost::make_shared(z, sigma1, X(i), L(j)); + GaussianFactor::shared_ptr factor = nonlinear->linearize(values); + HessianFactor hessian(*factor); + JacobianFactor jacobian(hessian); + EXPECT( + assert_equal(factor->augmentedInformation(), + jacobian.augmentedInformation(), 1e-9)); + } + } +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index df56f5260..f812cd308 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -49,7 +49,8 @@ typedef NonlinearEquality Point3Constraint; /* ************************************************************************* */ class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { + void addMeasurement(const int& i, const int& j, const Point2& z, + const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, X(i), L(j))); } @@ -65,97 +66,101 @@ public: }; -static double getGaussian() -{ - double S,V1,V2; - // Use Box-Muller method to create gauss noise from uniform noise - do - { - double U1 = rand() / (double)(RAND_MAX); - double U2 = rand() / (double)(RAND_MAX); - V1 = 2 * U1 - 1; /* V1=[-1,1] */ - V2 = 2 * U2 - 1; /* V2=[-1,1] */ - S = V1 * V1 + V2 * V2; - } while(S>=1); - return sqrt(-2.0f * (double)log(S) / S) * V1; +static double getGaussian() { + double S, V1, V2; + // Use Box-Muller method to create gauss noise from uniform noise + do { + double U1 = rand() / (double) (RAND_MAX); + double U2 = rand() / (double) (RAND_MAX); + V1 = 2 * U1 - 1; /* V1=[-1,1] */ + V2 = 2 * U2 - 1; /* V2=[-1,1] */ + S = V1 * V1 + V2 * V2; + } while (S >= 1); + return sqrt(-2.f * (double) log(S) / S) * V1; } static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); /* ************************************************************************* */ -TEST( GeneralSFMFactor_Cal3Bundler, equals ) -{ +TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Point2 z(323.,240.); - const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); + Point2 z(323., 240.); + const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor1( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - boost::shared_ptr - factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor2( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ TEST( GeneralSFMFactor_Cal3Bundler, error ) { - Point2 z(3.,0.); + Point2 z(3., 0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor(new Projection(z, sigma, X(1), L(1))); + boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; - Point3 t1(0,0,-6); - Pose3 x1(R,t1); + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values))); + Point3 l1; + values.insert(L(1), l1); + EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values))); } - -static const double baseline = 5.0 ; +static const double baseline = 5.; /* ************************************************************************* */ static vector genPoint3() { const double z = 5; - vector landmarks ; - landmarks.push_back(Point3 (-1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); - landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); - return landmarks ; + vector landmarks; + landmarks.push_back(Point3(-1., -1., z)); + landmarks.push_back(Point3(-1., 1., z)); + landmarks.push_back(Point3(1., 1., z)); + landmarks.push_back(Point3(1., -1., z)); + landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-2., -2., 2 * z)); + landmarks.push_back(Point3(-2., 2., 2 * z)); + landmarks.push_back(Point3(2., 2., 2 * z)); + landmarks.push_back(Point3(2., -2., 2 * z)); + return landmarks; } static vector genCameraDefaultCalibration() { - vector cameras ; - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); - return cameras ; + vector cameras; + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)))); + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)))); + return cameras; } static vector genCameraVariableCalibration() { - const Cal3Bundler K(500,1e-3,1e-3); - vector cameras ; - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); - return cameras ; + const Cal3Bundler K(500, 1e-3, 1e-3); + vector cameras; + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K)); + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K)); + return cameras; } -static boost::shared_ptr getOrdering(const std::vector& cameras, const std::vector& landmarks) { +static boost::shared_ptr getOrdering( + const std::vector& cameras, + const std::vector& landmarks) { boost::shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; - return ordering ; + for (size_t i = 0; i < landmarks.size(); ++i) + ordering->push_back(L(i)); + for (size_t i = 0; i < cameras.size(); ++i) + ordering->push_back(X(i)); + return ordering; } /* ************************************************************************* */ @@ -166,32 +171,32 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -203,38 +208,37 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - if ( i == 0 ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; - } - else { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + if (i == 0) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); + } else { + values.insert(L(i), landmarks[i]); } } graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -247,35 +251,35 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); - values.insert(L(i), pt) ; + values.insert(L(i), pt); } - for ( size_t i = 0 ; i < cameras.size() ; ++i ) + for (size_t i = 0; i < cameras.size(); ++i) graph.addCameraConstraint(i, cameras[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -289,46 +293,43 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) { - const double - rot_noise = 1e-5, trans_noise = 1e-3, - focal_noise = 1, distort_noise = 1e-3; - if ( i == 0 ) { - values.insert(X(i), cameras[i]) ; - } - else { + for (size_t i = 0; i < cameras.size(); ++i) { + const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, + distort_noise = 1e-3; + if (i == 0) { + values.insert(X(i), cameras[i]); + } else { - Vector delta = (Vector(9) << - rot_noise, rot_noise, rot_noise, // rotation - trans_noise, trans_noise, trans_noise, // translation - focal_noise, distort_noise, distort_noise // f, k1, k2 + Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation + trans_noise, trans_noise, trans_noise, // translation + focal_noise, distort_noise, distort_noise // f, k1, k2 ).finished(); - values.insert(X(i), cameras[i].retract(delta)) ; + values.insert(X(i), cameras[i].retract(delta)); } } - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + values.insert(L(i), landmarks[i]); } // fix X0 and all landmarks, allow only the cameras[1] to move graph.addCameraConstraint(0, cameras[0]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + for (size_t i = 0; i < landmarks.size(); ++i) graph.addPoint3Constraint(i, landmarks[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -341,43 +342,48 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } // Constrain position of system with the first camera constrained to the origin graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.))); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 508182557..636e69b4b 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -78,7 +78,7 @@ TEST( SmartProjectionCameraFactor, Constructor) { /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor2) { using namespace vanilla; - SmartFactor factor1(SmartFactor::HESSIAN, rankTol); + SmartFactor factor1(gtsam::HESSIAN, rankTol); } /* ************************************************************************* */ @@ -91,7 +91,7 @@ TEST( SmartProjectionCameraFactor, Constructor3) { /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor4) { using namespace vanilla; - SmartFactor factor1(SmartFactor::HESSIAN, rankTol); + SmartFactor factor1(gtsam::HESSIAN, rankTol); factor1.add(measurement1, x1, unit2); } @@ -806,8 +806,8 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { // Hessian version SmartFactor::shared_ptr explicitFactor( - new SmartFactor(SmartFactor::HESSIAN, rankTol, - SmartFactor::IGNORE_DEGENERACY, useEPI)); + new SmartFactor(gtsam::HESSIAN, rankTol, + gtsam::IGNORE_DEGENERACY, useEPI)); explicitFactor->add(level_uv, c1, unit2); explicitFactor->add(level_uv_right, c2, unit2); @@ -818,8 +818,8 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { // Implicit Schur version SmartFactor::shared_ptr implicitFactor( - new SmartFactor(SmartFactor::IMPLICIT_SCHUR, rankTol, - SmartFactor::IGNORE_DEGENERACY, useEPI)); + new SmartFactor(gtsam::IMPLICIT_SCHUR, rankTol, + gtsam::IGNORE_DEGENERACY, useEPI)); implicitFactor->add(level_uv, c1, unit2); implicitFactor->add(level_uv_right, c2, unit2); GaussianFactor::shared_ptr gaussianImplicitSchurFactor = diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 5dda02350..4db265036 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -76,7 +76,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index cac711043..c4f09c71c 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -65,7 +65,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 4b89a4b60..0530af556 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -116,7 +116,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); } diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index f30f88935..aca1ba043 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -144,7 +144,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; // IndexFactor diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 9813df331..0a5427ba3 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -111,7 +111,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index 49b14bc07..f2d891827 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -46,10 +46,10 @@ TEST( JunctionTree, constructor ) frontal1 = list_of(2)(3), frontal2 = list_of(0)(1), sep1, sep2 = list_of(2); - EXPECT(assert_container_equality(frontal1, actual.roots().front()->keys)); + EXPECT(assert_container_equality(frontal1, actual.roots().front()->orderedFrontalKeys)); //EXPECT(assert_equal(sep1, actual.roots().front()->separator)); LONGS_EQUAL(1, (long)actual.roots().front()->factors.size()); - EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->keys)); + EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->orderedFrontalKeys)); //EXPECT(assert_equal(sep2, actual.roots().front()->children.front()->separator)); LONGS_EQUAL(2, (long)actual.roots().front()->children.front()->factors.size()); EXPECT(assert_equal(*simpleChain[2], *actual.roots().front()->factors[0])); diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 89a97d51b..781b08d57 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -13,6 +13,10 @@ if("@GTSAM_USE_TBB@") list(APPEND GTSAM_INCLUDE_DIR "@TBB_INCLUDE_DIRS@") endif() +# Append Eigen include path, set in top-level CMakeLists.txt to either +# system-eigen, or GTSAM eigen path +list(APPEND GTSAM_INCLUDE_DIR "@GTSAM_EIGEN_INCLUDE_PREFIX@") + if("@GTSAM_USE_EIGEN_MKL@") list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@") endif() diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 818266d4c..942e1ab55 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -3,18 +3,15 @@ * @author Alex Cunningham */ -#include -#include - -#include - #include +#include +#include namespace gtsam { using namespace std; -static const Vector g = delta(3, 2, 9.81); +static const Vector kGravity = delta(3, 2, 9.81); /* ************************************************************************* */ double bound(double a, double min, double max) { @@ -24,28 +21,30 @@ double bound(double a, double min, double max) { } /* ************************************************************************* */ -PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, - double vx, double vy, double vz) -: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {} +PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, + double z, double vx, double vy, double vz) : + Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), + Velocity3(vx, vy, vz)) { +} /* ************************************************************************* */ -PoseRTV::PoseRTV(const Vector& rtv) -: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3)) -{ +PoseRTV::PoseRTV(const Vector& rtv) : + Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), + Velocity3(rtv.tail(3))) { } /* ************************************************************************* */ Vector PoseRTV::vector() const { Vector rtv(9); - rtv.head(3) = Rt_.rotation().xyz(); - rtv.segment(3,3) = Rt_.translation().vector(); - rtv.tail(3) = v_.vector(); + rtv.head(3) = rotation().xyz(); + rtv.segment(3,3) = translation().vector(); + rtv.tail(3) = velocity().vector(); return rtv; } /* ************************************************************************* */ bool PoseRTV::equals(const PoseRTV& other, double tol) const { - return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol); + return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol); } /* ************************************************************************* */ @@ -53,73 +52,7 @@ void PoseRTV::print(const string& s) const { cout << s << ":" << endl; gtsam::print((Vector)R().xyz(), " R:rpy"); t().print(" T"); - v_.print(" V"); -} - -/* ************************************************************************* */ -PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) { - if (H) CONCEPT_NOT_IMPLEMENTED; - Pose3 newPose = Pose3::Expmap(v.head<6>()); - Velocity3 newVel = Velocity3(v.tail<3>()); - return PoseRTV(newPose, newVel); -} - -/* ************************************************************************* */ -Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) { - if (H) CONCEPT_NOT_IMPLEMENTED; - Vector6 Lx = Pose3::Logmap(p.Rt_); - Vector3 Lv = p.v_.vector(); - return (Vector9() << Lx, Lv).finished(); -} - -/* ************************************************************************* */ -PoseRTV PoseRTV::retract(const Vector& v, - ChartJacobian Horigin, - ChartJacobian Hv) const { - if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED; - assert(v.size() == 9); - // First order approximation - Pose3 newPose = Rt_.retract(sub(v, 0, 6)); - Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9)); - return PoseRTV(newPose, newVel); -} - -/* ************************************************************************* */ -Vector PoseRTV::localCoordinates(const PoseRTV& p1, - ChartJacobian Horigin, - ChartJacobian Hp) const { - if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED; - const Pose3& x0 = pose(), &x1 = p1.pose(); - // First order approximation - Vector6 poseLogmap = x0.localCoordinates(x1); - Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector(); - return (Vector(9) << poseLogmap, lv).finished(); -} - -/* ************************************************************************* */ -PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } -PoseRTV PoseRTV::inverse(ChartJacobian H1) const { - if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); - return PoseRTV(Rt_.inverse(), - v_); -} - -/* ************************************************************************* */ -PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); } -PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1, - ChartJacobian H2) const { - if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5); - if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5); - return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_); -} - -/* ************************************************************************* */ -PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); } -PoseRTV PoseRTV::between(const PoseRTV& p, - ChartJacobian H1, - ChartJacobian H2) const { - if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5); - if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5); - return inverse().compose(p); + velocity().print(" V"); } /* ************************************************************************* */ @@ -187,7 +120,7 @@ PoseRTV PoseRTV::generalDynamics( Rot3 r2 = rotation().retract(gyro * dt); // Integrate Velocity Equations - Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g))); + Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity)); // Integrate Position Equations Point3 t2 = translationIntegration(r2, v2, dt); @@ -205,7 +138,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { // acceleration Vector3 accel = (v2-v1).vector() / dt; - imu.head<3>() = r2.transpose() * (accel - g); + imu.head<3>() = r2.transpose() * (accel - kGravity); // rotation rates // just using euler angles based on matlab code @@ -232,54 +165,40 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub } /* ************************************************************************* */ -double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } double PoseRTV::range(const PoseRTV& other, OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const { - if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5); - if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5); - return t().distance(other.t()); + Matrix36 D_t1_pose, D_t2_other; + const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); + const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); + Matrix13 D_d_t1, D_d_t2; + double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; + if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; + return d; } /* ************************************************************************* */ -PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) { - return global.transformed_from(transform); -} - PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, OptionalJacobian<9, 6> Dtrans) const { - // Note that we rotate the velocity - Matrix DVr, DTt; - Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt); - if (!Dglobal && !Dtrans) - return PoseRTV(trans.compose(pose()), newvel); // Pose3 transform is just compose - Matrix DTc, DGc; - Pose3 newpose = trans.compose(pose(), DTc, DGc); + Matrix6 D_newpose_trans, D_newpose_pose; + Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose); + + // Note that we rotate the velocity + Matrix3 D_newvel_R, D_newvel_v; + Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v); if (Dglobal) { - *Dglobal = zeros(9,9); - insertSub(*Dglobal, DGc, 0, 0); - - // Rotate velocity - insertSub(*Dglobal, eye(3,3), 6, 6); // FIXME: should this actually be an identity matrix? + Dglobal->setZero(); + Dglobal->topLeftCorner<6,6>() = D_newpose_pose; + Dglobal->bottomRightCorner<3,3>() = D_newvel_v; } if (Dtrans) { - *Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-8); - // - // *Dtrans = zeros(9,6); - // // directly affecting the pose - // insertSub(*Dtrans, DTc, 0, 0); // correct in tests - // - // // rotating the velocity - // Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z()); - // trans.rotation().print("Transform rotation"); - // gtsam::print(vRhat, "vRhat"); - // gtsam::print(DVr, "DVr"); - // // FIXME: find analytic derivative - //// insertSub(*Dtrans, vRhat, 6, 0); // works if PoseRTV.rotation() = I - //// insertSub(*Dtrans, trans.rotation().matrix() * vRhat, 6, 0); // FAIL: both tests fail + Dtrans->setZero(); + Dtrans->topLeftCorner<6,6>() = D_newpose_trans; + Dtrans->bottomLeftCorner<3,3>() = D_newvel_R; } return PoseRTV(newpose, newvel); } diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index ea7a2c9ff..78bd1fe6f 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -7,8 +7,8 @@ #pragma once #include -#include #include +#include namespace gtsam { @@ -19,29 +19,30 @@ typedef Point3 Velocity3; * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation */ -class GTSAM_UNSTABLE_EXPORT PoseRTV { +class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup { protected: - Pose3 Rt_; - Velocity3 v_; - + typedef ProductLieGroup Base; typedef OptionalJacobian<9, 9> ChartJacobian; public: - enum { dimension = 9 }; // constructors - with partial versions PoseRTV() {} - PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel) - : Rt_(rot, pt), v_(vel) {} - PoseRTV(const Rot3& rot, const Point3& pt, const Velocity3& vel) - : Rt_(rot, pt), v_(vel) {} - explicit PoseRTV(const Point3& pt) - : Rt_(Rot3::identity(), pt) {} + PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel) + : Base(Pose3(rot, t), vel) {} + PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel) + : Base(Pose3(rot, t), vel) {} + explicit PoseRTV(const Point3& t) + : Base(Pose3(Rot3(), t),Velocity3()) {} PoseRTV(const Pose3& pose, const Velocity3& vel) - : Rt_(pose), v_(vel) {} + : Base(pose, vel) {} explicit PoseRTV(const Pose3& pose) - : Rt_(pose) {} + : Base(pose,Velocity3()) {} + + // Construct from Base + PoseRTV(const Base& base) + : Base(base) {} /** build from components - useful for data files */ PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, @@ -51,72 +52,46 @@ public: explicit PoseRTV(const Vector& v); // access - const Point3& t() const { return Rt_.translation(); } - const Rot3& R() const { return Rt_.rotation(); } - const Velocity3& v() const { return v_; } - const Pose3& pose() const { return Rt_; } + const Pose3& pose() const { return first; } + const Velocity3& v() const { return second; } + const Point3& t() const { return pose().translation(); } + const Rot3& R() const { return pose().rotation(); } // longer function names - const Point3& translation() const { return Rt_.translation(); } - const Rot3& rotation() const { return Rt_.rotation(); } - const Velocity3& velocity() const { return v_; } + const Point3& translation() const { return pose().translation(); } + const Rot3& rotation() const { return pose().rotation(); } + const Velocity3& velocity() const { return second; } // Access to vector for ease of use with Matlab // and avoidance of Point3 Vector vector() const; - Vector translationVec() const { return Rt_.translation().vector(); } - Vector velocityVec() const { return v_.vector(); } + Vector translationVec() const { return pose().translation().vector(); } + Vector velocityVec() const { return velocity().vector(); } // testable bool equals(const PoseRTV& other, double tol=1e-6) const; void print(const std::string& s="") const; - // Manifold - static size_t Dim() { return 9; } - size_t dim() const { return Dim(); } + /// @name Manifold + /// @{ + using Base::dimension; + using Base::dim; + using Base::Dim; + using Base::retract; + using Base::localCoordinates; + /// @} - /** - * retract/unretract assume independence of components - * Tangent space parameterization: - * - v(0-2): Rot3 (roll, pitch, yaw) - * - v(3-5): Point3 - * - v(6-8): Translational velocity - */ - PoseRTV retract(const Vector& v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const; - Vector localCoordinates(const PoseRTV& p, ChartJacobian Horigin=boost::none,ChartJacobian Hp=boost::none) const; + /// @name measurement functions + /// @{ - // Lie TODO IS this a Lie group or just a Manifold???? - /** - * expmap/logmap are poor approximations that assume independence of components - * Currently implemented using the poor retract/unretract approximations - */ - static PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none); - static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none); - - static PoseRTV identity() { return PoseRTV(); } - - /** Derivatives calculated numerically */ - PoseRTV inverse(ChartJacobian H1=boost::none) const; - - /** Derivatives calculated numerically */ - PoseRTV compose(const PoseRTV& p, - ChartJacobian H1=boost::none, - ChartJacobian H2=boost::none) const; - - PoseRTV operator*(const PoseRTV& p) const { return compose(p); } - - /** Derivatives calculated numerically */ - PoseRTV between(const PoseRTV& p, - ChartJacobian H1=boost::none, - ChartJacobian H2=boost::none) const; - - // measurement functions - /** Derivatives calculated numerically */ + /** range between translations */ double range(const PoseRTV& other, OptionalJacobian<1,9> H1=boost::none, OptionalJacobian<1,9> H2=boost::none) const; + /// @} - // IMU-specific + /// @name IMU-specific + /// @{ /// Dynamics integrator for ground robots /// Always move from time 1 to time 2 @@ -164,7 +139,9 @@ public: ChartJacobian Dglobal = boost::none, OptionalJacobian<9, 6> Dtrans = boost::none) const; - // Utility functions + /// @} + /// @name Utility functions + /// @{ /// RRTMbn - Function computes the rotation rate transformation matrix from /// body axis rates to euler angle (global) rates @@ -177,19 +154,20 @@ public: static Matrix RRTMnb(const Vector& euler); static Matrix RRTMnb(const Rot3& att); + /// @} private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(Rt_); - ar & BOOST_SERIALIZATION_NVP(v_); + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(first); + ar & BOOST_SERIALIZATION_NVP(second); } }; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index a92ae0426..9ecc7619e 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -52,7 +52,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 0261257be..2ea582292 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -76,11 +76,12 @@ TEST( testPoseRTV, equals ) { /* ************************************************************************* */ TEST( testPoseRTV, Lie ) { // origin and zero deltas - EXPECT(assert_equal(PoseRTV(), PoseRTV().retract(zero(9)), tol)); - EXPECT(assert_equal(zero(9), PoseRTV().localCoordinates(PoseRTV()), tol)); + PoseRTV identity; + EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol)); + EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol)); PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(state1, state1.retract(zero(9)), tol)); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol)); EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished(); @@ -88,9 +89,9 @@ TEST( testPoseRTV, Lie ) { Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); PoseRTV state2(pt2, rot2, vel2); - EXPECT(assert_equal(state2, state1.retract(delta), 1e-1)); + EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1)); EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1)); - EXPECT(assert_equal(-delta, state2.localCoordinates(state1), 1e-1)); // loose tolerance due to retract approximation + EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1)); } /* ************************************************************************* */ @@ -119,6 +120,43 @@ TEST( testPoseRTV, dynamics_identities ) { } +/* ************************************************************************* */ +PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); } +TEST( testPoseRTV, compose ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); } +TEST( testPoseRTV, between ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); } +TEST( testPoseRTV, inverse ) { + PoseRTV state1(pt, rot, vel); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); + EXPECT(assert_equal(numericH1, actH1, tol)); +} + /* ************************************************************************* */ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } TEST( testPoseRTV, range ) { diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index cfed9ae0c..ac2c31077 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -88,11 +88,13 @@ int main(int argc, char** argv){ } Values initial_pose_values = initial_estimate.filter(); - if(output_poses){ - init_pose3Out.open(init_poseOutput.c_str(),ios::out); - for(int i = 1; i<=initial_pose_values.size(); i++){ - init_pose3Out << i << " " << initial_pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, - " ", " ")) << endl; + if (output_poses) { + init_pose3Out.open(init_poseOutput.c_str(), ios::out); + for (size_t i = 1; i <= initial_pose_values.size(); i++) { + init_pose3Out + << i << " " + << initial_pose_values.at(Symbol('x', i)).matrix().format( + Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } } @@ -141,7 +143,7 @@ int main(int argc, char** argv){ if(output_poses){ pose3Out.open(poseOutput.c_str(),ios::out); - for(int i = 1; i<=pose_values.size(); i++){ + for(size_t i = 1; i<=pose_values.size(); i++){ pose3Out << i << " " << pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index 3c9247048..70a22b9a5 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -92,7 +92,7 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(azimuth_); ar & BOOST_SERIALIZATION_NVP(elevation_); } diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 9347b9ffb..4ce0eaedb 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -175,7 +175,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(k_); } diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index c48508fa0..9d01e20a5 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -133,7 +133,7 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(T_); ar & BOOST_SERIALIZATION_NVP(z_); } diff --git a/gtsam_unstable/geometry/SimPolygon2D.h b/gtsam_unstable/geometry/SimPolygon2D.h index 02892c519..835bb4083 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.h +++ b/gtsam_unstable/geometry/SimPolygon2D.h @@ -128,7 +128,6 @@ public: }; typedef std::vector SimPolygon2DVector; -typedef std::vector Point2Vector; } //\namespace gtsam diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index cfc508ac7..8d75d767c 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -100,6 +100,12 @@ void Similarity3::print(const std::string& s) const { std::cout << "s: " << scale() << std::endl; } +std::ostream &operator<<(std::ostream &os, const Similarity3& p) { + os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " << + p.scale() << "]\';"; + return os; +} + Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { // Will retracting or localCoordinating R work if R is not a unit rotation? // Also, how do we actually get s out? Seems like we need to store it somewhere. diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 89f1010c4..eec2124ee 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -70,6 +70,8 @@ public: /// Print with optional string void print(const std::string& s) const; + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); + /// @} /// @name Group /// @{ @@ -146,5 +148,5 @@ public: }; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 9aa32e1c3..6b57bfcd0 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -8,6 +8,7 @@ class gtsam::Vector6; class gtsam::LieScalar; class gtsam::LieVector; class gtsam::Point2; +class gtsam::Point2Vector; class gtsam::Rot2; class gtsam::Pose2; class gtsam::Point3; @@ -159,32 +160,6 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; -// std::vector -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - //Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; - #include class SimWall2D { SimWall2D(); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 05b0bb49e..563da4a9f 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -28,25 +28,34 @@ namespace gtsam { /* ************************************************************************* */ -void BatchFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { +void BatchFixedLagSmoother::print(const std::string& s, + const KeyFormatter& keyFormatter) const { FixedLagSmoother::print(s, keyFormatter); // TODO: What else to print? } /* ************************************************************************* */ -bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - const BatchFixedLagSmoother* e = dynamic_cast (&rhs); - return e != NULL - && FixedLagSmoother::equals(*e, tol) - && factors_.equals(e->factors_, tol) - && theta_.equals(e->theta_, tol); +bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, + double tol) const { + const BatchFixedLagSmoother* e = + dynamic_cast(&rhs); + return e != NULL && FixedLagSmoother::equals(*e, tol) + && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol); } /* ************************************************************************* */ -FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { + throw std::runtime_error( + "BatchFixedLagSmoother::marginalCovariance not implemented"); +} + +/* ************************************************************************* */ +FixedLagSmoother::Result BatchFixedLagSmoother::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const KeyTimestampMap& timestamps) { const bool debug = ISDEBUG("BatchFixedLagSmoother update"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::update() START" << std::endl; } @@ -70,11 +79,13 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap // Get current timestamp double current_timestamp = getCurrentTimestamp(); - if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + if (debug) + std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); - if(debug) { + std::set marginalizableKeys = findKeysBefore( + current_timestamp - smootherLag_); + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizableKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -90,19 +101,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap // Optimize gttic(optimize); Result result; - if(factors_.size() > 0) { + if (factors_.size() > 0) { result = optimize(); } gttoc(optimize); // Marginalize out old variables. gttic(marginalize); - if(marginalizableKeys.size() > 0) { + if (marginalizableKeys.size() > 0) { marginalize(marginalizableKeys); } gttoc(marginalize); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl; } @@ -110,11 +121,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap } /* ************************************************************************* */ -void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors) { +void BatchFixedLagSmoother::insertFactors( + const NonlinearFactorGraph& newFactors) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { Key index; // Insert the factor into an existing hole in the factor graph, if possible - if(availableSlots_.size() > 0) { + if (availableSlots_.size() > 0) { index = availableSlots_.front(); availableSlots_.pop(); factors_.replace(index, factor); @@ -130,9 +142,10 @@ void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors } /* ************************************************************************* */ -void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) { +void BatchFixedLagSmoother::removeFactors( + const std::set& deleteFactors) { BOOST_FOREACH(size_t slot, deleteFactors) { - if(factors_.at(slot)) { + if (factors_.at(slot)) { // Remove references to this factor from the FactorIndex BOOST_FOREACH(Key key, *(factors_.at(slot))) { factorIndex_[key].erase(slot); @@ -143,7 +156,8 @@ void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) availableSlots_.push(slot); } else { // TODO: Throw an error?? - std::cout << "Attempting to remove a factor from slot " << slot << ", but it is already NULL." << std::endl; + std::cout << "Attempting to remove a factor from slot " << slot + << ", but it is already NULL." << std::endl; } } } @@ -159,7 +173,7 @@ void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { factorIndex_.erase(key); // Erase the key from the set of linearized keys - if(linearKeys_.exists(key)) { + if (linearKeys_.exists(key)) { linearKeys_.erase(key); } } @@ -178,11 +192,11 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { const bool debug = ISDEBUG("BatchFixedLagSmoother reorder"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::reorder() START" << std::endl; } - if(debug) { + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizeKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -191,13 +205,14 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { } // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); + ordering_ = Ordering::colamdConstrainedFirst(factors_, + std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); - if(debug) { + if (debug) { ordering_.print("New Ordering: "); } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::reorder() FINISH" << std::endl; } } @@ -207,7 +222,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { const bool debug = ISDEBUG("BatchFixedLagSmoother optimize"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::optimize() START" << std::endl; } @@ -231,14 +246,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { result.error = factors_.error(evalpoint); // check if we're already close enough - if(result.error <= errorTol) { - if(debug) { std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " << result.error << " < " << errorTol << std::endl; } + if (result.error <= errorTol) { + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " + << result.error << " < " << errorTol << std::endl; + } return result; } - if(debug) { - std::cout << "BatchFixedLagSmoother::optimize linearValues: " << linearKeys_.size() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize Initial error: " << result.error << std::endl; + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize linearValues: " + << linearKeys_.size() << std::endl; + std::cout << "BatchFixedLagSmoother::optimize Initial error: " + << result.error << std::endl; } // Use a custom optimization loop so the linearization points can be controlled @@ -254,9 +274,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_); // Keep increasing lambda until we make make progress - while(true) { + while (true) { - if(debug) { std::cout << "BatchFixedLagSmoother::optimize trying lambda = " << lambda << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize trying lambda = " + << lambda << std::endl; + } // Add prior factors at the current solution gttic(damp); @@ -267,10 +290,11 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double sigma = 1.0 / std::sqrt(lambda); BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) { size_t dim = key_value.second.size(); - Matrix A = Matrix::Identity(dim,dim); + Matrix A = Matrix::Identity(dim, dim); Vector b = key_value.second; SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); - GaussianFactor::shared_ptr prior(new JacobianFactor(key_value.first, A, b, model)); + GaussianFactor::shared_ptr prior( + new JacobianFactor(key_value.first, A, b, model)); dampedFactorGraph.push_back(prior); } } @@ -279,7 +303,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { gttic(solve); // Solve Damped Gaussian Factor Graph - newDelta = dampedFactorGraph.optimize(ordering_, parameters_.getEliminationFunction()); + newDelta = dampedFactorGraph.optimize(ordering_, + parameters_.getEliminationFunction()); // update the evalpoint with the new delta evalpoint = theta_.retract(newDelta); gttoc(solve); @@ -289,12 +314,14 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double error = factors_.error(evalpoint); gttoc(compute_error); - if(debug) { - std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " << newDelta.norm() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize next error = " << error << std::endl; + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " + << newDelta.norm() << std::endl; + std::cout << "BatchFixedLagSmoother::optimize next error = " << error + << std::endl; } - if(error < result.error) { + if (error < result.error) { // Keep this change // Update the error value result.error = error; @@ -303,7 +330,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Reset the deltas to zeros delta_.setZero(); // Put the linearization points and deltas back for specific variables - if(enforceConsistency_ && (linearKeys_.size() > 0)) { + if (enforceConsistency_ && (linearKeys_.size() > 0)) { theta_.update(linearKeys_); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) { delta_.at(key_value.key) = newDelta.at(key_value.key); @@ -311,16 +338,18 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { } // Decrease lambda for next time lambda /= lambdaFactor; - if(lambda < lambdaLowerBound) { + if (lambda < lambdaLowerBound) { lambda = lambdaLowerBound; } // End this lambda search iteration break; } else { // Reject this change - if(lambda >= lambdaUpperBound) { + if (lambda >= lambdaUpperBound) { // The maximum lambda has been used. Print a warning and end the search. - std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl; + std::cout + << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" + << std::endl; break; } else { // Increase lambda and continue searching @@ -331,15 +360,22 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { } gttoc(optimizer_iteration); - if(debug) { std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda + << std::endl; + } result.iterations++; - } while(result.iterations < maxIterations && - !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); + } while (result.iterations < maxIterations + && !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, + previousError, result.error, NonlinearOptimizerParams::SILENT)); - if(debug) { std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error + << std::endl; + } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::optimize() FINISH" << std::endl; } @@ -356,9 +392,10 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { const bool debug = ISDEBUG("BatchFixedLagSmoother marginalize"); - if(debug) std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl; - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::marginalize Marginalize Keys: "; BOOST_FOREACH(Key key, marginalizeKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -374,7 +411,7 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { removedFactorSlots.insert(slots.begin(), slots.end()); } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::marginalize Removed Factor Slots: "; BOOST_FOREACH(size_t slot, removedFactorSlots) { std::cout << slot << " "; @@ -385,20 +422,24 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { // Add the removed factors to a factor graph NonlinearFactorGraph removedFactors; BOOST_FOREACH(size_t slot, removedFactorSlots) { - if(factors_.at(slot)) { + if (factors_.at(slot)) { removedFactors.push_back(factors_.at(slot)); } } - if(debug) { - PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Removed Factors: "); + if (debug) { + PrintSymbolicGraph(removedFactors, + "BatchFixedLagSmoother::marginalize Removed Factors: "); } // Calculate marginal factors on the remaining keys - NonlinearFactorGraph marginalFactors = calculateMarginalFactors(removedFactors, theta_, marginalizeKeys, parameters_.getEliminationFunction()); + NonlinearFactorGraph marginalFactors = calculateMarginalFactors( + removedFactors, theta_, marginalizeKeys, + parameters_.getEliminationFunction()); - if(debug) { - PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Marginal Factors: "); + if (debug) { + PrintSymbolicGraph(removedFactors, + "BatchFixedLagSmoother::marginalize Marginal Factors: "); } // Remove marginalized factors from the factor graph @@ -412,7 +453,8 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -421,7 +463,8 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::st } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -430,9 +473,10 @@ void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, const s } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) { +void BatchFixedLagSmoother::PrintSymbolicFactor( + const NonlinearFactor::shared_ptr& factor) { std::cout << "f("; - if(factor) { + if (factor) { BOOST_FOREACH(Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); } @@ -443,7 +487,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_pt } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { +void BatchFixedLagSmoother::PrintSymbolicFactor( + const GaussianFactor::shared_ptr& factor) { std::cout << "f("; BOOST_FOREACH(Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -452,7 +497,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph( + const NonlinearFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); @@ -460,59 +506,79 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, + const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); } } - - /* ************************************************************************* */ -NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { +NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors( + const NonlinearFactorGraph& graph, const Values& theta, + const std::set& marginalizeKeys, + const GaussianFactorGraph::Eliminate& eliminateFunction) { const bool debug = ISDEBUG("BatchFixedLagSmoother calculateMarginalFactors"); - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" + << std::endl; - if(debug) PrintKeySet(marginalizeKeys, "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); + if (debug) + PrintKeySet(marginalizeKeys, + "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); // Get the set of all keys involved in the factor graph FastSet allKeys(graph.keys()); - if(debug) PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); + if (debug) + PrintKeySet(allKeys, + "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys FastSet remainingKeys; - std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); - if(debug) PrintKeySet(remainingKeys, "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); + std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), + marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); + if (debug) + PrintKeySet(remainingKeys, + "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); - if(marginalizeKeys.size() == 0) { + if (marginalizeKeys.size() == 0) { // There are no keys to marginalize. Simply return the input factors - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" + << std::endl; return graph; } else { // Create the linear factor graph GaussianFactorGraph linearFactorGraph = *graph.linearize(theta); // .first is the eliminated Bayes tree, while .second is the remaining factor graph - GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; + GaussianFactorGraph marginalLinearFactors = + *linearFactorGraph.eliminatePartialMultifrontal( + std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; // Wrap in nonlinear container factors NonlinearFactorGraph marginalFactors; marginalFactors.reserve(marginalLinearFactors.size()); BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { - marginalFactors += boost::make_shared(gaussianFactor, theta); - if(debug) { - std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; + marginalFactors += boost::make_shared( + gaussianFactor, theta); + if (debug) { + std::cout + << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; PrintSymbolicFactor(marginalFactors.back()); } } - if(debug) PrintSymbolicGraph(marginalFactors, "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); + if (debug) + PrintSymbolicGraph(marginalFactors, + "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" + << std::endl; return marginalFactors; } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 5c66d411f..89da3d7e5 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -100,6 +100,12 @@ public: return delta_; } + /// Calculate marginal covariance on given variable + Matrix marginalCovariance(Key key) const; + + static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, + const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + protected: /** A typedef defining an Key-Factor mapping **/ @@ -134,8 +140,6 @@ protected: /** A cross-reference structure to allow efficient factor lookups by key **/ FactorIndex factorIndex_; - - /** Augment the list of factors with a set of new factors */ void insertFactors(const NonlinearFactorGraph& newFactors); @@ -154,9 +158,6 @@ protected: /** Marginalize out selected variables */ void marginalize(const std::set& marginalizableKeys); - static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); - private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label); diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp index 66d367148..369db51c3 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -37,7 +37,7 @@ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { // Loop through each key and add/update it in the map BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { - // Check to see if this key already exists inthe database + // Check to see if this key already exists in the database KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); // If the key already exists diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index b5556994c..23cd42a0a 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -25,10 +25,13 @@ namespace gtsam { /* ************************************************************************* */ -void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { +void recursiveMarkAffectedKeys(const Key& key, + const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { // Check if the separator keys of the current clique contain the specified key - if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { + if (std::find(clique->conditional()->beginParents(), + clique->conditional()->endParents(), key) + != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique BOOST_FOREACH(Key i, clique->conditional()->frontals()) { @@ -44,32 +47,36 @@ void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& cl } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { +void IncrementalFixedLagSmoother::print(const std::string& s, + const KeyFormatter& keyFormatter) const { FixedLagSmoother::print(s, keyFormatter); // TODO: What else to print? } /* ************************************************************************* */ -bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - const IncrementalFixedLagSmoother* e = dynamic_cast (&rhs); - return e != NULL - && FixedLagSmoother::equals(*e, tol) +bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, + double tol) const { + const IncrementalFixedLagSmoother* e = + dynamic_cast(&rhs); + return e != NULL && FixedLagSmoother::equals(*e, tol) && isam_.equals(e->isam_, tol); } /* ************************************************************************* */ -FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +FixedLagSmoother::Result IncrementalFixedLagSmoother::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const KeyTimestampMap& timestamps) { const bool debug = ISDEBUG("IncrementalFixedLagSmoother update"); - if(debug) { + if (debug) { std::cout << "IncrementalFixedLagSmoother::update() Start" << std::endl; PrintSymbolicTree(isam_, "Bayes Tree Before Update:"); std::cout << "END" << std::endl; } FastVector removedFactors; - boost::optional > constrainedKeys = boost::none; + boost::optional > constrainedKeys = boost::none; // Update the Timestamps associated with the factor keys updateKeyTimestampMap(timestamps); @@ -77,12 +84,14 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Get current timestamp double current_timestamp = getCurrentTimestamp(); - if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + if (debug) + std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); + std::set marginalizableKeys = findKeysBefore( + current_timestamp - smootherLag_); - if(debug) { + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizableKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -93,11 +102,13 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Force iSAM2 to put the marginalizable variables at the beginning createOrderingConstraints(marginalizableKeys, constrainedKeys); - if(debug) { + if (debug) { std::cout << "Constrained Keys: "; - if(constrainedKeys) { - for(FastMap::const_iterator iter = constrainedKeys->begin(); iter != constrainedKeys->end(); ++iter) { - std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second << ") "; + if (constrainedKeys) { + for (FastMap::const_iterator iter = constrainedKeys->begin(); + iter != constrainedKeys->end(); ++iter) { + std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second + << ") "; } } std::cout << std::endl; @@ -114,23 +125,26 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 - ISAM2Result isamResult = isam_.update(newFactors, newTheta, FastVector(), constrainedKeys, boost::none, additionalMarkedKeys); + ISAM2Result isamResult = isam_.update(newFactors, newTheta, + FastVector(), constrainedKeys, boost::none, additionalMarkedKeys); - if(debug) { - PrintSymbolicTree(isam_, "Bayes Tree After Update, Before Marginalization:"); + if (debug) { + PrintSymbolicTree(isam_, + "Bayes Tree After Update, Before Marginalization:"); std::cout << "END" << std::endl; } // Marginalize out any needed variables - if(marginalizableKeys.size() > 0) { - FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); + if (marginalizableKeys.size() > 0) { + FastList leafKeys(marginalizableKeys.begin(), + marginalizableKeys.end()); isam_.marginalizeLeaves(leafKeys); } // Remove marginalized keys from the KeyTimestampMap eraseKeyTimestampMap(marginalizableKeys); - if(debug) { + if (debug) { PrintSymbolicTree(isam_, "Final Bayes Tree:"); std::cout << "END" << std::endl; } @@ -142,7 +156,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact result.nonlinearVariables = 0; result.error = 0; - if(debug) std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl; + if (debug) + std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl; return result; } @@ -151,30 +166,33 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) { TimestampKeyMap::iterator end = timestampKeyMap_.lower_bound(timestamp); TimestampKeyMap::iterator iter = timestampKeyMap_.begin(); - while(iter != end) { + while (iter != end) { keyTimestampMap_.erase(iter->second); timestampKeyMap_.erase(iter++); } } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::createOrderingConstraints(const std::set& marginalizableKeys, boost::optional >& constrainedKeys) const { - if(marginalizableKeys.size() > 0) { - constrainedKeys = FastMap(); +void IncrementalFixedLagSmoother::createOrderingConstraints( + const std::set& marginalizableKeys, + boost::optional >& constrainedKeys) const { + if (marginalizableKeys.size() > 0) { + constrainedKeys = FastMap(); // Generate ordering constraints so that the marginalizable variables will be eliminated first // Set all variables to Group1 BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { constrainedKeys->operator[](timestamp_key.second) = 1; } // Set marginalizable variables to Group0 - BOOST_FOREACH(Key key, marginalizableKeys){ + BOOST_FOREACH(Key key, marginalizableKeys) { constrainedKeys->operator[](key) = 0; } } } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -183,7 +201,8 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const s } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { +void IncrementalFixedLagSmoother::PrintSymbolicFactor( + const GaussianFactor::shared_ptr& factor) { std::cout << "f("; BOOST_FOREACH(gtsam::Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -192,7 +211,8 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shar } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicGraph( + const GaussianFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); @@ -200,27 +220,28 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, + const std::string& label) { std::cout << label << std::endl; - if(!isam.roots().empty()) - { - BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()){ - PrintSymbolicTreeHelper(root); + if (!isam.roots().empty()) { + BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) { + PrintSymbolicTreeHelper(root); } - } - else + } else std::cout << "{Empty Tree}" << std::endl; } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { +void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper( + const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { // Print the current clique std::cout << indent << "P( "; BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) { std::cout << gtsam::DefaultKeyFormatter(key) << " "; } - if(clique->conditional()->nrParents() > 0) std::cout << "| "; + if (clique->conditional()->nrParents() > 0) + std::cout << "| "; BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) { std::cout << gtsam::DefaultKeyFormatter(key) << " "; } @@ -228,7 +249,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Cliq // Recursively print all of the children BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { - PrintSymbolicTreeHelper(child, indent+" "); + PrintSymbolicTreeHelper(child, indent + " "); } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 9f015ef19..31ae20c50 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -23,7 +23,6 @@ #include #include - namespace gtsam { /** @@ -31,7 +30,7 @@ namespace gtsam { * such that the active states are placed in/near the root. This base class implements a function * to calculate the ordering, and an update function to incorporate new factors into the HMF. */ -class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoother { +class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother { public: @@ -39,20 +38,30 @@ public: typedef boost::shared_ptr shared_ptr; /** default constructor */ - IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters = ISAM2Params()) - : FixedLagSmoother(smootherLag), isam_(parameters) {} + IncrementalFixedLagSmoother(double smootherLag = 0.0, + const ISAM2Params& parameters = ISAM2Params()) : + FixedLagSmoother(smootherLag), isam_(parameters) { + } /** destructor */ - virtual ~IncrementalFixedLagSmoother() {} + virtual ~IncrementalFixedLagSmoother() { + } /** Print the factor for debugging and testing (implementing Testable) */ - virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two IncrementalFixedLagSmoother Objects are equal */ virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; - /** Add new factors, updating the solution and relinearizing as needed. */ - Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + /** + * Add new factors, updating the solution and re-linearizing as needed. + * @param newFactors new factors on old and/or new variables + * @param newTheta new values for new variables only + * @param timestamps an (optional) map from keys to real time stamps + */ + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + const Values& newTheta = Values(), // const KeyTimestampMap& timestamps = KeyTimestampMap()); /** Compute an estimate from the incomplete linear delta computed during the last update. @@ -94,6 +103,11 @@ public: return isam_.getDelta(); } + /// Calculate marginal covariance on given variable + Matrix marginalCovariance(Key key) const { + return isam_.marginalCovariance(key); + } + protected: /** An iSAM2 object used to perform inference. The smoother lag is controlled * by what factors are removed each iteration */ @@ -103,16 +117,23 @@ protected: void eraseKeysBefore(double timestamp); /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */ - void createOrderingConstraints(const std::set& marginalizableKeys, boost::optional >& constrainedKeys) const; + void createOrderingConstraints(const std::set& marginalizableKeys, + boost::optional >& constrainedKeys) const; private: /** Private methods for printing debug information */ - static void PrintKeySet(const std::set& keys, const std::string& label = "Keys:"); + static void PrintKeySet(const std::set& keys, const std::string& label = + "Keys:"); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); - static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label = "Factor Graph:"); - static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); - static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = ""); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, + const std::string& label = "Factor Graph:"); + static void PrintSymbolicTree(const gtsam::ISAM2& isam, + const std::string& label = "Bayes Tree:"); + static void PrintSymbolicTreeHelper( + const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = + ""); -}; // IncrementalFixedLagSmoother +}; +// IncrementalFixedLagSmoother -} /// namespace gtsam +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 72c7c66f5..c6710bd70 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -62,7 +62,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("LinearizedGaussianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(lin_points_); @@ -149,7 +149,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("LinearizedJacobianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(Ab_); @@ -264,7 +264,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("LinearizedHessianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(info_); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp deleted file mode 100644 index 7c2f9d9b9..000000000 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ /dev/null @@ -1,247 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testExpressionMeta.cpp - * @date October 14, 2014 - * @author Frank Dellaert - * @brief Test meta-programming constructs for Expressions - */ - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -namespace mpl = boost::mpl; - -#include -#include -template struct Incomplete; - -// Check generation of FunctionalNode -typedef mpl::vector MyTypes; -typedef FunctionalNode::type Generated; -//Incomplete incomplete; - -// Try generating vectors of ExecutionTrace -typedef mpl::vector, ExecutionTrace > ExpectedTraces; - -typedef mpl::transform >::type MyTraces; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >)); - -template -struct MakeTrace { - typedef ExecutionTrace type; -}; -typedef mpl::transform >::type MyTraces1; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >)); - -// Try generating vectors of Expression types -typedef mpl::vector, Expression > ExpectedExpressions; -typedef mpl::transform >::type Expressions; -BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >)); - -// Try generating vectors of Jacobian types -typedef mpl::vector ExpectedJacobians; -typedef mpl::transform >::type Jacobians; -BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >)); - -// Try accessing a Jacobian -typedef mpl::at_c::type Jacobian23; // base zero ! -BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>)); - -/* ************************************************************************* */ -// Boost Fusion includes allow us to create/access values from MPL vectors -#include -#include - -// Create a value and access it -TEST(ExpressionFactor, JacobiansValue) { - using boost::fusion::at_c; - Matrix23 expected; - Jacobians jacobians; - - at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6; - - Matrix23 actual = at_c<1>(jacobians); - CHECK(actual.cols() == expected.cols()); - CHECK(actual.rows() == expected.rows()); -} - -/* ************************************************************************* */ -// Test out polymorphic transform -#include -#include -#include - -struct triple { - template struct result; // says we will provide result - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - // actual function - template - typename result::type operator()(const T& x) const { - return (double) x; - } -}; - -// Test out polymorphic transform -TEST(ExpressionFactor, Triple) { - typedef boost::fusion::vector IntDouble; - IntDouble H = boost::fusion::make_vector(1, 2.0); - - // Only works if I use Double2 - typedef boost::fusion::result_of::transform::type Result; - typedef boost::fusion::vector Double2; - Double2 D = boost::fusion::transform(H, triple()); - - using boost::fusion::at_c; - DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9); - DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9); -} - -/* ************************************************************************* */ -#include -#include -#include -#include - -// Test out invoke -TEST(ExpressionFactor, Invoke) { - EXPECT_LONGS_EQUAL(2, invoke(plus(),boost::fusion::make_vector(1,1))); - - // Creating a Pose3 (is there another way?) - boost::fusion::vector pair; - Pose3 pose = boost::fusion::invoke(boost::value_factory(), pair); -} - -/* ************************************************************************* */ -// debug const issue (how to make read/write arguments for invoke) -struct test { - typedef void result_type; - void operator()(int& a, int& b) const { - a = 6; - b = 7; - } -}; - -TEST(ExpressionFactor, ConstIssue) { - int a, b; - boost::fusion::invoke(test(), - boost::fusion::make_vector(boost::ref(a), boost::ref(b))); - LONGS_EQUAL(6, a); - LONGS_EQUAL(7, b); -} - -/* ************************************************************************* */ -// Test out invoke on a given GTSAM function -// then construct prototype for it's derivatives -TEST(ExpressionFactor, InvokeDerivatives) { - // This is the method in Pose3: - // Point3 transform_to(const Point3& p) const; - // Point3 transform_to(const Point3& p, - // boost::optional Dpose, boost::optional Dpoint) const; - - // Let's assign it it to a boost function object - // cast is needed because Pose3::transform_to is overloaded -// typedef boost::function F; -// F f = static_cast(&Pose3::transform_to); -// -// // Create arguments -// Pose3 pose; -// Point3 point; -// typedef boost::fusion::vector Arguments; -// Arguments args = boost::fusion::make_vector(pose, point); -// -// // Create fused function (takes fusion vector) and call it -// boost::fusion::fused g(f); -// Point3 actual = g(args); -// CHECK(assert_equal(point,actual)); -// -// // We can *immediately* do this using invoke -// Point3 actual2 = boost::fusion::invoke(f, args); -// CHECK(assert_equal(point,actual2)); - - // Now, let's create the optional Jacobian arguments - typedef Point3 T; - typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; - - // Unfortunately this is moot: we need a pointer to a function with the - // optional derivatives; I don't see a way of calling a function that we - // did not get access to by the caller passing us a pointer. - // Let's test below whether we can have a proxy object -} - -struct proxy { - typedef Point3 result_type; - Point3 operator()(const Pose3& pose, const Point3& point) const { - return pose.transform_to(point); - } - Point3 operator()(const Pose3& pose, const Point3& point, - OptionalJacobian<3,6> Dpose, - OptionalJacobian<3,3> Dpoint) const { - return pose.transform_to(point, Dpose, Dpoint); - } -}; - -TEST(ExpressionFactor, InvokeDerivatives2) { - // without derivatives - Pose3 pose; - Point3 point; - Point3 actual = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point)); - CHECK(assert_equal(point,actual)); - - // with derivatives, does not work, const issue again - Matrix36 Dpose; - Matrix3 Dpoint; - Point3 actual2 = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point, boost::ref(Dpose), - boost::ref(Dpoint))); - CHECK(assert_equal(point,actual2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 832d37d14..0b86a60e9 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -17,19 +17,20 @@ */ -#include #include -#include -#include -#include -#include -#include -#include #include #include +#include #include #include +#include +#include +#include +#include #include +#include + +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index dcf43c569..56b1269f0 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -417,7 +417,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index c3f3f1d10..6f39ce1b6 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -95,7 +95,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 614521e76..79a37c2ff 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -652,7 +652,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 8cf22946a..8216942cd 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -574,7 +574,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 857c07761..7f9564ee3 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -114,7 +114,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(dt_); ar & BOOST_SERIALIZATION_NVP(tau_); diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 6b111b759..d8fceeb68 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -386,7 +386,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index efa7f5f7d..ac481f979 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -114,7 +114,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 2998fdc9b..2fd964a35 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -132,7 +132,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 154afbdff..879e1e1dd 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -140,7 +140,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 2e2d1e310..feff0b62c 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -131,7 +131,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); @@ -253,7 +253,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 088bfd2ea..da60c2c31 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -216,7 +216,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 5f6d0ef92..e3080466f 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -136,7 +136,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 87b9f4f5c..d2a9110d9 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -120,7 +120,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 991aae1fd..2de060302 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -101,7 +101,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 0426c3ba4..5b1540c83 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -170,7 +170,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 5906a6664..069274065 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -161,7 +161,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(throwCheirality_); diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index d331053b6..407652583 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -65,7 +65,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 2e3fdcbdb..f4e82d98d 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -200,7 +200,7 @@ public: const Cal3_S2& K = camera.calibration()->calibration(); mono_cameras.push_back(PinholeCamera(pose, K)); } - point_ = triangulatePoint3(mono_cameras, mono_measurements, + point_ = triangulatePoint3 >(mono_cameras, mono_measurements, parameters_.rankTolerance, parameters_.enableEPI); // // // End temporary hack @@ -616,7 +616,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(throwCheirality_); ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index a9cec0f57..bc4d3ccfb 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -212,7 +212,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_all_); } diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 2abc49fa1..156ac242e 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -217,7 +217,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); //ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index adfb9ae36..bf10ae531 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -415,7 +415,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); //ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 20e96e6bf..99a4f90a4 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -73,9 +73,8 @@ TEST( InvDepthFactorVariant1, optimize) { // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - Vector6 actual = result.at(landmarkKey); - +// Vector6 actual = result.at(landmarkKey); // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); // pose1.print("Pose1 Truth:\n"); @@ -116,8 +115,11 @@ TEST( InvDepthFactorVariant1, optimize) { // However, since this is an over-parameterization, there can be // many 6D landmark values that equate to the same 3D world position // Instead, directly test the recovered 3D landmark position - //EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(landmark, world_landmarkAfter, 1e-9)); + + // Frank asks: why commented out? + //Vector6 actual = result.at(landmarkKey); + //EXPECT(assert_equal(expected, actual, 1e-9)); } diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m new file mode 100644 index 000000000..d46493328 --- /dev/null +++ b/matlab/gtsam_tests/testTriangulation.m @@ -0,0 +1,70 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Test triangulation +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Some common constants +sharedCal = Cal3_S2(1500, 1200, 0, 640, 480); + +%% Looking along X-axis, 1 meter above ground plane (x-y) +upright = Rot3.Ypr(-pi / 2, 0., -pi / 2); +pose1 = Pose3(upright, Point3(0, 0, 1)); +camera1 = SimpleCamera(pose1, sharedCal); + +%% create second camera 1 meter to the right of first camera +pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))); +camera2 = SimpleCamera(pose2, sharedCal); + +%% landmark ~5 meters infront of camera +landmark =Point3 (5, 0.5, 1.2); + +%% 1. Project two landmarks into two cameras and triangulate +z1 = camera1.project(landmark); +z2 = camera2.project(landmark); + +%% twoPoses +poses = Pose3Vector; +measurements = Point2Vector; + +poses.push_back(pose1); +poses.push_back(pose2); +measurements.push_back(z1); +measurements.push_back(z2); + +optimize = true; +rank_tol = 1e-9; + +triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); + +%% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) +measurements = Point2Vector; +measurements.push_back(z1.retract([0.1;0.5])); +measurements.push_back(z2.retract([-0.2;0.3])); + +triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2)); + +%% two Poses with Bundler Calibration +bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480); +camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal); +camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal); + +z1 = camera1.project(landmark); +z2 = camera2.project(landmark); + +measurements = Point2Vector; +measurements.push_back(z1); +measurements.push_back(z2); + +triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 4cbe42364..e43fbcf76 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -48,8 +48,10 @@ testVisualISAMExample display 'Starting: testUtilities' testUtilities -display 'Starting: testSerialization' -testSerialization +if(exist('testSerialization.m','file')) + display 'Starting: testSerialization' + testSerialization +end % end of tests display 'Tests complete!' diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 37d2455eb..a1080a6de 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -158,7 +158,7 @@ namespace simulated2D { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } @@ -204,7 +204,7 @@ namespace simulated2D { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } @@ -251,7 +251,7 @@ namespace simulated2D { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 958087c32..312ad89eb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -36,6 +37,10 @@ using namespace gtsam; Point2 measured(-17, 30); SharedNoiseModel model = noiseModel::Unit::Create(2); +// This deals with the overload problem and makes the expressions factor +// understand that we work on Point3 +Point2 (*Project)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; + namespace leaf { // Create some values struct MyValues: public Values { @@ -169,7 +174,7 @@ static Point2 myUncal(const Cal3_S2& K, const Point2& p, // Binary(Leaf,Leaf) TEST(ExpressionFactor, Binary) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; Cal3_S2_ K_(1); Point2_ p_(2); @@ -184,11 +189,15 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(8, sizeof(double)); EXPECT_LONGS_EQUAL(16, sizeof(Point2)); EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian::type)); - EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian::type)); - size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32; + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian::type)); + EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian::type)); + size_t expectedRecordSize = sizeof(Cal3_S2) + + sizeof(internal::ExecutionTrace) + + +sizeof(internal::Jacobian::type) + sizeof(Point2) + + sizeof(internal::ExecutionTrace) + + sizeof(internal::Jacobian::type); EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); // Check size @@ -197,8 +206,8 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = binary.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -212,9 +221,8 @@ TEST(ExpressionFactor, Binary) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT( - assert_equal(expected25, (Matrix) (*r)-> jacobian(), 1e-9)); - EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); + EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) @@ -250,22 +258,22 @@ TEST(ExpressionFactor, Shallow) { LONGS_EQUAL(3,dims[1]); // traceExecution of shallow tree - typedef UnaryExpression Unary; - typedef BinaryExpression Binary; + typedef internal::UnaryExpression Unary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); - EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record)); + EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record)); #ifdef GTSAM_USE_QUATERNIONS EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record)); - LONGS_EQUAL(112+352, expectedTraceSize); + LONGS_EQUAL(96+352, expectedTraceSize); #else - EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record)); - LONGS_EQUAL(112+400, expectedTraceSize); + EXPECT_LONGS_EQUAL(384, sizeof(Binary::Record)); + LONGS_EQUAL(96+384, expectedTraceSize); #endif size_t size = expression.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedTraceSize, size); - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -277,7 +285,7 @@ TEST(ExpressionFactor, Shallow) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9)); // Linearization ExpressionFactor f2(model, measured, expression); @@ -309,7 +317,7 @@ TEST(ExpressionFactor, tree) { // Create expression tree Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat = project(p_cam); + Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); // Create factor and check value, dimension, linearization @@ -483,7 +491,7 @@ TEST(ExpressionFactor, tree_finite_differences) { // Create expression tree Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat = project(p_cam); + Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); const double fd_step = 1e-5; @@ -491,6 +499,11 @@ TEST(ExpressionFactor, tree_finite_differences) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance); } +TEST(ExpressionFactor, push_back) { + NonlinearFactorGraph graph; + graph.addExpressionFactor(model, Point2(0, 0), leaf::p); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp new file mode 100644 index 000000000..d43e7e31a --- /dev/null +++ b/tests/testGeneralSFMFactorB.cpp @@ -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 testGeneralSFMFactorB.cpp + * @author Frank Dellaert + * @brief test general SFM class, with nonlinear optimization and BAL files + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef GeneralSFMFactor, Point3> sfmFactor; +using symbol_shorthand::P; + +/* ************************************************************************* */ +TEST(PinholeCamera, BAL) { + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data db; + bool success = readBAL(filename, db); + if (!success) throw runtime_error("Could not access file!"); + + SharedNoiseModel unit2 = noiseModel::Unit::Create(2); + NonlinearFactorGraph graph; + + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) + graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + } + + Values initial = initialCamerasAndPointsEstimate(db); + + LevenbergMarquardtOptimizer lm(graph, initial); + + Values actual = lm.optimize(); + double actualError = graph.error(actual); + EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-7); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/tests/testLie.cpp b/tests/testLie.cpp new file mode 100644 index 000000000..7be629053 --- /dev/null +++ b/tests/testLie.cpp @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testLie.cpp + * @date May, 2015 + * @author Frank Dellaert + * @brief unit tests for Lie group type machinery + */ + +#include + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +static const double tol = 1e-9; + +//****************************************************************************** +typedef ProductLieGroup Product; + +// Define any direct product group to be a model of the multiplicative Group concept +namespace gtsam { +template<> struct traits : internal::LieGroupTraits { + static void Print(const Product& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second.translation() << "/" + << m.second.theta() << ")" << endl; + } + static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { + return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol); + } +}; +} + +//****************************************************************************** +TEST(Lie, ProductLieGroup) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + Product pair1; + Vector5 d; + d << 1, 2, 0.1, 0.2, 0.3; + Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3))); + Product pair2 = pair1.retract(d); + EXPECT(assert_equal(expected, pair2, 1e-9)); + EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9)); +} + +/* ************************************************************************* */ +Product compose_proxy(const Product& A, const Product& B) { + return A.compose(B); +} +TEST( testProduct, compose ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +Product between_proxy(const Product& A, const Product& B) { + return A.between(B); +} +TEST( testProduct, between ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +Product inverse_proxy(const Product& A) { + return A.inverse(); +} +TEST( testProduct, inverse ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); + EXPECT(assert_equal(numericH1, actH1, tol)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index ef0456146..89b296824 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -10,13 +10,14 @@ * -------------------------------1------------------------------------------- */ /** - * @file testExpression.cpp + * @file testManifold.cpp * @date September 18, 2014 * @author Frank Dellaert * @author Paul Furgale - * @brief unit tests for Block Automatic Differentiation + * @brief unit tests for Manifold type machinery */ +#include #include #include #include @@ -148,6 +149,32 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(zero(3), traits::Local(R, R))); } +//****************************************************************************** +typedef ProductManifold MyPoint2Pair; + +// Define any direct product group to be a model of the multiplicative Group concept +namespace gtsam { +template<> struct traits : internal::ManifoldTraits { + static void Print(const MyPoint2Pair& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second << ")" << endl; + } + static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, double tol = 1e-8) { + return m1 == m2; + } +}; +} + +TEST(Manifold, ProductManifold) { + BOOST_CONCEPT_ASSERT((IsManifold)); + MyPoint2Pair pair1; + Vector4 d; + d << 1,2,3,4; + MyPoint2Pair expected(Point2(1,2),Point2(3,4)); + MyPoint2Pair pair2 = pair1.retract(d); + EXPECT(assert_equal(expected,pair2,1e-9)); + EXPECT(assert_equal(d, pair1.localCoordinates(pair2),1e-9)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/timing/timeCalibratedCamera.cpp b/timing/timeCalibratedCamera.cpp index 0a003a4c7..2d0576aea 100644 --- a/timing/timeCalibratedCamera.cpp +++ b/timing/timeCalibratedCamera.cpp @@ -15,10 +15,10 @@ * @author Frank Dellaert */ -#include -#include - +#include #include +#include +#include using namespace std; using namespace gtsam; diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index 931e2498f..3a4da89b5 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 4ad9d7d47..26eed0207 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -15,10 +15,10 @@ * @author Richard Roberts */ -#include +#include #include -#include +#include using namespace std; using namespace gtsam; diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp new file mode 100644 index 000000000..154a72dc9 --- /dev/null +++ b/timing/timeSFMBAL.cpp @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBAL.cpp + * @brief time structure from motion with BAL file + * @author Frank Dellaert + * @date June 6, 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//#define TERNARY + +int main(int argc, char* argv[]) { + typedef GeneralSFMFactor, Point3> sfmFactor; + using symbol_shorthand::P; + + // Load BAL file (default is tiny) + string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data db; + bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db); + if (!success) throw runtime_error("Could not access file!"); + + // Build graph + SharedNoiseModel unit2 = noiseModel::Unit::Create(2); + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) + graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + } + + Values initial = initialCamerasAndPointsEstimate(db); + +// Create Schur-complement ordering +#ifdef CCOLAMD + vector pointKeys; + for (size_t j = 0; j < db.number_tracks(); j++) pointKeys.push_back(P(j)); + Ordering ordering = Ordering::colamdConstrainedFirst(graph, pointKeys, true); +#else + Ordering ordering; + for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j)); + for (size_t i = 0; i < db.number_cameras(); i++) ordering.push_back(i); +#endif + + // Optimize + // Set parameters to be similar to ceres + LevenbergMarquardtParams params; + params.setOrdering(ordering); + params.setVerbosity("ERROR"); + params.setVerbosityLM("TRYLAMBDA"); + params.setDiagonalDamping(true); + params.setlambdaInitial(1e-4); + params.setlambdaFactor(2.0); + LevenbergMarquardtOptimizer lm(graph, initial, params); + Values actual = lm.optimize(); + + tictoc_finishedIteration_(); + tictoc_print_(); + + return 0; +} diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 1a9c82143..c03c40444 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -41,10 +41,6 @@ static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not define typedef vector strvec; -// NOTE: as this path is only used to generate makefiles, it is hardcoded here for testing -// In practice, this path will be an absolute system path, which makes testing it more annoying -static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap"; - /* ************************************************************************* */ TEST( wrap, ArgumentList ) { ArgumentList args; diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 51dc6f4c3..57a5bce29 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -91,7 +91,8 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; command << "diff --ignore-all-space " << expected << " " << actual << endl; - system(command.str().c_str()); + if (system(command.str().c_str())<0) + throw "command '" + command.str() + "' failed"; cerr << ">>>\n"; } return equal;