diff --git a/.cproject b/.cproject index 9726fec60..ac9b166ec 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..6e9aa0009 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -192,36 +192,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 +393,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 ba616b025..d1d3d93dd 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -270,7 +270,7 @@ function(install_wrapped_library_internal interfaceHeader) if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - if("${build_type_upper}" STREQUAL "RELEASE") + if(${build_type_upper} STREQUAL "RELEASE") set(build_type_tag "") # Don't create release mode tag on installed directory else() set(build_type_tag "${build_type}") @@ -367,13 +367,18 @@ 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() if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - if("${build_type_upper}" STREQUAL "RELEASE") + if(${build_type_upper} STREQUAL "RELEASE") set(build_type_tag "") # Don't create release mode tag on installed directory else() set(build_type_tag "${build_type}") @@ -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 1aee996dc..70f3b566f 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_to_camera(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_to_camera(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/Eigen/.hg_archival.txt b/gtsam/3rdparty/Eigen/.hg_archival.txt index 9fceca658..aea5a5515 100644 --- a/gtsam/3rdparty/Eigen/.hg_archival.txt +++ b/gtsam/3rdparty/Eigen/.hg_archival.txt @@ -1,4 +1,4 @@ repo: 8a21fd850624c931e448cbcfb38168cb2717c790 -node: ffa86ffb557094721ca71dcea6aed2651b9fd610 +node: 10219c95fe653d4962aa9db4946f6fbea96dd740 branch: 3.2 -tag: 3.2.0 +tag: 3.2.4 diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags index 1b9b1142e..7a6e19411 100644 --- a/gtsam/3rdparty/Eigen/.hgtags +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -23,3 +23,7 @@ bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2 da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1 4b687cad1d23066f66863f4f87298447298443df 3.2-rc1 1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2 +ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0 +6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1 +1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2 +36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3 diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index c52b7d1a6..02ab93880 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -442,6 +442,7 @@ LDLT& LDLT::compute(const MatrixType& a) m_transpositions.resize(size); m_isInitialized = false; m_temporary.resize(size); + m_sign = internal::ZeroSign; internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, m_sign); @@ -502,7 +503,6 @@ struct solve_retval, Rhs> using std::abs; using std::max; typedef typename LDLTType::MatrixType MatrixType; - typedef typename LDLTType::Scalar Scalar; typedef typename LDLTType::RealScalar RealScalar; const typename Diagonal::RealReturnType vectorD(dec().vectorD()); // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h index a791bc358..b4641e2a0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h @@ -29,6 +29,11 @@ struct traits > : public traits::type > { typedef ArrayXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits::type >::Flags, + Flags = Flags0 & ~NestByRefBit + }; }; } @@ -149,6 +154,11 @@ struct traits > : public traits::type > { typedef MatrixXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits::type >::Flags, + Flags = Flags0 & ~NestByRefBit + }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index c5800f6c8..04862f374 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -462,8 +462,10 @@ template class DenseBase template RealScalar lpNorm() const; template - const Replicate replicate() const; - const Replicate replicate(Index rowFacor,Index colFactor) const; + inline const Replicate replicate() const; + + typedef Replicate ReplicateReturnType; + inline const ReplicateReturnType replicate(Index rowFacor,Index colFactor) const; typedef Reverse ReverseReturnType; typedef const Reverse ConstReverseReturnType; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h index aab8007b3..68cf6d4b0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h @@ -190,18 +190,18 @@ MatrixBase::diagonal() const * * \sa MatrixBase::diagonal(), class Diagonal */ template -inline typename MatrixBase::template DiagonalIndexReturnType::Type +inline typename MatrixBase::DiagonalDynamicIndexReturnType MatrixBase::diagonal(Index index) { - return typename DiagonalIndexReturnType::Type(derived(), index); + return DiagonalDynamicIndexReturnType(derived(), index); } /** This is the const version of diagonal(Index). */ template -inline typename MatrixBase::template ConstDiagonalIndexReturnType::Type +inline typename MatrixBase::ConstDiagonalDynamicIndexReturnType MatrixBase::diagonal(Index index) const { - return typename ConstDiagonalIndexReturnType::Type(derived(), index); + return ConstDiagonalDynamicIndexReturnType(derived(), index); } /** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h index 2a59d9464..9e805a80f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h @@ -232,7 +232,7 @@ EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest& // FIXME not very good if rhs is real and lhs complex while alpha is real too const Index cols = dest.cols(); for (Index j=0; j diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index ab50c9b81..cebed2bb6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -168,6 +168,7 @@ template class MapBase template class MapBase : public MapBase { + typedef MapBase ReadOnlyMapBase; public: typedef MapBase Base; @@ -230,11 +231,13 @@ template class MapBase Derived& operator=(const MapBase& other) { - Base::Base::operator=(other); + ReadOnlyMapBase::Base::operator=(other); return derived(); } - using Base::Base::operator=; + // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base, + // see bugs 821 and 920. + using ReadOnlyMapBase::Base::operator=; }; #undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index 344b38f2f..cc3279746 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -215,7 +215,7 @@ template class MatrixBase typedef Diagonal DiagonalReturnType; DiagonalReturnType diagonal(); - typedef typename internal::add_const >::type ConstDiagonalReturnType; + typedef typename internal::add_const >::type ConstDiagonalReturnType; ConstDiagonalReturnType diagonal() const; template struct DiagonalIndexReturnType { typedef Diagonal Type; }; @@ -223,16 +223,12 @@ template class MatrixBase template typename DiagonalIndexReturnType::Type diagonal(); template typename ConstDiagonalIndexReturnType::Type diagonal() const; + + typedef Diagonal DiagonalDynamicIndexReturnType; + typedef typename internal::add_const >::type ConstDiagonalDynamicIndexReturnType; - // Note: The "MatrixBase::" prefixes are added to help MSVC9 to match these declarations with the later implementations. - // On the other hand they confuse MSVC8... - #if (defined _MSC_VER) && (_MSC_VER >= 1500) // 2008 or later - typename MatrixBase::template DiagonalIndexReturnType::Type diagonal(Index index); - typename MatrixBase::template ConstDiagonalIndexReturnType::Type diagonal(Index index) const; - #else - typename DiagonalIndexReturnType::Type diagonal(Index index); - typename ConstDiagonalIndexReturnType::Type diagonal(Index index) const; - #endif + DiagonalDynamicIndexReturnType diagonal(Index index); + ConstDiagonalDynamicIndexReturnType diagonal(Index index) const; #ifdef EIGEN2_SUPPORT template typename internal::eigen2_part_return_type::type part(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 1297b8413..f26f3e5cc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -555,7 +555,10 @@ struct permut_matrix_product_retval const Index n = Side==OnTheLeft ? rows() : cols(); // FIXME we need an is_same for expression that is not sensitive to constness. For instance // is_same_xpr, Block >::value should be true. - if(is_same::value && extract_data(dst) == extract_data(m_matrix)) + if( is_same::value + && blas_traits::HasUsableDirectAccess + && blas_traits::HasUsableDirectAccess + && extract_data(dst) == extract_data(m_matrix)) { // apply the permutation inplace Matrix mask(m_permutation.size()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h index a494b5f87..cf74470a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h @@ -85,7 +85,14 @@ class ProductBase : public MatrixBase public: +#ifndef EIGEN_NO_MALLOC + typedef typename Base::PlainObject BasePlainObject; + typedef Matrix DynPlainObject; + typedef typename internal::conditional<(BasePlainObject::SizeAtCompileTime==Dynamic) || (BasePlainObject::SizeAtCompileTime*int(sizeof(Scalar)) < int(EIGEN_STACK_ALLOCATION_LIMIT)), + BasePlainObject, DynPlainObject>::type PlainObject; +#else typedef typename Base::PlainObject PlainObject; +#endif ProductBase(const Lhs& a_lhs, const Rhs& a_rhs) : m_lhs(a_lhs), m_rhs(a_rhs) @@ -180,7 +187,12 @@ namespace internal { template struct nested, N, PlainObject> { - typedef PlainObject const& type; + typedef typename GeneralProduct::PlainObject const& type; +}; +template +struct nested, N, PlainObject> +{ + typedef typename GeneralProduct::PlainObject const& type; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index cd6d949c4..df87b1af4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -188,6 +188,8 @@ template class Ref : public RefBase > { typedef internal::traits Traits; + template + inline Ref(const PlainObjectBase& expr); public: typedef RefBase Base; @@ -196,20 +198,21 @@ template class Ref #ifndef EIGEN_PARSED_BY_DOXYGEN template - inline Ref(PlainObjectBase& expr, - typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) + inline Ref(PlainObjectBase& expr) { - Base::construct(expr); + EIGEN_STATIC_ASSERT(static_cast(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + Base::construct(expr.derived()); } template - inline Ref(const DenseBase& expr, - typename internal::enable_if::value&&bool(Traits::template match::MatchAtCompileTime)),Derived>::type* = 0, - int = Derived::ThisConstantIsPrivateInPlainObjectBase) + inline Ref(const DenseBase& expr) #else template inline Ref(DenseBase& expr) #endif { + EIGEN_STATIC_ASSERT(static_cast(internal::is_lvalue::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); + EIGEN_STATIC_ASSERT(static_cast(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + enum { THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY = Derived::ThisConstantIsPrivateInPlainObjectBase}; Base::construct(expr.const_cast_derived()); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h index dde86a834..ac4537c14 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h @@ -135,7 +135,7 @@ template class Replicate */ template template -inline const Replicate +const Replicate DenseBase::replicate() const { return Replicate(derived()); @@ -150,7 +150,7 @@ DenseBase::replicate() const * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate */ template -inline const Replicate +const typename DenseBase::ReplicateReturnType DenseBase::replicate(Index rowFactor,Index colFactor) const { return Replicate(derived(),rowFactor,colFactor); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h index 845ae1aec..4d65392c6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h @@ -380,19 +380,19 @@ template class TriangularView EIGEN_STRONG_INLINE TriangularView& operator=(const ProductBase& other) { setZero(); - return assignProduct(other,1); + return assignProduct(other.derived(),1); } template EIGEN_STRONG_INLINE TriangularView& operator+=(const ProductBase& other) { - return assignProduct(other,1); + return assignProduct(other.derived(),1); } template EIGEN_STRONG_INLINE TriangularView& operator-=(const ProductBase& other) { - return assignProduct(other,-1); + return assignProduct(other.derived(),-1); } @@ -400,25 +400,34 @@ template class TriangularView EIGEN_STRONG_INLINE TriangularView& operator=(const ScaledProduct& other) { setZero(); - return assignProduct(other,other.alpha()); + return assignProduct(other.derived(),other.alpha()); } template EIGEN_STRONG_INLINE TriangularView& operator+=(const ScaledProduct& other) { - return assignProduct(other,other.alpha()); + return assignProduct(other.derived(),other.alpha()); } template EIGEN_STRONG_INLINE TriangularView& operator-=(const ScaledProduct& other) { - return assignProduct(other,-other.alpha()); + return assignProduct(other.derived(),-other.alpha()); } protected: template EIGEN_STRONG_INLINE TriangularView& assignProduct(const ProductBase& prod, const Scalar& alpha); + + template + EIGEN_STRONG_INLINE TriangularView& assignProduct(const TriangularProduct& prod, const Scalar& alpha) + { + lazyAssign(alpha*prod.eval()); + return *this; + } MatrixTypeNested m_matrix; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h index f183d31de..8d9255eef 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h @@ -110,7 +110,7 @@ template<> EIGEN_STRONG_INLINE Packet2cf ploaddup(const std::complex< template<> EIGEN_STRONG_INLINE void pstore >(std::complex * to, const Packet2cf& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((float*)to, from.v); } template<> EIGEN_STRONG_INLINE void pstoreu >(std::complex * to, const Packet2cf& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((float*)to, from.v); } -template<> EIGEN_STRONG_INLINE void prefetch >(const std::complex * addr) { __pld((float *)addr); } +template<> EIGEN_STRONG_INLINE void prefetch >(const std::complex * addr) { EIGEN_ARM_PREFETCH((float *)addr); } template<> EIGEN_STRONG_INLINE std::complex pfirst(const Packet2cf& a) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h index 163bac215..94dfab330 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h @@ -48,9 +48,18 @@ typedef uint32x4_t Packet4ui; #define EIGEN_INIT_NEON_PACKET2(X, Y) {X, Y} #define EIGEN_INIT_NEON_PACKET4(X, Y, Z, W) {X, Y, Z, W} #endif - -#ifndef __pld -#define __pld(x) asm volatile ( " pld [%[addr]]\n" :: [addr] "r" (x) : "cc" ); + +// arm64 does have the pld instruction. If available, let's trust the __builtin_prefetch built-in function +// which available on LLVM and GCC (at least) +#if EIGEN_HAS_BUILTIN(__builtin_prefetch) || defined(__GNUC__) + #define EIGEN_ARM_PREFETCH(ADDR) __builtin_prefetch(ADDR); +#elif defined __pld + #define EIGEN_ARM_PREFETCH(ADDR) __pld(ADDR) +#elif !defined(__aarch64__) + #define EIGEN_ARM_PREFETCH(ADDR) __asm__ __volatile__ ( " pld [%[addr]]\n" :: [addr] "r" (ADDR) : "cc" ); +#else + // by default no explicit prefetching + #define EIGEN_ARM_PREFETCH(ADDR) #endif template<> struct packet_traits : default_packet_traits @@ -209,8 +218,8 @@ template<> EIGEN_STRONG_INLINE void pstore(int* to, const Packet4i& f template<> EIGEN_STRONG_INLINE void pstoreu(float* to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to, from); } template<> EIGEN_STRONG_INLINE void pstoreu(int* to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to, from); } -template<> EIGEN_STRONG_INLINE void prefetch(const float* addr) { __pld(addr); } -template<> EIGEN_STRONG_INLINE void prefetch(const int* addr) { __pld(addr); } +template<> EIGEN_STRONG_INLINE void prefetch(const float* addr) { EIGEN_ARM_PREFETCH(addr); } +template<> EIGEN_STRONG_INLINE void prefetch(const int* addr) { EIGEN_ARM_PREFETCH(addr); } // FIXME only store the 2 first elements ? template<> EIGEN_STRONG_INLINE float pfirst(const Packet4f& a) { float EIGEN_ALIGN16 x[4]; vst1q_f32(x, a); return x[0]; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h index 99cbd0d95..d16f30bb0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h @@ -52,7 +52,7 @@ Packet4f plog(const Packet4f& _x) Packet4i emm0; - Packet4f invalid_mask = _mm_cmplt_ps(x, _mm_setzero_ps()); + Packet4f invalid_mask = _mm_cmpnge_ps(x, _mm_setzero_ps()); // not greater equal is true if x is NaN Packet4f iszero_mask = _mm_cmpeq_ps(x, _mm_setzero_ps()); x = pmax(x, p4f_min_norm_pos); /* cut off denormalized stuff */ @@ -166,7 +166,7 @@ Packet4f pexp(const Packet4f& _x) emm0 = _mm_cvttps_epi32(fx); emm0 = _mm_add_epi32(emm0, p4i_0x7f); emm0 = _mm_slli_epi32(emm0, 23); - return pmul(y, _mm_castsi128_ps(emm0)); + return pmax(pmul(y, Packet4f(_mm_castsi128_ps(emm0))), _x); } template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2d pexp(const Packet2d& _x) @@ -239,7 +239,7 @@ Packet2d pexp(const Packet2d& _x) emm0 = _mm_add_epi32(emm0, p4i_1023_0); emm0 = _mm_slli_epi32(emm0, 20); emm0 = _mm_shuffle_epi32(emm0, _MM_SHUFFLE(1,2,0,3)); - return pmul(x, _mm_castsi128_pd(emm0)); + return pmax(pmul(x, Packet2d(_mm_castsi128_pd(emm0))), _x); } /* evaluation of 4 sines at onces, using SSE2 intrinsics. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h index c06a0df1c..421f925e1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h @@ -90,6 +90,7 @@ struct traits > | (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0), CoeffReadCost = InnerSize == Dynamic ? Dynamic + : InnerSize == 0 ? 0 : InnerSize * (NumTraits::MulCost + LhsCoeffReadCost + RhsCoeffReadCost) + (InnerSize - 1) * NumTraits::AddCost, @@ -133,7 +134,7 @@ class CoeffBasedProduct }; typedef internal::product_coeff_impl ScalarCoeffImpl; typedef CoeffBasedProduct LazyCoeffBasedProductType; @@ -184,7 +185,7 @@ class CoeffBasedProduct { PacketScalar res; internal::product_packet_impl ::run(row, col, m_lhs, m_rhs, res); return res; @@ -262,10 +263,7 @@ struct product_coeff_impl typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar& res) { - eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix"); - res = lhs.coeff(row, 0) * rhs.coeff(0, col); - for(Index i = 1; i < lhs.cols(); ++i) - res += lhs.coeff(row, i) * rhs.coeff(i, col); + res = (lhs.row(row).transpose().cwiseProduct( rhs.col(col) )).sum(); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index 3a010ec6a..6d1e6c133 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h @@ -13,7 +13,7 @@ #define EIGEN_WORLD_VERSION 3 #define EIGEN_MAJOR_VERSION 2 -#define EIGEN_MINOR_VERSION 2 +#define EIGEN_MINOR_VERSION 4 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -96,6 +96,13 @@ #define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t #endif +// Cross compiler wrapper around LLVM's __has_builtin +#ifdef __has_builtin +# define EIGEN_HAS_BUILTIN(x) __has_builtin(x) +#else +# define EIGEN_HAS_BUILTIN(x) 0 +#endif + /** Allows to disable some optimizations which might affect the accuracy of the result. * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them. * They currently include: @@ -247,7 +254,7 @@ namespace Eigen { #if !defined(EIGEN_ASM_COMMENT) #if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) ) - #define EIGEN_ASM_COMMENT(X) asm("#" X) + #define EIGEN_ASM_COMMENT(X) __asm__("#" X) #else #define EIGEN_ASM_COMMENT(X) #endif @@ -306,7 +313,7 @@ namespace Eigen { // just an empty macro ! #define EIGEN_EMPTY -#if defined(_MSC_VER) && (!defined(__INTEL_COMPILER)) +#if defined(_MSC_VER) && (_MSC_VER < 1900) && (!defined(__INTEL_COMPILER)) #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \ using Base::operator =; #elif defined(__clang__) // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index 6c2461725..41dd7db06 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -63,7 +63,7 @@ // Currently, let's include it only on unix systems: #if defined(__unix__) || defined(__unix) #include - #if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0) + #if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || (defined __PGI) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0) #define EIGEN_HAS_POSIX_MEMALIGN 1 #endif #endif @@ -417,6 +417,8 @@ template inline T* conditional_aligned_realloc_new(T* pt template inline T* conditional_aligned_new_auto(size_t size) { + if(size==0) + return 0; // short-cut. Also fixes Bug 884 check_size_for_overflow(size); T *result = reinterpret_cast(conditional_aligned_malloc(sizeof(T)*size)); if(NumTraits::RequireInitialization) @@ -464,9 +466,8 @@ template inline void conditional_aligned_delete_auto(T * template static inline Index first_aligned(const Scalar* array, Index size) { - enum { PacketSize = packet_traits::size, - PacketAlignedMask = PacketSize-1 - }; + static const Index PacketSize = packet_traits::size; + static const Index PacketAlignedMask = PacketSize-1; if(PacketSize==1) { @@ -612,7 +613,6 @@ template class aligned_stack_memory_handler void* operator new(size_t size, const std::nothrow_t&) throw() { \ try { return Eigen::internal::conditional_aligned_malloc(size); } \ catch (...) { return 0; } \ - return 0; \ } #else #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h index 8872c5b64..bac5d9fe9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h @@ -90,7 +90,9 @@ YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED, THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE, THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH, - OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG + OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG, + IMPLICIT_CONVERSION_TO_SCALAR_IS_FOR_INNER_PRODUCT_ONLY, + STORAGE_LAYOUT_DOES_NOT_MATCH }; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h index 3c4773054..781965d2c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h @@ -341,7 +341,7 @@ template::type> str }; template -T* const_cast_ptr(const T* ptr) +inline T* const_cast_ptr(const T* ptr) { return const_cast(ptr); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h index 0e6fdb488..7992d4944 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h @@ -147,7 +147,6 @@ void fitHyperplane(int numPoints, // compute the covariance matrix CovMatrixType covMat = CovMatrixType::Zero(size, size); - VectorType remean = VectorType::Zero(size); for(int i = 0; i < numPoints; ++i) { VectorType diff = (*(points[i]) - mean).conjugate(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h index 5706eeebe..4f36091db 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h @@ -313,7 +313,7 @@ namespace Eigen { using std::abs; using std::sqrt; const Index dim=m_S.cols(); - if (abs(m_S.coeff(i+1,i)==Scalar(0))) + if (abs(m_S.coeff(i+1,i))==Scalar(0)) return; Index z = findSmallDiagEntry(i,i+1); if (z==i-1) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h index 3993046a8..be89de4a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -563,7 +563,6 @@ template struct direct_selfadjoint_eigenvalues::epsilon(); - safeNorm2 *= safeNorm2; if((eivals(2)-eivals(0))<=Eigen::NumTraits::epsilon()) { eivecs.setIdentity(); @@ -577,7 +576,7 @@ template struct direct_selfadjoint_eigenvalues d1 ? 2 : 0; - d0 = d0 > d1 ? d1 : d0; + d0 = d0 > d1 ? d0 : d1; tmp.diagonal().array () -= eivals(k); VectorType cross; @@ -585,19 +584,25 @@ template struct direct_selfadjoint_eigenvaluessafeNorm2) + { eivecs.col(k) = cross / sqrt(n); + } else { n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm(); if(n>safeNorm2) + { eivecs.col(k) = cross / sqrt(n); + } else { n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm(); if(n>safeNorm2) + { eivecs.col(k) = cross / sqrt(n); + } else { // the input matrix and/or the eigenvaues probably contains some inf/NaN, @@ -617,12 +622,16 @@ template struct direct_selfadjoint_eigenvalues::epsilon()) + { eivecs.col(1) = eivecs.col(k).unitOrthogonal(); + } else { - n = (cross = eivecs.col(k).cross(tmp.row(0).normalized())).squaredNorm(); + n = (cross = eivecs.col(k).cross(tmp.row(0))).squaredNorm(); if(n>safeNorm2) + { eivecs.col(1) = cross / sqrt(n); + } else { n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm(); @@ -636,13 +645,14 @@ template struct direct_selfadjoint_eigenvalues::epsilon()) + { + Matrix m; m << v0.transpose(), v1.transpose(); + JacobiSVD > svd(m, ComputeFullV); + result.normal() = svd.matrixV().col(2); + } + else + result.normal() /= norm; result.offset() = -p0.dot(result.normal()); return result; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h index 1cac343a5..a2d59fce1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h @@ -60,6 +60,9 @@ public: /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ inline Rotation2D(const Scalar& a) : m_angle(a) {} + + /** Default constructor wihtout initialization. The represented rotation is undefined. */ + Rotation2D() {} /** \returns the rotation angle */ inline Scalar angle() const { return m_angle; } @@ -81,10 +84,10 @@ public: /** Applies the rotation to a 2D vector */ Vector2 operator* (const Vector2& vec) const { return toRotationMatrix() * vec; } - + template Rotation2D& fromRotationMatrix(const MatrixBase& m); - Matrix2 toRotationMatrix(void) const; + Matrix2 toRotationMatrix() const; /** \returns the spherical interpolation between \c *this and \a other using * parameter \a t. It is in fact equivalent to a linear interpolation. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 56f361566..e786e5356 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -62,6 +62,8 @@ struct transform_construct_from_matrix; template struct transform_take_affine_part; +template struct transform_make_affine; + } // end namespace internal /** \geometry_module \ingroup Geometry_Module @@ -230,8 +232,7 @@ public: inline Transform() { check_template_params(); - if (int(Mode)==Affine) - makeAffine(); + internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix); } inline Transform(const Transform& other) @@ -591,11 +592,7 @@ public: */ void makeAffine() { - if(int(Mode)!=int(AffineCompact)) - { - matrix().template block<1,Dim>(Dim,0).setZero(); - matrix().coeffRef(Dim,Dim) = Scalar(1); - } + internal::transform_make_affine::run(m_matrix); } /** \internal @@ -1079,6 +1076,24 @@ Transform::fromPositionOrientationScale(const MatrixBas namespace internal { +template +struct transform_make_affine +{ + template + static void run(MatrixType &mat) + { + static const int Dim = MatrixType::ColsAtCompileTime-1; + mat.template block<1,Dim>(Dim,0).setZero(); + mat.coeffRef(Dim,Dim) = typename MatrixType::Scalar(1); + } +}; + +template<> +struct transform_make_affine +{ + template static void run(MatrixType &) { } +}; + // selector needed to avoid taking the inverse of a 3x4 matrix template struct projective_transform_inverse diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h index 2b9fb7f88..dd135c21f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h @@ -39,7 +39,6 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, int maxIters = iters; int n = mat.cols(); - x = precond.solve(x); VectorType r = rhs - mat * x; VectorType r0 = r; @@ -143,7 +142,7 @@ struct traits > * SparseMatrix A(n,n); * // fill A and b * BiCGSTAB > solver; - * solver(A); + * solver.compute(A); * x = solver.solve(b); * std::cout << "#iterations: " << solver.iterations() << std::endl; * std::cout << "estimated error: " << solver.error() << std::endl; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h index 1c48f0df7..18cd7d88a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h @@ -219,7 +219,7 @@ class PardisoImpl void pardisoInit(int type) { m_type = type; - bool symmetric = abs(m_type) < 10; + bool symmetric = std::abs(m_type) < 10; m_iparm[0] = 1; // No solver default m_iparm[1] = 3; // use Metis for the ordering m_iparm[2] = 1; // Numbers of processors, value of OMP_NUM_THREADS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index dff9e44eb..c57f2974c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -762,6 +762,7 @@ template class JacobiSVD internal::qr_preconditioner_impl m_qr_precond_morecols; internal::qr_preconditioner_impl m_qr_precond_morerows; + MatrixType m_scaledMatrix; }; template @@ -808,8 +809,9 @@ void JacobiSVD::allocate(Index rows, Index cols, u : 0); m_workMatrix.resize(m_diagSize, m_diagSize); - if(m_cols>m_rows) m_qr_precond_morecols.allocate(*this); - if(m_rows>m_cols) m_qr_precond_morerows.allocate(*this); + if(m_cols>m_rows) m_qr_precond_morecols.allocate(*this); + if(m_rows>m_cols) m_qr_precond_morerows.allocate(*this); + if(m_cols!=m_cols) m_scaledMatrix.resize(rows,cols); } template @@ -826,21 +828,26 @@ JacobiSVD::compute(const MatrixType& matrix, unsig // limit for very small denormal numbers to be considered zero in order to avoid infinite loops (see bug 286) const RealScalar considerAsZero = RealScalar(2) * std::numeric_limits::denorm_min(); + // Scaling factor to reduce over/under-flows + RealScalar scale = matrix.cwiseAbs().maxCoeff(); + if(scale==RealScalar(0)) scale = RealScalar(1); + /*** step 1. The R-SVD step: we use a QR decomposition to reduce to the case of a square matrix */ - if(!m_qr_precond_morecols.run(*this, matrix) && !m_qr_precond_morerows.run(*this, matrix)) + if(m_rows!=m_cols) { - m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize); + m_scaledMatrix = matrix / scale; + m_qr_precond_morecols.run(*this, m_scaledMatrix); + m_qr_precond_morerows.run(*this, m_scaledMatrix); + } + else + { + m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize) / scale; if(m_computeFullU) m_matrixU.setIdentity(m_rows,m_rows); if(m_computeThinU) m_matrixU.setIdentity(m_rows,m_diagSize); if(m_computeFullV) m_matrixV.setIdentity(m_cols,m_cols); if(m_computeThinV) m_matrixV.setIdentity(m_cols, m_diagSize); } - - // Scaling factor to reduce over/under-flows - RealScalar scale = m_workMatrix.cwiseAbs().maxCoeff(); - if(scale==RealScalar(0)) scale = RealScalar(1); - m_workMatrix /= scale; /*** step 2. The main Jacobi SVD iteration. ***/ @@ -861,7 +868,8 @@ JacobiSVD::compute(const MatrixType& matrix, unsig using std::max; RealScalar threshold = (max)(considerAsZero, precision * (max)(abs(m_workMatrix.coeff(p,p)), abs(m_workMatrix.coeff(q,q)))); - if((max)(abs(m_workMatrix.coeff(p,q)),abs(m_workMatrix.coeff(q,p))) > threshold) + // We compare both values to threshold instead of calling max to be robust to NaN (See bug 791) + if(abs(m_workMatrix.coeff(p,q))>threshold || abs(m_workMatrix.coeff(q,p)) > threshold) { finished = false; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h index 17fff96a7..220c6451c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h @@ -69,7 +69,7 @@ class AmbiVector delete[] m_buffer; if (size<1000) { - Index allocSize = (size * sizeof(ListEl))/sizeof(Scalar); + Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1)/sizeof(Scalar); m_allocatedElements = (allocSize*sizeof(Scalar))/sizeof(ListEl); m_buffer = new Scalar[allocSize]; } @@ -88,7 +88,7 @@ class AmbiVector Index copyElements = m_allocatedElements; m_allocatedElements = (std::min)(Index(m_allocatedElements*1.5),m_size); Index allocSize = m_allocatedElements * sizeof(ListEl); - allocSize = allocSize/sizeof(Scalar) + (allocSize%sizeof(Scalar)>0?1:0); + allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar); Scalar* newBuffer = new Scalar[allocSize]; memcpy(newBuffer, m_buffer, copyElements * sizeof(ListEl)); delete[] m_buffer; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 16a20a574..0ede034ba 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -68,6 +68,8 @@ public: const internal::variable_if_dynamic m_outerSize; EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + private: + Index nonZeros() const; }; @@ -82,6 +84,7 @@ class BlockImpl,BlockRows,BlockCols,true typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; typedef typename internal::remove_all::type _MatrixTypeNested; typedef Block BlockType; + typedef Block ConstBlockType; public: enum { IsRowMajor = internal::traits::IsRowMajor }; EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) @@ -245,6 +248,93 @@ public: }; + +template +class BlockImpl,BlockRows,BlockCols,true,Sparse> + : public SparseMatrixBase,BlockRows,BlockCols,true> > +{ + typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef Block BlockType; +public: + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) +protected: + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + + class InnerIterator: public SparseMatrixType::InnerIterator + { + public: + inline InnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator + { + public: + inline ReverseInnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline BlockImpl(const SparseMatrixType& xpr, int i) + : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) + {} + + inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) + {} + + inline const Scalar* valuePtr() const + { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* innerIndexPtr() const + { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* outerIndexPtr() const + { return m_matrix.outerIndexPtr() + m_outerStart; } + + Index nonZeros() const + { + if(m_matrix.isCompressed()) + return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) + - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); + else if(m_outerSize.value()==0) + return 0; + else + return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); + } + + const Scalar& lastCoeff() const + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); + eigen_assert(nonZeros()>0); + if(m_matrix.isCompressed()) + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; + else + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + typename SparseMatrixType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; + +}; + //---------- /** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h index 78411db98..a9084192e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h @@ -306,15 +306,6 @@ class DenseTimeSparseProduct DenseTimeSparseProduct& operator=(const DenseTimeSparseProduct&); }; -// sparse * dense -template -template -inline const typename SparseDenseProductReturnType::Type -SparseMatrixBase::operator*(const MatrixBase &other) const -{ - return typename SparseDenseProductReturnType::Type(derived(), other.derived()); -} - } // end namespace Eigen #endif // EIGEN_SPARSEDENSEPRODUCT_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h index bbcf7fb1c..485e85bec 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h @@ -358,7 +358,8 @@ template class SparseMatrixBase : public EigenBase /** sparse * dense (returns a dense object unless it is an outer product) */ template const typename SparseDenseProductReturnType::Type - operator*(const MatrixBase &other) const; + operator*(const MatrixBase &other) const + { return typename SparseDenseProductReturnType::Type(derived(), other.derived()); } /** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */ SparseSymmetricPermutationProduct twistedBy(const PermutationMatrix& perm) const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h index b85be93f6..75e210009 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h @@ -61,7 +61,7 @@ struct permut_sparsematrix_product_retval for(Index j=0; jcols(); ++j) { for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) { - if(it.row() < j) continue; - if(it.row() == j) + if(it.index() == j) { det *= (std::abs)(it.value()); break; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h index ad6f2183f..b16afd6a4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h @@ -189,8 +189,8 @@ class MappedSuperNodalMatrix::InnerIterator m_idval(mat.colIndexPtr()[outer]), m_startidval(m_idval), m_endidval(mat.colIndexPtr()[outer+1]), - m_idrow(mat.rowIndexPtr()[outer]), - m_endidrow(mat.rowIndexPtr()[outer+1]) + m_idrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]]), + m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]+1]) {} inline InnerIterator& operator++() { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h index 4c6553bf2..a00bd5db1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h @@ -75,7 +75,7 @@ class SparseQR typedef Matrix ScalarVector; typedef PermutationMatrix PermutationType; public: - SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false) + SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false) { } /** Construct a QR factorization of the matrix \a mat. @@ -84,7 +84,7 @@ class SparseQR * * \sa compute() */ - SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false) + SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false) { compute(mat); } @@ -262,6 +262,7 @@ class SparseQR IndexVector m_etree; // Column elimination tree IndexVector m_firstRowElt; // First element in each row bool m_isQSorted; // whether Q is sorted or not + bool m_isEtreeOk; // whether the elimination tree match the initial input matrix template friend struct SparseQR_QProduct; template friend struct SparseQRMatrixQReturnType; @@ -281,9 +282,11 @@ template void SparseQR::analyzePattern(const MatrixType& mat) { eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR"); + // Copy to a column major matrix if the input is rowmajor + typename internal::conditional::type matCpy(mat); // Compute the column fill reducing ordering OrderingType ord; - ord(mat, m_perm_c); + ord(matCpy, m_perm_c); Index n = mat.cols(); Index m = mat.rows(); Index diagSize = (std::min)(m,n); @@ -296,7 +299,8 @@ void SparseQR::analyzePattern(const MatrixType& mat) // Compute the column elimination tree of the permuted matrix m_outputPerm_c = m_perm_c.inverse(); - internal::coletree(mat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + m_isEtreeOk = true; m_R.resize(m, n); m_Q.resize(m, diagSize); @@ -330,15 +334,38 @@ void SparseQR::factorize(const MatrixType& mat) Index nzcolR, nzcolQ; // Number of nonzero for the current column of R and Q ScalarVector tval(m); // The dense vector used to compute the current column RealScalar pivotThreshold = m_threshold; - + + m_R.setZero(); + m_Q.setZero(); m_pmat = mat; - m_pmat.uncompress(); // To have the innerNonZeroPtr allocated - // Apply the fill-in reducing permutation lazily: - for (int i = 0; i < n; i++) + if(!m_isEtreeOk) { - Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i; - m_pmat.outerIndexPtr()[p] = mat.outerIndexPtr()[i]; - m_pmat.innerNonZeroPtr()[p] = mat.outerIndexPtr()[i+1] - mat.outerIndexPtr()[i]; + m_outputPerm_c = m_perm_c.inverse(); + internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + m_isEtreeOk = true; + } + + m_pmat.uncompress(); // To have the innerNonZeroPtr allocated + + // Apply the fill-in reducing permutation lazily: + { + // If the input is row major, copy the original column indices, + // otherwise directly use the input matrix + // + IndexVector originalOuterIndicesCpy; + const Index *originalOuterIndices = mat.outerIndexPtr(); + if(MatrixType::IsRowMajor) + { + originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1); + originalOuterIndices = originalOuterIndicesCpy.data(); + } + + for (int i = 0; i < n; i++) + { + Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i; + m_pmat.outerIndexPtr()[p] = originalOuterIndices[i]; + m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i]; + } } /* Compute the default threshold as in MatLab, see: @@ -349,6 +376,8 @@ void SparseQR::factorize(const MatrixType& mat) { RealScalar max2Norm = 0.0; for (int j = 0; j < n; j++) max2Norm = (max)(max2Norm, m_pmat.col(j).norm()); + if(max2Norm==RealScalar(0)) + max2Norm = RealScalar(1); pivotThreshold = 20 * (m + n) * max2Norm * NumTraits::epsilon(); } @@ -357,7 +386,7 @@ void SparseQR::factorize(const MatrixType& mat) Index nonzeroCol = 0; // Record the number of valid pivots m_Q.startVec(0); - + // Left looking rank-revealing QR factorization: compute a column of R and Q at a time for (Index col = 0; col < n; ++col) { @@ -373,7 +402,7 @@ void SparseQR::factorize(const MatrixType& mat) // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k. // Note: if the diagonal entry does not exist, then its contribution must be explicitly added, // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found. - for (typename MatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp) + for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp) { Index curIdx = nonzeroCol; if(itp) curIdx = itp.row(); @@ -447,7 +476,7 @@ void SparseQR::factorize(const MatrixType& mat) } } // End update current column - Scalar tau; + Scalar tau = 0; RealScalar beta = 0; if(nonzeroCol < diagSize) @@ -461,7 +490,6 @@ void SparseQR::factorize(const MatrixType& mat) for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq))); if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0)) { - tau = RealScalar(0); beta = numext::real(c0); tval(Qidx(0)) = 1; } @@ -514,6 +542,7 @@ void SparseQR::factorize(const MatrixType& mat) // Recompute the column elimination tree internal::coletree(m_pmat, m_etree, m_firstRowElt, m_pivotperm.indices().data()); + m_isEtreeOk = false; } } @@ -525,13 +554,13 @@ void SparseQR::factorize(const MatrixType& mat) m_R.finalize(); m_R.makeCompressed(); m_isQSorted = false; - + m_nonzeropivots = nonzeroCol; if(nonzeroCol *Mx, double *Ex, void *N return umfpack_zi_get_determinant(&mx_real,0,Ex,NumericHandle,User_Info); } +namespace internal { + template struct umfpack_helper_is_sparse_plain : false_type {}; + template + struct umfpack_helper_is_sparse_plain > + : true_type {}; + template + struct umfpack_helper_is_sparse_plain > + : true_type {}; +} + /** \ingroup UmfPackSupport_Module * \brief A sparse LU factorization and solver based on UmfPack * @@ -192,10 +202,14 @@ class UmfPackLU : internal::noncopyable * Note that the matrix should be column-major, and in compressed format for best performance. * \sa SparseMatrix::makeCompressed(). */ - void compute(const MatrixType& matrix) + template + void compute(const InputMatrixType& matrix) { - analyzePattern(matrix); - factorize(matrix); + if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar()); + if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar()); + grapInput(matrix.derived()); + analyzePattern_impl(); + factorize_impl(); } /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A. @@ -230,23 +244,15 @@ class UmfPackLU : internal::noncopyable * * \sa factorize(), compute() */ - void analyzePattern(const MatrixType& matrix) + template + void analyzePattern(const InputMatrixType& matrix) { - if(m_symbolic) - umfpack_free_symbolic(&m_symbolic,Scalar()); - if(m_numeric) - umfpack_free_numeric(&m_numeric,Scalar()); + if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar()); + if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar()); - grapInput(matrix); + grapInput(matrix.derived()); - int errorCode = 0; - errorCode = umfpack_symbolic(matrix.rows(), matrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, - &m_symbolic, 0, 0); - - m_isInitialized = true; - m_info = errorCode ? InvalidInput : Success; - m_analysisIsOk = true; - m_factorizationIsOk = false; + analyzePattern_impl(); } /** Performs a numeric decomposition of \a matrix @@ -255,20 +261,16 @@ class UmfPackLU : internal::noncopyable * * \sa analyzePattern(), compute() */ - void factorize(const MatrixType& matrix) + template + void factorize(const InputMatrixType& matrix) { eigen_assert(m_analysisIsOk && "UmfPackLU: you must first call analyzePattern()"); if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar()); - grapInput(matrix); - - int errorCode; - errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, - m_symbolic, &m_numeric, 0, 0); - - m_info = errorCode ? NumericalIssue : Success; - m_factorizationIsOk = true; + grapInput(matrix.derived()); + + factorize_impl(); } #ifndef EIGEN_PARSED_BY_DOXYGEN @@ -283,19 +285,20 @@ class UmfPackLU : internal::noncopyable protected: - void init() { - m_info = InvalidInput; - m_isInitialized = false; - m_numeric = 0; - m_symbolic = 0; - m_outerIndexPtr = 0; - m_innerIndexPtr = 0; - m_valuePtr = 0; + m_info = InvalidInput; + m_isInitialized = false; + m_numeric = 0; + m_symbolic = 0; + m_outerIndexPtr = 0; + m_innerIndexPtr = 0; + m_valuePtr = 0; + m_extractedDataAreDirty = true; } - void grapInput(const MatrixType& mat) + template + void grapInput_impl(const InputMatrixType& mat, internal::true_type) { m_copyMatrix.resize(mat.rows(), mat.cols()); if( ((MatrixType::Flags&RowMajorBit)==RowMajorBit) || sizeof(typename MatrixType::Index)!=sizeof(int) || !mat.isCompressed() ) @@ -313,6 +316,45 @@ class UmfPackLU : internal::noncopyable m_valuePtr = mat.valuePtr(); } } + + template + void grapInput_impl(const InputMatrixType& mat, internal::false_type) + { + m_copyMatrix = mat; + m_outerIndexPtr = m_copyMatrix.outerIndexPtr(); + m_innerIndexPtr = m_copyMatrix.innerIndexPtr(); + m_valuePtr = m_copyMatrix.valuePtr(); + } + + template + void grapInput(const InputMatrixType& mat) + { + grapInput_impl(mat, internal::umfpack_helper_is_sparse_plain()); + } + + void analyzePattern_impl() + { + int errorCode = 0; + errorCode = umfpack_symbolic(m_copyMatrix.rows(), m_copyMatrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, + &m_symbolic, 0, 0); + + m_isInitialized = true; + m_info = errorCode ? InvalidInput : Success; + m_analysisIsOk = true; + m_factorizationIsOk = false; + m_extractedDataAreDirty = true; + } + + void factorize_impl() + { + int errorCode; + errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, + m_symbolic, &m_numeric, 0, 0); + + m_info = errorCode ? NumericalIssue : Success; + m_factorizationIsOk = true; + m_extractedDataAreDirty = true; + } // cached data to reduce reallocation, etc. mutable LUMatrixType m_l; diff --git a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake index d9e22ab1a..80b2841df 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake @@ -452,20 +452,12 @@ macro(ei_set_build_string) endmacro(ei_set_build_string) macro(ei_is_64bit_env VAR) - - file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp" - "int main() { return (sizeof(int*) == 8 ? 1 : 0); } - ") - try_run(run_res compile_res - ${CMAKE_CURRENT_BINARY_DIR} "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp" - RUN_OUTPUT_VARIABLE run_output) - - if(compile_res AND run_res) - set(${VAR} ${run_res}) - elseif(CMAKE_CL_64) - set(${VAR} 1) - elseif("$ENV{Platform}" STREQUAL "X64") # nmake 64 bit + if(CMAKE_SIZEOF_VOID_P EQUAL 8) set(${VAR} 1) + elseif(CMAKE_SIZEOF_VOID_P EQUAL 4) + set(${VAR} 0) + else() + message(WARNING "Unsupported pointer size. Please contact the authors.") endif() endmacro(ei_is_64bit_env) diff --git a/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake b/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake index 7b3046d45..23239c300 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake @@ -86,4 +86,4 @@ include(FindPackageHandleStandardArgs) find_package_handle_standard_args(CHOLMOD DEFAULT_MSG CHOLMOD_INCLUDES CHOLMOD_LIBRARIES) -mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY) +mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY CAMD_LIBRARY CCOLAMD_LIBRARY CHOLMOD_METIS_LIBRARY) diff --git a/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake b/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake index a9e9925e7..6c4dc9ab4 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake @@ -115,5 +115,5 @@ include(FindPackageHandleStandardArgs) find_package_handle_standard_args(FFTW DEFAULT_MSG FFTW_INCLUDES FFTW_LIBRARIES) -mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES) +mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES FFTW_LIB FFTWF_LIB FFTWL_LIB) diff --git a/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake b/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake index 627c3e9ae..e0040d320 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake @@ -10,16 +10,50 @@ find_path(METIS_INCLUDES PATHS $ENV{METISDIR} ${INCLUDE_INSTALL_DIR} - PATH_SUFFIXES + PATH_SUFFIXES + . metis include ) +macro(_metis_check_version) + file(READ "${METIS_INCLUDES}/metis.h" _metis_version_header) + + string(REGEX MATCH "define[ \t]+METIS_VER_MAJOR[ \t]+([0-9]+)" _metis_major_version_match "${_metis_version_header}") + set(METIS_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+METIS_VER_MINOR[ \t]+([0-9]+)" _metis_minor_version_match "${_metis_version_header}") + set(METIS_MINOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+METIS_VER_SUBMINOR[ \t]+([0-9]+)" _metis_subminor_version_match "${_metis_version_header}") + set(METIS_SUBMINOR_VERSION "${CMAKE_MATCH_1}") + if(NOT METIS_MAJOR_VERSION) + message(WARNING "Could not determine Metis version. Assuming version 4.0.0") + set(METIS_VERSION 4.0.0) + else() + set(METIS_VERSION ${METIS_MAJOR_VERSION}.${METIS_MINOR_VERSION}.${METIS_SUBMINOR_VERSION}) + endif() + if(${METIS_VERSION} VERSION_LESS ${Metis_FIND_VERSION}) + set(METIS_VERSION_OK FALSE) + else() + set(METIS_VERSION_OK TRUE) + endif() + + if(NOT METIS_VERSION_OK) + message(STATUS "Metis version ${METIS_VERSION} found in ${METIS_INCLUDES}, " + "but at least version ${Metis_FIND_VERSION} is required") + endif(NOT METIS_VERSION_OK) +endmacro(_metis_check_version) + + if(METIS_INCLUDES AND Metis_FIND_VERSION) + _metis_check_version() + else() + set(METIS_VERSION_OK TRUE) + endif() + find_library(METIS_LIBRARIES metis PATHS $ENV{METISDIR} ${LIB_INSTALL_DIR} PATH_SUFFIXES lib) include(FindPackageHandleStandardArgs) find_package_handle_standard_args(METIS DEFAULT_MSG - METIS_INCLUDES METIS_LIBRARIES) + METIS_INCLUDES METIS_LIBRARIES METIS_VERSION_OK) mark_as_advanced(METIS_INCLUDES METIS_LIBRARIES) diff --git a/gtsam/3rdparty/Eigen/cmake/language_support.cmake b/gtsam/3rdparty/Eigen/cmake/language_support.cmake index d687b71f6..231f7ff70 100644 --- a/gtsam/3rdparty/Eigen/cmake/language_support.cmake +++ b/gtsam/3rdparty/Eigen/cmake/language_support.cmake @@ -33,7 +33,7 @@ function(workaround_9220 language language_works) file(WRITE ${CMAKE_BINARY_DIR}/language_tests/${language}/CMakeLists.txt ${text}) execute_process( - COMMAND ${CMAKE_COMMAND} . + COMMAND ${CMAKE_COMMAND} . -G "${CMAKE_GENERATOR}" WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language} RESULT_VARIABLE return_code OUTPUT_QUIET diff --git a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt index 1e74e0528..b9f497f87 100644 --- a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt +++ b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt @@ -67,10 +67,10 @@ P.rightCols() // P(:, end-cols+1:end) P.rightCols(cols) // P(:, end-cols+1:end) P.topRows() // P(1:rows, :) P.topRows(rows) // P(1:rows, :) -P.middleRows(i) // P(:, i+1:i+rows) -P.middleRows(i, rows) // P(:, i+1:i+rows) -P.bottomRows() // P(:, end-rows+1:end) -P.bottomRows(rows) // P(:, end-rows+1:end) +P.middleRows(i) // P(i+1:i+rows, :) +P.middleRows(i, rows) // P(i+1:i+rows, :) +P.bottomRows() // P(end-rows+1:end, :) +P.bottomRows(rows) // P(end-rows+1:end, :) P.topLeftCorner(rows, cols) // P(1:rows, 1:cols) P.topRightCorner(rows, cols) // P(1:rows, end-cols+1:end) P.bottomLeftCorner(rows, cols) // P(end-rows+1:end, 1:cols) diff --git a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox index 4a33d0cc9..d04ac35c5 100644 --- a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox +++ b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox @@ -71,11 +71,10 @@ i.e either row major or column major. The default is column major. Most arithmet Constant or Random Insertion \code -sm1.setZero(); // Set the matrix with zero elements -sm1.setConstant(val); //Replace all the nonzero values with val +sm1.setZero(); \endcode - The matrix sm1 should have been created before ??? +Remove all non-zero coefficients diff --git a/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt index 71b272a15..08cf8efd7 100644 --- a/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt @@ -6,12 +6,10 @@ foreach(example_src ${examples_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(${example} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - get_target_property(example_executable - ${example} LOCATION) add_custom_command( TARGET ${example} POST_BUILD - COMMAND ${example_executable} + COMMAND ${example} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out ) add_dependencies(all_examples ${example}) diff --git a/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt index 92a22ea61..1135900cf 100644 --- a/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt @@ -14,12 +14,10 @@ foreach(snippet_src ${snippets_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(${compile_snippet_target} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - get_target_property(compile_snippet_executable - ${compile_snippet_target} LOCATION) add_custom_command( TARGET ${compile_snippet_target} POST_BUILD - COMMAND ${compile_snippet_executable} + COMMAND ${compile_snippet_target} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${snippet}.out ) add_dependencies(all_snippets ${compile_snippet_target}) @@ -27,4 +25,4 @@ foreach(snippet_src ${snippets_SRCS}) PROPERTIES OBJECT_DEPENDS ${snippet_src}) endforeach(snippet_src) -ei_add_target_property(compile_tut_arithmetic_transpose_aliasing COMPILE_FLAGS -DEIGEN_NO_DEBUG) \ No newline at end of file +ei_add_target_property(compile_tut_arithmetic_transpose_aliasing COMPILE_FLAGS -DEIGEN_NO_DEBUG) diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt index 0c9b3c3ba..f8a0148c8 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt @@ -1,4 +1,3 @@ - if(NOT EIGEN_TEST_NOQT) find_package(Qt4) if(QT4_FOUND) @@ -6,16 +5,16 @@ if(NOT EIGEN_TEST_NOQT) endif() endif(NOT EIGEN_TEST_NOQT) - if(QT4_FOUND) add_executable(Tutorial_sparse_example Tutorial_sparse_example.cpp Tutorial_sparse_example_details.cpp) target_link_libraries(Tutorial_sparse_example ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) - + add_custom_command( TARGET Tutorial_sparse_example POST_BUILD COMMAND Tutorial_sparse_example ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg ) - + add_dependencies(all_examples Tutorial_sparse_example) endif(QT4_FOUND) + diff --git a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt index 5afa2ac82..5c4860237 100644 --- a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt @@ -26,6 +26,12 @@ ei_add_failtest("block_on_const_type_actually_const_1") ei_add_failtest("transpose_on_const_type_actually_const") ei_add_failtest("diagonal_on_const_type_actually_const") +ei_add_failtest("ref_1") +ei_add_failtest("ref_2") +ei_add_failtest("ref_3") +ei_add_failtest("ref_4") +ei_add_failtest("ref_5") + if (EIGEN_FAILTEST_FAILURE_COUNT) message(FATAL_ERROR "${EIGEN_FAILTEST_FAILURE_COUNT} out of ${EIGEN_FAILTEST_COUNT} failtests FAILED. " diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index ccb0fc798..d32451df6 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -66,7 +66,7 @@ endif() find_package(Pastix) find_package(Scotch) -find_package(Metis) +find_package(Metis 5.0 REQUIRED) if(PASTIX_FOUND AND BLAS_FOUND) add_definitions("-DEIGEN_PASTIX_SUPPORT") include_directories(${PASTIX_INCLUDES}) @@ -279,6 +279,7 @@ ei_add_property(EIGEN_TESTING_SUMMARY "CXX_FLAGS: ${CMAKE_CXX_FLAGS}\n") ei_add_property(EIGEN_TESTING_SUMMARY "Sparse lib flags: ${SPARSE_LIBS}\n") option(EIGEN_TEST_EIGEN2 "Run whole Eigen2 test suite against EIGEN2_SUPPORT" OFF) +mark_as_advanced(EIGEN_TEST_EIGEN2) if(EIGEN_TEST_EIGEN2) add_subdirectory(eigen2) endif() diff --git a/gtsam/3rdparty/Eigen/test/cholesky.cpp b/gtsam/3rdparty/Eigen/test/cholesky.cpp index 64bcbccc4..56885deb7 100644 --- a/gtsam/3rdparty/Eigen/test/cholesky.cpp +++ b/gtsam/3rdparty/Eigen/test/cholesky.cpp @@ -320,33 +320,35 @@ template void cholesky_definiteness(const MatrixType& m) { eigen_assert(m.rows() == 2 && m.cols() == 2); MatrixType mat; + LDLT ldlt(2); + { mat << 1, 0, 0, -1; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } { mat << 1, 2, 2, 1; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } { mat << 0, 0, 0, 0; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(ldlt.isNegative()); VERIFY(ldlt.isPositive()); } { mat << 0, 0, 0, 1; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(!ldlt.isNegative()); VERIFY(ldlt.isPositive()); } { mat << -1, 0, 0, 0; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } diff --git a/gtsam/3rdparty/Eigen/test/cwiseop.cpp b/gtsam/3rdparty/Eigen/test/cwiseop.cpp index 247fa2a09..e3361da17 100644 --- a/gtsam/3rdparty/Eigen/test/cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/cwiseop.cpp @@ -9,6 +9,8 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #define EIGEN2_SUPPORT +#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING + #define EIGEN_NO_STATIC_ASSERT #include "main.h" #include diff --git a/gtsam/3rdparty/Eigen/test/dynalloc.cpp b/gtsam/3rdparty/Eigen/test/dynalloc.cpp index 9a6a9d140..7e41bfa97 100644 --- a/gtsam/3rdparty/Eigen/test/dynalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/dynalloc.cpp @@ -87,32 +87,6 @@ template void check_dynaligned() delete obj; } -template void check_custom_new_delete() -{ - { - T* t = new T; - delete t; - } - - { - std::size_t N = internal::random(1,10); - T* t = new T[N]; - delete[] t; - } - -#ifdef EIGEN_ALIGN - { - T* t = static_cast((T::operator new)(sizeof(T))); - (T::operator delete)(t, sizeof(T)); - } - - { - T* t = static_cast((T::operator new)(sizeof(T))); - (T::operator delete)(t); - } -#endif -} - void test_dynalloc() { // low level dynamic memory allocation @@ -128,12 +102,6 @@ void test_dynalloc() CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); - CALL_SUBTEST(check_dynaligned() ); - - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); } // check static allocation, who knows ? diff --git a/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt index 84931e037..9615a6026 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt @@ -4,6 +4,7 @@ add_dependencies(eigen2_check eigen2_buildtests) add_dependencies(buildtests eigen2_buildtests) add_definitions("-DEIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API") +add_definitions("-DEIGEN_NO_EIGEN2_DEPRECATED_WARNING") ei_add_test(eigen2_meta) ei_add_test(eigen2_sizeof) diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp index 8ec9c9202..c0f811459 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp @@ -29,8 +29,6 @@ template void adjoint(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), - identity = SquareMatrixType::Identity(rows, rows), square = SquareMatrixType::Random(rows, rows); VectorType v1 = VectorType::Random(rows), v2 = VectorType::Random(rows), diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp index 4fa16d5ae..dd2dec1ef 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp @@ -23,11 +23,8 @@ template void basicStuff(const MatrixType& m) m2 = MatrixType::Random(rows, cols), m3(rows, cols), mzero = MatrixType::Zero(rows, cols), - identity = Matrix - ::Identity(rows, rows), square = Matrix::Random(rows, rows); VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), vzero = VectorType::Zero(rows); Scalar x = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp index 4391d19b4..22e1cc342 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp @@ -35,11 +35,8 @@ template void cwiseops(const MatrixType& m) mzero = MatrixType::Zero(rows, cols), mones = MatrixType::Ones(rows, cols), identity = Matrix - ::Identity(rows, rows), - square = Matrix::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - vzero = VectorType::Zero(rows), + ::Identity(rows, rows); + VectorType vzero = VectorType::Zero(rows), vones = VectorType::Ones(rows), v3(rows); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp index 70b4ab5f1..514040774 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp @@ -392,6 +392,7 @@ template void geometry(void) #define VERIFY_EULER(I,J,K, X,Y,Z) { \ Vector3 ea = m.eulerAngles(I,J,K); \ Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ + VERIFY_IS_APPROX(m, m1); \ VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ } VERIFY_EULER(0,1,2, X,Y,Z); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp index f83b57249..12d4a71c3 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp @@ -394,6 +394,7 @@ template void geometry(void) #define VERIFY_EULER(I,J,K, X,Y,Z) { \ Vector3 ea = m.eulerAngles(I,J,K); \ Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ + VERIFY_IS_APPROX(m, m1); \ VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ } VERIFY_EULER(0,1,2, X,Y,Z); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp index 5de1dfefa..ccd24a194 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp @@ -25,7 +25,6 @@ template void inverse(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2(rows, cols), - mzero = MatrixType::Zero(rows, cols), identity = MatrixType::Identity(rows, rows); while(ei_abs(m1.determinant()) < RealScalar(0.1) && rows <= 8) diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp index 22f02c396..488f4c485 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp @@ -25,8 +25,7 @@ template void linearStructure(const MatrixType& m) // to test it, hence I consider that we will have tested Random.h MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols); + m3(rows, cols); Scalar s1 = ei_random(); while (ei_abs(s1)<1e-3) s1 = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp index e234abe4b..d34a69999 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp @@ -25,22 +25,12 @@ template void nomalloc(const MatrixType& m) */ typedef typename MatrixType::Scalar Scalar; - typedef Matrix VectorType; int rows = m.rows(); int cols = m.cols(); MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), - identity = Matrix - ::Identity(rows, rows), - square = Matrix - ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); + m2 = MatrixType::Random(rows, cols); Scalar s1 = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp index c5d3f243d..dee970b63 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp @@ -51,16 +51,10 @@ template void submatrices(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), ones = MatrixType::Ones(rows, cols), - identity = Matrix - ::Identity(rows, rows), square = Matrix ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - v3 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); + VectorType v1 = VectorType::Random(rows); Scalar s1 = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp index 3748c7d71..6f17b7dff 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp @@ -13,7 +13,6 @@ template void triangular(const MatrixType& m) { typedef typename MatrixType::Scalar Scalar; typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; RealScalar largerEps = 10*test_precision(); @@ -25,16 +24,7 @@ template void triangular(const MatrixType& m) m3(rows, cols), m4(rows, cols), r1(rows, cols), - r2(rows, cols), - mzero = MatrixType::Zero(rows, cols), - mones = MatrixType::Ones(rows, cols), - identity = Matrix - ::Identity(rows, rows), - square = Matrix - ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); + r2(rows, cols); MatrixType m1up = m1.template part(); MatrixType m2up = m2.template part(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/product.h b/gtsam/3rdparty/Eigen/test/eigen2/product.h index 2c9604d9a..ae1b4bae4 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/product.h +++ b/gtsam/3rdparty/Eigen/test/eigen2/product.h @@ -40,8 +40,7 @@ template void product(const MatrixType& m) // to test it, hence I consider that we will have tested Random.h MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols); + m3(rows, cols); RowSquareMatrixType identity = RowSquareMatrixType::Identity(rows, rows), square = RowSquareMatrixType::Random(rows, rows), @@ -49,9 +48,7 @@ template void product(const MatrixType& m) ColSquareMatrixType square2 = ColSquareMatrixType::Random(cols, cols), res2 = ColSquareMatrixType::Random(cols, cols); - RowVectorType v1 = RowVectorType::Random(rows), - v2 = RowVectorType::Random(rows), - vzero = RowVectorType::Zero(rows); + RowVectorType v1 = RowVectorType::Random(rows); ColVectorType vc2 = ColVectorType::Random(cols), vcres(cols); OtherMajorMatrixType tm1 = m1; diff --git a/gtsam/3rdparty/Eigen/test/eigen2support.cpp b/gtsam/3rdparty/Eigen/test/eigen2support.cpp index ad1d98091..1fa49a8c8 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2support.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2support.cpp @@ -8,6 +8,7 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #define EIGEN2_SUPPORT +#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING #include "main.h" diff --git a/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp b/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp index 5c6ecd875..38689cfbf 100644 --- a/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp +++ b/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp @@ -29,7 +29,21 @@ template void selfadjointeigensolver(const MatrixType& m) MatrixType a = MatrixType::Random(rows,cols); MatrixType a1 = MatrixType::Random(rows,cols); MatrixType symmA = a.adjoint() * a + a1.adjoint() * a1; + MatrixType symmC = symmA; + + // randomly nullify some rows/columns + { + Index count = 1;//internal::random(-cols,cols); + for(Index k=0; k(0,cols-1); + symmA.row(i).setZero(); + symmA.col(i).setZero(); + } + } + symmA.template triangularView().setZero(); + symmC.template triangularView().setZero(); MatrixType b = MatrixType::Random(rows,cols); MatrixType b1 = MatrixType::Random(rows,cols); @@ -40,7 +54,7 @@ template void selfadjointeigensolver(const MatrixType& m) SelfAdjointEigenSolver eiDirect; eiDirect.computeDirect(symmA); // generalized eigen pb - GeneralizedSelfAdjointEigenSolver eiSymmGen(symmA, symmB); + GeneralizedSelfAdjointEigenSolver eiSymmGen(symmC, symmB); VERIFY_IS_EQUAL(eiSymm.info(), Success); VERIFY((symmA.template selfadjointView() * eiSymm.eigenvectors()).isApprox( @@ -57,27 +71,28 @@ template void selfadjointeigensolver(const MatrixType& m) VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmNoEivecs.eigenvalues()); // generalized eigen problem Ax = lBx - eiSymmGen.compute(symmA, symmB,Ax_lBx); + eiSymmGen.compute(symmC, symmB,Ax_lBx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); - VERIFY((symmA.template selfadjointView() * eiSymmGen.eigenvectors()).isApprox( + VERIFY((symmC.template selfadjointView() * eiSymmGen.eigenvectors()).isApprox( symmB.template selfadjointView() * (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); // generalized eigen problem BAx = lx - eiSymmGen.compute(symmA, symmB,BAx_lx); + eiSymmGen.compute(symmC, symmB,BAx_lx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); - VERIFY((symmB.template selfadjointView() * (symmA.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( + VERIFY((symmB.template selfadjointView() * (symmC.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); // generalized eigen problem ABx = lx - eiSymmGen.compute(symmA, symmB,ABx_lx); + eiSymmGen.compute(symmC, symmB,ABx_lx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); - VERIFY((symmA.template selfadjointView() * (symmB.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( + VERIFY((symmC.template selfadjointView() * (symmB.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); + eiSymm.compute(symmC); MatrixType sqrtSymmA = eiSymm.operatorSqrt(); - VERIFY_IS_APPROX(MatrixType(symmA.template selfadjointView()), sqrtSymmA*sqrtSymmA); - VERIFY_IS_APPROX(sqrtSymmA, symmA.template selfadjointView()*eiSymm.operatorInverseSqrt()); + VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView()), sqrtSymmA*sqrtSymmA); + VERIFY_IS_APPROX(sqrtSymmA, symmC.template selfadjointView()*eiSymm.operatorInverseSqrt()); MatrixType id = MatrixType::Identity(rows, cols); VERIFY_IS_APPROX(id.template selfadjointView().operatorNorm(), RealScalar(1)); @@ -95,15 +110,15 @@ template void selfadjointeigensolver(const MatrixType& m) VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt()); // test Tridiagonalization's methods - Tridiagonalization tridiag(symmA); + Tridiagonalization tridiag(symmC); // FIXME tridiag.matrixQ().adjoint() does not work - VERIFY_IS_APPROX(MatrixType(symmA.template selfadjointView()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint()); + VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint()); if (rows > 1) { // Test matrix with NaN - symmA(0,0) = std::numeric_limits::quiet_NaN(); - SelfAdjointEigenSolver eiSymmNaN(symmA); + symmC(0,0) = std::numeric_limits::quiet_NaN(); + SelfAdjointEigenSolver eiSymmNaN(symmC); VERIFY_IS_EQUAL(eiSymmNaN.info(), NoConvergence); } } @@ -113,8 +128,10 @@ void test_eigensolver_selfadjoint() int s = 0; for(int i = 0; i < g_repeat; i++) { // very important to test 3x3 and 2x2 matrices since we provide special paths for them + CALL_SUBTEST_1( selfadjointeigensolver(Matrix2f()) ); CALL_SUBTEST_1( selfadjointeigensolver(Matrix2d()) ); CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) ); + CALL_SUBTEST_1( selfadjointeigensolver(Matrix3d()) ); CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) ); s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(s,s)) ); diff --git a/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp b/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp index f26fc1329..327537801 100644 --- a/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp @@ -114,6 +114,32 @@ template void lines() } } +template void planes() +{ + using std::abs; + typedef Hyperplane Plane; + typedef Matrix Vector; + + for(int i = 0; i < 10; i++) + { + Vector v0 = Vector::Random(); + Vector v1(v0), v2(v0); + if(internal::random(0,1)>0.25) + v1 += Vector::Random(); + if(internal::random(0,1)>0.25) + v2 += v1 * std::pow(internal::random(0,1),internal::random(1,16)); + if(internal::random(0,1)>0.25) + v2 += Vector::Random() * std::pow(internal::random(0,1),internal::random(1,16)); + + Plane p0 = Plane::Through(v0, v1, v2); + + VERIFY_IS_APPROX(p0.normal().norm(), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v0), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v1), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v2), Scalar(1)); + } +} + template void hyperplane_alignment() { typedef Hyperplane Plane3a; @@ -153,5 +179,7 @@ void test_geo_hyperplane() CALL_SUBTEST_4( hyperplane(Hyperplane,5>()) ); CALL_SUBTEST_1( lines() ); CALL_SUBTEST_3( lines() ); + CALL_SUBTEST_2( planes() ); + CALL_SUBTEST_5( planes() ); } } diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index ee3030b5d..547765714 100644 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -98,11 +98,19 @@ template void transformations() Matrix3 matrot1, m; Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI)); - Scalar s0 = internal::random(); + Scalar s0 = internal::random(), + s1 = internal::random(); + + while(v0.norm() < test_precision()) v0 = Vector3::Random(); + while(v1.norm() < test_precision()) v1 = Vector3::Random(); + VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0); VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0); - VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); + if(abs(cos(a)) > test_precision()) + { + VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); + } m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint(); VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized())); VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m); @@ -123,11 +131,18 @@ template void transformations() // angle-axis conversion AngleAxisx aa = AngleAxisx(q1); VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1); + + if(abs(aa.angle()) > NumTraits::dummy_precision()) + { + VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); + } aa.fromRotationMatrix(aa.toRotationMatrix()); VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1); + if(abs(aa.angle()) > NumTraits::dummy_precision()) + { + VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); + } // AngleAxis VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(), @@ -347,7 +362,9 @@ template void transformations() // test transform inversion t0.setIdentity(); t0.translate(v0); - t0.linear().setRandom(); + do { + t0.linear().setRandom(); + } while(t0.linear().jacobiSvd().singularValues()(2)()); Matrix4 t044 = Matrix4::Zero(); t044(3,3) = 1; t044.block(0,0,t0.matrix().rows(),4) = t0.matrix(); @@ -397,6 +414,29 @@ template void transformations() t20 = Translation2(v20) * (Rotation2D(s0) * Eigen::Scaling(s0)); t21 = Translation2(v20) * Rotation2D(s0) * Eigen::Scaling(s0); VERIFY_IS_APPROX(t20,t21); + + Rotation2D R0(s0), R1(s1); + t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0)); + t21 = Translation2(v20) * R0 * Eigen::Scaling(s0); + VERIFY_IS_APPROX(t20,t21); + + t20 = Translation2(v20) * (R0 * R0.inverse() * Eigen::Scaling(s0)); + t21 = Translation2(v20) * Eigen::Scaling(s0); + VERIFY_IS_APPROX(t20,t21); + + VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle()); + VERIFY_IS_APPROX(s1, (R0.slerp(1, R1)).angle()); + VERIFY_IS_APPROX(s0, (R0.slerp(0.5, R0)).angle()); + VERIFY_IS_APPROX(Scalar(0), (R0.slerp(0.5, R0.inverse())).angle()); + + // check basic features + { + Rotation2D r1; // default ctor + r1 = Rotation2D(s0); // copy assignment + VERIFY_IS_APPROX(r1.angle(),s0); + Rotation2D r2(r1); // copy ctor + VERIFY_IS_APPROX(r2.angle(),s0); + } } template void transform_alignment() diff --git a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp index 7c21f0ab3..12c556e59 100644 --- a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp +++ b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp @@ -321,16 +321,23 @@ void jacobisvd_inf_nan() VERIFY(sub(some_inf, some_inf) != sub(some_inf, some_inf)); svd.compute(MatrixType::Constant(10,10,some_inf), ComputeFullU | ComputeFullV); - Scalar some_nan = zero() / zero(); - VERIFY(some_nan != some_nan); - svd.compute(MatrixType::Constant(10,10,some_nan), ComputeFullU | ComputeFullV); + Scalar nan = std::numeric_limits::quiet_NaN(); + VERIFY(nan != nan); + svd.compute(MatrixType::Constant(10,10,nan), ComputeFullU | ComputeFullV); MatrixType m = MatrixType::Zero(10,10); m(internal::random(0,9), internal::random(0,9)) = some_inf; svd.compute(m, ComputeFullU | ComputeFullV); m = MatrixType::Zero(10,10); - m(internal::random(0,9), internal::random(0,9)) = some_nan; + m(internal::random(0,9), internal::random(0,9)) = nan; + svd.compute(m, ComputeFullU | ComputeFullV); + + // regression test for bug 791 + m.resize(3,3); + m << 0, 2*NumTraits::epsilon(), 0.5, + 0, -0.5, 0, + nan, 0, 0; svd.compute(m, ComputeFullU | ComputeFullV); } @@ -434,6 +441,7 @@ void test_jacobisvd() // Test on inf/nan matrix CALL_SUBTEST_7( jacobisvd_inf_nan() ); + CALL_SUBTEST_10( jacobisvd_inf_nan() ); } CALL_SUBTEST_7(( jacobisvd(MatrixXf(internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2), internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) )); diff --git a/gtsam/3rdparty/Eigen/test/main.h b/gtsam/3rdparty/Eigen/test/main.h index 14f0d2f78..664204866 100644 --- a/gtsam/3rdparty/Eigen/test/main.h +++ b/gtsam/3rdparty/Eigen/test/main.h @@ -17,13 +17,36 @@ #include #include #include + +// The following includes of STL headers have to be done _before_ the +// definition of macros min() and max(). The reason is that many STL +// implementations will not work properly as the min and max symbols collide +// with the STL functions std:min() and std::max(). The STL headers may check +// for the macro definition of min/max and issue a warning or undefine the +// macros. +// +// Still, Windows defines min() and max() in windef.h as part of the regular +// Windows system interfaces and many other Windows APIs depend on these +// macros being available. To prevent the macro expansion of min/max and to +// make Eigen compatible with the Windows environment all function calls of +// std::min() and std::max() have to be written with parenthesis around the +// function name. +// +// All STL headers used by Eigen should be included here. Because main.h is +// included before any Eigen header and because the STL headers are guarded +// against multiple inclusions, no STL header will see our own min/max macro +// definitions. #include #include -#include #include #include #include +#include +// To test that all calls from Eigen code to std::min() and std::max() are +// protected by parenthesis against macro expansion, the min()/max() macros +// are defined here and any not-parenthesized min/max call will cause a +// compiler error. #define min(A,B) please_protect_your_min_with_parentheses #define max(A,B) please_protect_your_max_with_parentheses @@ -383,6 +406,26 @@ void randomPermutationVector(PermutationVectorType& v, typename PermutationVecto } } +template bool isNotNaN(const T& x) +{ + return x==x; +} + +template bool isNaN(const T& x) +{ + return x!=x; +} + +template bool isInf(const T& x) +{ + return x > NumTraits::highest(); +} + +template bool isMinusInf(const T& x) +{ + return x < NumTraits::lowest(); +} + } // end namespace Eigen template struct GetDifferentType; diff --git a/gtsam/3rdparty/Eigen/test/nomalloc.cpp b/gtsam/3rdparty/Eigen/test/nomalloc.cpp index cbd02dd21..8e0402358 100644 --- a/gtsam/3rdparty/Eigen/test/nomalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/nomalloc.cpp @@ -165,6 +165,38 @@ void ctms_decompositions() Eigen::JacobiSVD jSVD; jSVD.compute(A, ComputeFullU | ComputeFullV); } +void test_zerosized() { + // default constructors: + Eigen::MatrixXd A; + Eigen::VectorXd v; + // explicit zero-sized: + Eigen::ArrayXXd A0(0,0); + Eigen::ArrayXd v0(std::ptrdiff_t(0)); // FIXME ArrayXd(0) is ambiguous + + // assigning empty objects to each other: + A=A0; + v=v0; +} + +template void test_reference(const MatrixType& m) { + typedef typename MatrixType::Scalar Scalar; + enum { Flag = MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor}; + enum { TransposeFlag = !MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor}; + typename MatrixType::Index rows = m.rows(), cols=m.cols(); + // Dynamic reference: + typedef Eigen::Ref > Ref; + typedef Eigen::Ref > RefT; + + Ref r1(m); + Ref r2(m.block(rows/3, cols/4, rows/2, cols/2)); + RefT r3(m.transpose()); + RefT r4(m.topLeftCorner(rows/2, cols/2).transpose()); + + VERIFY_RAISES_ASSERT(RefT r5(m)); + VERIFY_RAISES_ASSERT(Ref r6(m.transpose())); + VERIFY_RAISES_ASSERT(Ref r7(Scalar(2) * m)); +} + void test_nomalloc() { // check that our operator new is indeed called: @@ -175,5 +207,6 @@ void test_nomalloc() // Check decomposition modules with dynamic matrices that have a known compile-time max size (ctms) CALL_SUBTEST_4(ctms_decompositions()); - + CALL_SUBTEST_5(test_zerosized()); + CALL_SUBTEST_6(test_reference(Matrix())); } diff --git a/gtsam/3rdparty/Eigen/test/nullary.cpp b/gtsam/3rdparty/Eigen/test/nullary.cpp index 5408d88b2..fbc721a1a 100644 --- a/gtsam/3rdparty/Eigen/test/nullary.cpp +++ b/gtsam/3rdparty/Eigen/test/nullary.cpp @@ -80,7 +80,9 @@ void testVectorType(const VectorType& base) Matrix col_vector(size); row_vector.setLinSpaced(size,low,high); col_vector.setLinSpaced(size,low,high); - VERIFY( row_vector.isApprox(col_vector.transpose(), NumTraits::epsilon())); + // when using the extended precision (e.g., FPU) the relative error might exceed 1 bit + // when computing the squared sum in isApprox, thus the 2x factor. + VERIFY( row_vector.isApprox(col_vector.transpose(), Scalar(2)*NumTraits::epsilon())); Matrix size_changer(size+50); size_changer.setLinSpaced(size,low,high); diff --git a/gtsam/3rdparty/Eigen/test/packetmath.cpp b/gtsam/3rdparty/Eigen/test/packetmath.cpp index 2c0519c41..38aa256ce 100644 --- a/gtsam/3rdparty/Eigen/test/packetmath.cpp +++ b/gtsam/3rdparty/Eigen/test/packetmath.cpp @@ -239,6 +239,12 @@ template void packetmath_real() data2[i] = internal::random(-87,88); } CHECK_CWISE1_IF(internal::packet_traits::HasExp, std::exp, internal::pexp); + { + data1[0] = std::numeric_limits::quiet_NaN(); + packet_helper::HasExp,Packet> h; + h.store(data2, internal::pexp(h.load(data1))); + VERIFY(isNaN(data2[0])); + } for (int i=0; i void packetmath_real() } if(internal::random(0,1)<0.1) data1[internal::random(0, PacketSize)] = 0; - CHECK_CWISE1_IF(internal::packet_traits::HasLog, std::log, internal::plog); CHECK_CWISE1_IF(internal::packet_traits::HasSqrt, std::sqrt, internal::psqrt); + CHECK_CWISE1_IF(internal::packet_traits::HasLog, std::log, internal::plog); + { + data1[0] = std::numeric_limits::quiet_NaN(); + packet_helper::HasLog,Packet> h; + h.store(data2, internal::plog(h.load(data1))); + VERIFY(isNaN(data2[0])); + data1[0] = -1.0f; + h.store(data2, internal::plog(h.load(data1))); + VERIFY(isNaN(data2[0])); +#if !EIGEN_FAST_MATH + h.store(data2, internal::psqrt(h.load(data1))); + VERIFY(isNaN(data2[0])); + VERIFY(isNaN(data2[1])); +#endif + } } template void packetmath_notcomplex() diff --git a/gtsam/3rdparty/Eigen/test/product.h b/gtsam/3rdparty/Eigen/test/product.h index 856b234ac..0b3abe402 100644 --- a/gtsam/3rdparty/Eigen/test/product.h +++ b/gtsam/3rdparty/Eigen/test/product.h @@ -139,4 +139,12 @@ template void product(const MatrixType& m) // inner product Scalar x = square2.row(c) * square2.col(c2); VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum()); + + // outer product + VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.row(r).transpose() * m1.col(c).transpose(), m1.block(r,0,1,cols).transpose() * m1.block(0,c,rows,1).transpose()); + VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols)); } diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 19e81549c..32eb31048 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -183,15 +183,15 @@ void call_ref() VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0); VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0); -// call_ref_1(ac); // does not compile because ac is const +// call_ref_1(ac,a +void check_sparse_abs_determinant(Solver& solver, const typename Solver::MatrixType& A, const DenseMat& dA) +{ + using std::abs; + typedef typename Solver::MatrixType Mat; + typedef typename Mat::Scalar Scalar; + + solver.compute(A); + if (solver.info() != Success) + { + std::cerr << "sparse solver testing: factorization failed (check_sparse_abs_determinant)\n"; + return; + } + Scalar refDet = abs(dA.determinant()); + VERIFY_IS_APPROX(refDet,solver.absDeterminant()); +} template int generate_sparse_spd_problem(Solver& , typename Solver::MatrixType& A, typename Solver::MatrixType& halfA, DenseMat& dA, int maxSize = 300) @@ -324,3 +340,20 @@ template void check_sparse_square_determinant(Solver& solver) check_sparse_determinant(solver, A, dA); } } + +template void check_sparse_square_abs_determinant(Solver& solver) +{ + typedef typename Solver::MatrixType Mat; + typedef typename Mat::Scalar Scalar; + typedef Matrix DenseMatrix; + + // generate the problem + Mat A; + DenseMatrix dA; + generate_sparse_square_problem(solver, A, dA, 30); + A.makeCompressed(); + for (int i = 0; i < g_repeat; i++) { + check_sparse_abs_determinant(solver, A, dA); + } +} + diff --git a/gtsam/3rdparty/Eigen/test/sparselu.cpp b/gtsam/3rdparty/Eigen/test/sparselu.cpp index 37980defc..52371cb12 100644 --- a/gtsam/3rdparty/Eigen/test/sparselu.cpp +++ b/gtsam/3rdparty/Eigen/test/sparselu.cpp @@ -44,6 +44,9 @@ template void test_sparselu_T() check_sparse_square_solving(sparselu_colamd); check_sparse_square_solving(sparselu_amd); check_sparse_square_solving(sparselu_natural); + + check_sparse_square_abs_determinant(sparselu_colamd); + check_sparse_square_abs_determinant(sparselu_amd); } void test_sparselu() diff --git a/gtsam/3rdparty/Eigen/test/sparseqr.cpp b/gtsam/3rdparty/Eigen/test/sparseqr.cpp index 1fe4a98ee..451c0e7f8 100644 --- a/gtsam/3rdparty/Eigen/test/sparseqr.cpp +++ b/gtsam/3rdparty/Eigen/test/sparseqr.cpp @@ -10,12 +10,11 @@ #include template -int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 150) +int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300) { - eigen_assert(maxRows >= maxCols); typedef typename MatrixType::Scalar Scalar; int rows = internal::random(1,maxRows); - int cols = internal::random(1,maxCols); + int cols = internal::random(1,rows); double density = (std::max)(8./(rows*cols), 0.01); A.resize(rows,cols); @@ -54,6 +53,8 @@ template void test_sparseqr_scalar() b = dA * DenseVector::Random(A.cols()); solver.compute(A); + if(internal::random(0,1)>0.5) + solver.factorize(A); // this checks that calling analyzePattern is not needed if the pattern do not change. if (solver.info() != Success) { std::cerr << "sparse QR factorization failed\n"; diff --git a/gtsam/3rdparty/Eigen/test/stable_norm.cpp b/gtsam/3rdparty/Eigen/test/stable_norm.cpp index 549f91fbf..231dd9189 100644 --- a/gtsam/3rdparty/Eigen/test/stable_norm.cpp +++ b/gtsam/3rdparty/Eigen/test/stable_norm.cpp @@ -9,11 +9,6 @@ #include "main.h" -template bool isNotNaN(const T& x) -{ - return x==x; -} - // workaround aggressive optimization in ICC template EIGEN_DONT_INLINE T sub(T a, T b) { return a - b; } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport index c4090ab11..e2769449c 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport @@ -178,11 +178,11 @@ template void glLoadMatrix(const Transform& t) template void glLoadMatrix(const Transform& t) { glLoadMatrix(t.matrix()); } template void glLoadMatrix(const Transform& t) { glLoadMatrix(Transform(t).matrix()); } -static void glRotate(const Rotation2D& rot) +inline void glRotate(const Rotation2D& rot) { glRotatef(rot.angle()*180.f/float(M_PI), 0.f, 0.f, 1.f); } -static void glRotate(const Rotation2D& rot) +inline void glRotate(const Rotation2D& rot) { glRotated(rot.angle()*180.0/M_PI, 0.0, 0.0, 1.0); } @@ -246,18 +246,18 @@ EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glGet,GLenum,_,double, 4,4,Doublev) #ifdef GL_VERSION_2_0 -static void glUniform2fv_ei (GLint loc, const float* v) { glUniform2fv(loc,1,v); } -static void glUniform2iv_ei (GLint loc, const int* v) { glUniform2iv(loc,1,v); } +inline void glUniform2fv_ei (GLint loc, const float* v) { glUniform2fv(loc,1,v); } +inline void glUniform2iv_ei (GLint loc, const int* v) { glUniform2iv(loc,1,v); } -static void glUniform3fv_ei (GLint loc, const float* v) { glUniform3fv(loc,1,v); } -static void glUniform3iv_ei (GLint loc, const int* v) { glUniform3iv(loc,1,v); } +inline void glUniform3fv_ei (GLint loc, const float* v) { glUniform3fv(loc,1,v); } +inline void glUniform3iv_ei (GLint loc, const int* v) { glUniform3iv(loc,1,v); } -static void glUniform4fv_ei (GLint loc, const float* v) { glUniform4fv(loc,1,v); } -static void glUniform4iv_ei (GLint loc, const int* v) { glUniform4iv(loc,1,v); } +inline void glUniform4fv_ei (GLint loc, const float* v) { glUniform4fv(loc,1,v); } +inline void glUniform4iv_ei (GLint loc, const int* v) { glUniform4iv(loc,1,v); } -static void glUniformMatrix2fv_ei (GLint loc, const float* v) { glUniformMatrix2fv(loc,1,false,v); } -static void glUniformMatrix3fv_ei (GLint loc, const float* v) { glUniformMatrix3fv(loc,1,false,v); } -static void glUniformMatrix4fv_ei (GLint loc, const float* v) { glUniformMatrix4fv(loc,1,false,v); } +inline void glUniformMatrix2fv_ei (GLint loc, const float* v) { glUniformMatrix2fv(loc,1,false,v); } +inline void glUniformMatrix3fv_ei (GLint loc, const float* v) { glUniformMatrix3fv(loc,1,false,v); } +inline void glUniformMatrix4fv_ei (GLint loc, const float* v) { glUniformMatrix4fv(loc,1,false,v); } EIGEN_GL_FUNC1_DECLARATION (glUniform,GLint,const) @@ -294,9 +294,9 @@ EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 4,3,Matrix #ifdef GL_VERSION_3_0 -static void glUniform2uiv_ei (GLint loc, const unsigned int* v) { glUniform2uiv(loc,1,v); } -static void glUniform3uiv_ei (GLint loc, const unsigned int* v) { glUniform3uiv(loc,1,v); } -static void glUniform4uiv_ei (GLint loc, const unsigned int* v) { glUniform4uiv(loc,1,v); } +inline void glUniform2uiv_ei (GLint loc, const unsigned int* v) { glUniform2uiv(loc,1,v); } +inline void glUniform3uiv_ei (GLint loc, const unsigned int* v) { glUniform3uiv(loc,1,v); } +inline void glUniform4uiv_ei (GLint loc, const unsigned int* v) { glUniform4uiv(loc,1,v); } EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 2,2uiv_ei) EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 3,3uiv_ei) @@ -305,9 +305,9 @@ EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 4,4uiv_ei) #endif #ifdef GL_ARB_gpu_shader_fp64 -static void glUniform2dv_ei (GLint loc, const double* v) { glUniform2dv(loc,1,v); } -static void glUniform3dv_ei (GLint loc, const double* v) { glUniform3dv(loc,1,v); } -static void glUniform4dv_ei (GLint loc, const double* v) { glUniform4dv(loc,1,v); } +inline void glUniform2dv_ei (GLint loc, const double* v) { glUniform2dv(loc,1,v); } +inline void glUniform3dv_ei (GLint loc, const double* v) { glUniform3dv(loc,1,v); } +inline void glUniform4dv_ei (GLint loc, const double* v) { glUniform4dv(loc,1,v); } EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,double, 2,2dv_ei) EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,double, 3,3dv_ei) diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h index c32437281..78a307e96 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h @@ -110,7 +110,6 @@ void MatrixPowerAtomic::compute2x2(MatrixType& res, RealScalar p) co using std::abs; using std::pow; - ArrayType logTdiag = m_A.diagonal().array().log(); res.coeffRef(0,0) = pow(m_A.coeff(0,0), p); for (Index i=1; i < m_A.cols(); ++i) { diff --git a/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt index 978f9afd8..c47646dfc 100644 --- a/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt @@ -10,12 +10,10 @@ FOREACH(example_src ${examples_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(example_${example} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - GET_TARGET_PROPERTY(example_executable - example_${example} LOCATION) ADD_CUSTOM_COMMAND( TARGET example_${example} POST_BUILD - COMMAND ${example_executable} + COMMAND example_${example} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out ) ADD_DEPENDENCIES(unsupported_examples example_${example}) diff --git a/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt index 4a4157933..f0c5cc2a8 100644 --- a/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt @@ -14,12 +14,10 @@ FOREACH(snippet_src ${snippets_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(${compile_snippet_target} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - GET_TARGET_PROPERTY(compile_snippet_executable - ${compile_snippet_target} LOCATION) ADD_CUSTOM_COMMAND( TARGET ${compile_snippet_target} POST_BUILD - COMMAND ${compile_snippet_executable} + COMMAND ${compile_snippet_target} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${snippet}.out ) ADD_DEPENDENCIES(unsupported_snippets ${compile_snippet_target}) diff --git a/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt index a94a3b5e5..2e4cfdb2e 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt @@ -29,11 +29,7 @@ ei_add_test(NonLinearOptimization) ei_add_test(NumericalDiff) ei_add_test(autodiff) - -if (NOT CMAKE_CXX_COMPILER MATCHES "clang\\+\\+$") ei_add_test(BVH) -endif() - ei_add_test(matrix_exponential) ei_add_test(matrix_function) ei_add_test(matrix_power) @@ -73,8 +69,9 @@ if(NOT EIGEN_TEST_NO_OPENGL) find_package(GLUT) find_package(GLEW) if(OPENGL_FOUND AND GLUT_FOUND AND GLEW_FOUND) + include_directories(${OPENGL_INCLUDE_DIR} ${GLUT_INCLUDE_DIR} ${GLEW_INCLUDE_DIRS}) ei_add_property(EIGEN_TESTED_BACKENDS "OpenGL, ") - set(EIGEN_GL_LIB ${GLUT_LIBRARIES} ${GLEW_LIBRARIES}) + set(EIGEN_GL_LIB ${GLUT_LIBRARIES} ${GLEW_LIBRARIES} ${OPENGL_LIBRARIES}) ei_add_test(openglsupport "" "${EIGEN_GL_LIB}" ) else() ei_add_property(EIGEN_MISSING_BACKENDS "OpenGL, ") diff --git a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h index ef0a6a9f0..dddda7dd9 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h +++ b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h @@ -1,59 +1,47 @@ /* - Multi-precision floating point number class for C++. + MPFR C++: Multi-precision floating point number class for C++. Based on MPFR library: http://mpfr.org Project homepage: http://www.holoborodko.com/pavel/mpfr Contact e-mail: pavel@holoborodko.com - Copyright (c) 2008-2012 Pavel Holoborodko + Copyright (c) 2008-2014 Pavel Holoborodko Contributors: Dmitriy Gubanov, Konstantin Holoborodko, Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, Tsai Chia Cheng, - Alexei Zubanov, Jauhien Piatlicki, Victor Berger, John Westwood. + Alexei Zubanov, Jauhien Piatlicki, Victor Berger, John Westwood, + Petr Aleksandrov, Orion Poplawski, Charles Karney. - **************************************************************************** - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + Licensing: + (A) MPFR C++ is under GNU General Public License ("GPL"). + + (B) Non-free licenses may also be purchased from the author, for users who + do not want their programs protected by the GPL. - This library is distributed in the hope that it will be useful, + The non-free licenses are for users that wish to use MPFR C++ in + their products but are unwilling to release their software + under the GPL (which would require them to release source code + and allow free redistribution). + + Such users can purchase an unlimited-use license from the author. + Contact us for more details. + + GNU General Public License ("GPL") copyright permissions statement: + ************************************************************************** + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - - **************************************************************************** - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: - - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - - 3. The name of the author may not be used to endorse or promote products - derived from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - SUCH DAMAGE. + You should have received a copy of the GNU General Public License + along with this program. If not, see . */ #ifndef __MPREAL_H__ @@ -65,11 +53,21 @@ #include #include #include +#include #include // Options #define MPREAL_HAVE_INT64_SUPPORT // Enable int64_t support if possible. Available only for MSVC 2010 & GCC. #define MPREAL_HAVE_MSVC_DEBUGVIEW // Enable Debugger Visualizer for "Debug" builds in MSVC. +#define MPREAL_HAVE_DYNAMIC_STD_NUMERIC_LIMITS // Enable extended std::numeric_limits specialization. + // Meaning that "digits", "round_style" and similar members are defined as functions, not constants. + // See std::numeric_limits at the end of the file for more information. + +// Library version +#define MPREAL_VERSION_MAJOR 3 +#define MPREAL_VERSION_MINOR 5 +#define MPREAL_VERSION_PATCHLEVEL 9 +#define MPREAL_VERSION_STRING "3.5.9" // Detect compiler using signatures from http://predef.sourceforge.net/ #if defined(__GNUC__) && defined(__INTEL_COMPILER) @@ -82,6 +80,32 @@ #define IsInf(x) std::isinf(x) // GNU C/C++ (and/or other compilers), just hope for C99 conformance #endif +// A Clang feature extension to determine compiler features. +#ifndef __has_feature + #define __has_feature(x) 0 +#endif + +// Detect support for r-value references (move semantic). Borrowed from Eigen. +#if (__has_feature(cxx_rvalue_references) || \ + defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L || \ + (defined(_MSC_VER) && _MSC_VER >= 1600)) + + #define MPREAL_HAVE_MOVE_SUPPORT + + // Use fields in mpfr_t structure to check if it was initialized / set dummy initialization + #define mpfr_is_initialized(x) (0 != (x)->_mpfr_d) + #define mpfr_set_uninitialized(x) ((x)->_mpfr_d = 0 ) +#endif + +// Detect support for explicit converters. +#if (__has_feature(cxx_explicit_conversions) || \ + defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L || \ + (defined(_MSC_VER) && _MSC_VER >= 1800)) + + #define MPREAL_HAVE_EXPLICIT_CONVERTERS +#endif + +// Detect available 64-bit capabilities #if defined(MPREAL_HAVE_INT64_SUPPORT) #define MPFR_USE_INTMAX_T // Should be defined before mpfr.h @@ -97,7 +121,7 @@ #endif #elif defined (__GNUC__) && defined(__linux__) - #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(__ia64) || defined(__itanium__) || defined(_M_IA64) + #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(__ia64) || defined(__itanium__) || defined(_M_IA64) || defined (__PPC64__) #undef MPREAL_HAVE_INT64_SUPPORT // Remove all shaman dances for x64 builds since #undef MPFR_USE_INTMAX_T // GCC already supports x64 as of "long int" is 64-bit integer, nothing left to do #else @@ -111,7 +135,7 @@ #endif #if defined(MPREAL_HAVE_MSVC_DEBUGVIEW) && defined(_MSC_VER) && defined(_DEBUG) -#define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString(); + #define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString(); #define MPREAL_MSVC_DEBUGVIEW_DATA std::string DebugView; #else #define MPREAL_MSVC_DEBUGVIEW_CODE @@ -149,7 +173,6 @@ public: // Constructors && type conversions mpreal(); mpreal(const mpreal& u); - mpreal(const mpfr_t u); mpreal(const mpf_t u); mpreal(const mpz_t u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); mpreal(const mpq_t u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); @@ -159,6 +182,10 @@ public: mpreal(const unsigned int u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); mpreal(const long int u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); mpreal(const int u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); + + // Construct mpreal from mpfr_t structure. + // shared = true allows to avoid deep copy, so that mpreal and 'u' share the same data & pointers. + mpreal(const mpfr_t u, bool shared = false); #if defined (MPREAL_HAVE_INT64_SUPPORT) mpreal(const uint64_t u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); @@ -170,6 +197,11 @@ public: ~mpreal(); +#ifdef MPREAL_HAVE_MOVE_SUPPORT + mpreal& operator=(mpreal&& v); + mpreal(mpreal&& u); +#endif + // Operations // = // +, -, *, /, ++, --, <<, >> @@ -292,14 +324,34 @@ public: friend bool operator == (const mpreal& a, const double b); // Type Conversion operators + bool toBool (mp_rnd_t mode = GMP_RNDZ) const; long toLong (mp_rnd_t mode = GMP_RNDZ) const; unsigned long toULong (mp_rnd_t mode = GMP_RNDZ) const; + float toFloat (mp_rnd_t mode = GMP_RNDN) const; double toDouble (mp_rnd_t mode = GMP_RNDN) const; long double toLDouble (mp_rnd_t mode = GMP_RNDN) const; +#if defined (MPREAL_HAVE_EXPLICIT_CONVERTERS) + explicit operator bool () const { return toBool(); } + explicit operator int () const { return toLong(); } + explicit operator long () const { return toLong(); } + explicit operator long long () const { return toLong(); } + explicit operator unsigned () const { return toULong(); } + explicit operator unsigned long () const { return toULong(); } + explicit operator unsigned long long () const { return toULong(); } + explicit operator float () const { return toFloat(); } + explicit operator double () const { return toDouble(); } + explicit operator long double () const { return toLDouble(); } +#endif + #if defined (MPREAL_HAVE_INT64_SUPPORT) int64_t toInt64 (mp_rnd_t mode = GMP_RNDZ) const; uint64_t toUInt64 (mp_rnd_t mode = GMP_RNDZ) const; + + #if defined (MPREAL_HAVE_EXPLICIT_CONVERTERS) + explicit operator int64_t () const { return toInt64(); } + explicit operator uint64_t () const { return toUInt64(); } + #endif #endif // Get raw pointers so that mpreal can be directly used in raw mpfr_* functions @@ -308,121 +360,125 @@ public: ::mpfr_srcptr mpfr_srcptr() const; // Convert mpreal to string with n significant digits in base b - // n = 0 -> convert with the maximum available digits - std::string toString(int n = 0, int b = 10, mp_rnd_t mode = mpreal::get_default_rnd()) const; + // n = -1 -> convert with the maximum available digits + std::string toString(int n = -1, int b = 10, mp_rnd_t mode = mpreal::get_default_rnd()) const; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - std::string toString(const std::string& format) const; + std::string toString(const std::string& format) const; #endif - // Math Functions - friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + std::ostream& output(std::ostream& os) const; - friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + // Math Functions + friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode); + friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode); + friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode); + friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode); + friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode); + + friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode); + friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode); + friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode); + friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode); + friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode); friend int cmpabs(const mpreal& a,const mpreal& b); - friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode); + friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode); + friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); - friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal lgamma (const mpreal& v, int *signp, mp_rnd_t rnd_mode); + friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode); + friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode); + friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode); + friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode); friend int sgn(const mpreal& v); // returns -1 or +1 // MPFR 2.4.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - friend int sinh_cosh (mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal li2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rec_sqrt (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend int sinh_cosh (mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal li2 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); + friend const mpreal rec_sqrt (const mpreal& v, mp_rnd_t rnd_mode); // MATLAB's semantic equivalents - friend const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); // Remainder after division - friend const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); // Modulus after division + friend const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); // Remainder after division + friend const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); // Modulus after division #endif // MPFR 3.0.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - friend const mpreal digamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal ai (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal digamma (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal ai (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal grandom (gmp_randstate_t& state, mp_rnd_t rnd_mode); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal grandom (unsigned int seed); #endif // Uniformly distributed random number generation in [0,1] using // Mersenne-Twister algorithm by default. // Use parameter to setup seed, e.g.: random((unsigned)time(NULL)) // Check urandom() for more precise control. - friend const mpreal random(unsigned int seed = 0); - + friend const mpreal random(unsigned int seed); + // Exponent and mantissa manipulation friend const mpreal frexp(const mpreal& v, mp_exp_t* exp); friend const mpreal ldexp(const mpreal& v, mp_exp_t exp); @@ -433,31 +489,31 @@ public: // Constants // don't forget to call mpfr_free_cache() for every thread where you are using const-functions - friend const mpreal const_log2 (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal const_pi (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal const_euler (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal const_catalan (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal const_log2 (mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal const_pi (mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal const_euler (mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal const_catalan (mp_prec_t prec, mp_rnd_t rnd_mode); // returns +inf iff sign>=0 otherwise -inf - friend const mpreal const_infinity(int sign = 1, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal const_infinity(int sign, mp_prec_t prec); // Output/ Input friend std::ostream& operator<<(std::ostream& os, const mpreal& v); friend std::istream& operator>>(std::istream& is, mpreal& v); // Integer Related Functions - friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode); friend const mpreal ceil (const mpreal& v); friend const mpreal floor(const mpreal& v); friend const mpreal round(const mpreal& v); friend const mpreal trunc(const mpreal& v); - friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rint_floor (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rint_round (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rint_trunc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal remainder ( const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal rint_floor (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal rint_round (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal rint_trunc (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal remainder ( const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); + friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); // Miscellaneous Functions friend const mpreal nexttoward (const mpreal& x, const mpreal& y); @@ -524,19 +580,22 @@ public: // Efficient swapping of two mpreal values - needed for std algorithms friend void swap(mpreal& x, mpreal& y); - friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); + friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); private: // Human friendly Debug Preview in Visual Studio. // Put one of these lines: // - // mpfr::mpreal= ; Show value only + // mpfr::mpreal= ; Show value only // mpfr::mpreal=, bits ; Show value & precision // // at the beginning of // [Visual Studio Installation Folder]\Common7\Packages\Debugger\autoexp.dat MPREAL_MSVC_DEBUGVIEW_DATA + + // "Smart" resources deallocation. Checks if instance initialized before deletion. + void clear(::mpfr_ptr); }; ////////////////////////////////////////////////////////////////////////// @@ -551,64 +610,89 @@ public: // Default constructor: creates mp number and initializes it to 0. inline mpreal::mpreal() { - mpfr_init2(mp,mpreal::get_default_prec()); - mpfr_set_ui(mp,0,mpreal::get_default_rnd()); + mpfr_init2 (mpfr_ptr(), mpreal::get_default_prec()); + mpfr_set_ui(mpfr_ptr(), 0, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpreal& u) { - mpfr_init2(mp,mpfr_get_prec(u.mp)); - mpfr_set(mp,u.mp,mpreal::get_default_rnd()); + mpfr_init2(mpfr_ptr(),mpfr_get_prec(u.mpfr_srcptr())); + mpfr_set (mpfr_ptr(),u.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } -inline mpreal::mpreal(const mpfr_t u) +#ifdef MPREAL_HAVE_MOVE_SUPPORT +inline mpreal::mpreal(mpreal&& other) { - mpfr_init2(mp,mpfr_get_prec(u)); - mpfr_set(mp,u,mpreal::get_default_rnd()); + mpfr_set_uninitialized(mpfr_ptr()); // make sure "other" holds no pinter to actual data + mpfr_swap(mpfr_ptr(), other.mpfr_ptr()); + + MPREAL_MSVC_DEBUGVIEW_CODE; +} + +inline mpreal& mpreal::operator=(mpreal&& other) +{ + mpfr_swap(mpfr_ptr(), other.mpfr_ptr()); + + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; +} +#endif + +inline mpreal::mpreal(const mpfr_t u, bool shared) +{ + if(shared) + { + std::memcpy(mpfr_ptr(), u, sizeof(mpfr_t)); + } + else + { + mpfr_init2(mpfr_ptr(), mpfr_get_prec(u)); + mpfr_set (mpfr_ptr(), u, mpreal::get_default_rnd()); + } MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpf_t u) { - mpfr_init2(mp,(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) - mpfr_set_f(mp,u,mpreal::get_default_rnd()); + mpfr_init2(mpfr_ptr(),(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) + mpfr_set_f(mpfr_ptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpz_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_z(mp,u,mode); + mpfr_init2(mpfr_ptr(), prec); + mpfr_set_z(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpq_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_q(mp,u,mode); + mpfr_init2(mpfr_ptr(), prec); + mpfr_set_q(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const double u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp, prec); + mpfr_init2(mpfr_ptr(), prec); #if (MPREAL_DOUBLE_BITS_OVERFLOW > -1) - if(fits_in_bits(u, MPREAL_DOUBLE_BITS_OVERFLOW)) - { - mpfr_set_d(mp, u, mode); - }else - throw conversion_overflow(); + if(fits_in_bits(u, MPREAL_DOUBLE_BITS_OVERFLOW)) + { + mpfr_set_d(mpfr_ptr(), u, mode); + }else + throw conversion_overflow(); #else - mpfr_set_d(mp, u, mode); + mpfr_set_d(mpfr_ptr(), u, mode); #endif MPREAL_MSVC_DEBUGVIEW_CODE; @@ -616,40 +700,40 @@ inline mpreal::mpreal(const double u, mp_prec_t prec, mp_rnd_t mode) inline mpreal::mpreal(const long double u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_ld(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_ld(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const unsigned long int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_ui(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const unsigned int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_ui(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const long int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_si(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_si(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } @@ -657,16 +741,16 @@ inline mpreal::mpreal(const int u, mp_prec_t prec, mp_rnd_t mode) #if defined (MPREAL_HAVE_INT64_SUPPORT) inline mpreal::mpreal(const uint64_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_uj(mp, u, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_uj(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const int64_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_sj(mp, u, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_sj(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } @@ -674,23 +758,31 @@ inline mpreal::mpreal(const int64_t u, mp_prec_t prec, mp_rnd_t mode) inline mpreal::mpreal(const char* s, mp_prec_t prec, int base, mp_rnd_t mode) { - mpfr_init2(mp, prec); - mpfr_set_str(mp, s, base, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_str(mpfr_ptr(), s, base, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const std::string& s, mp_prec_t prec, int base, mp_rnd_t mode) { - mpfr_init2(mp, prec); - mpfr_set_str(mp, s.c_str(), base, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_str(mpfr_ptr(), s.c_str(), base, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } +inline void mpreal::clear(::mpfr_ptr x) +{ +#ifdef MPREAL_HAVE_MOVE_SUPPORT + if(mpfr_is_initialized(x)) +#endif + mpfr_clear(x); +} + inline mpreal::~mpreal() { - mpfr_clear(mp); + clear(mpfr_ptr()); } // internal namespace needed for template magic @@ -761,6 +853,9 @@ const mpreal sqrt(const int v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); const mpreal sqrt(const long double v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); const mpreal sqrt(const double v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +// abs +inline const mpreal abs(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()); + ////////////////////////////////////////////////////////////////////////// // pow const mpreal pow(const mpreal& a, const unsigned int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); @@ -813,6 +908,11 @@ const mpreal pow(const double a, const unsigned int b, mp_rnd_t rnd_mode = mprea const mpreal pow(const double a, const long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + ////////////////////////////////////////////////////////////////////////// // Estimate machine epsilon for the given precision // Returns smallest eps such that 1.0 + eps != 1.0 @@ -860,15 +960,15 @@ inline mpreal& mpreal::operator=(const mpreal& v) { if (this != &v) { - mp_prec_t tp = mpfr_get_prec(mp); - mp_prec_t vp = mpfr_get_prec(v.mp); + mp_prec_t tp = mpfr_get_prec( mpfr_srcptr()); + mp_prec_t vp = mpfr_get_prec(v.mpfr_srcptr()); - if(tp != vp){ - mpfr_clear(mp); - mpfr_init2(mp, vp); - } + if(tp != vp){ + clear(mpfr_ptr()); + mpfr_init2(mpfr_ptr(), vp); + } - mpfr_set(mp, v.mp, mpreal::get_default_rnd()); + mpfr_set(mpfr_ptr(), v.mpfr_srcptr(), mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } @@ -877,7 +977,7 @@ inline mpreal& mpreal::operator=(const mpreal& v) inline mpreal& mpreal::operator=(const mpf_t v) { - mpfr_set_f(mp, v, mpreal::get_default_rnd()); + mpfr_set_f(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -885,7 +985,7 @@ inline mpreal& mpreal::operator=(const mpf_t v) inline mpreal& mpreal::operator=(const mpz_t v) { - mpfr_set_z(mp, v, mpreal::get_default_rnd()); + mpfr_set_z(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -893,7 +993,7 @@ inline mpreal& mpreal::operator=(const mpz_t v) inline mpreal& mpreal::operator=(const mpq_t v) { - mpfr_set_q(mp, v, mpreal::get_default_rnd()); + mpfr_set_q(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -901,7 +1001,7 @@ inline mpreal& mpreal::operator=(const mpq_t v) inline mpreal& mpreal::operator=(const long double v) { - mpfr_set_ld(mp, v, mpreal::get_default_rnd()); + mpfr_set_ld(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -910,22 +1010,22 @@ inline mpreal& mpreal::operator=(const long double v) inline mpreal& mpreal::operator=(const double v) { #if (MPREAL_DOUBLE_BITS_OVERFLOW > -1) - if(fits_in_bits(v, MPREAL_DOUBLE_BITS_OVERFLOW)) - { - mpfr_set_d(mp,v,mpreal::get_default_rnd()); - }else - throw conversion_overflow(); + if(fits_in_bits(v, MPREAL_DOUBLE_BITS_OVERFLOW)) + { + mpfr_set_d(mpfr_ptr(),v,mpreal::get_default_rnd()); + }else + throw conversion_overflow(); #else - mpfr_set_d(mp,v,mpreal::get_default_rnd()); + mpfr_set_d(mpfr_ptr(),v,mpreal::get_default_rnd()); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator=(const unsigned long int v) { - mpfr_set_ui(mp, v, mpreal::get_default_rnd()); + mpfr_set_ui(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -933,7 +1033,7 @@ inline mpreal& mpreal::operator=(const unsigned long int v) inline mpreal& mpreal::operator=(const unsigned int v) { - mpfr_set_ui(mp, v, mpreal::get_default_rnd()); + mpfr_set_ui(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -941,7 +1041,7 @@ inline mpreal& mpreal::operator=(const unsigned int v) inline mpreal& mpreal::operator=(const long int v) { - mpfr_set_si(mp, v, mpreal::get_default_rnd()); + mpfr_set_si(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -949,7 +1049,7 @@ inline mpreal& mpreal::operator=(const long int v) inline mpreal& mpreal::operator=(const int v) { - mpfr_set_si(mp, v, mpreal::get_default_rnd()); + mpfr_set_si(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -966,15 +1066,15 @@ inline mpreal& mpreal::operator=(const char* s) mpfr_t t; - mpfr_init2(t, mpfr_get_prec(mp)); + mpfr_init2(t, mpfr_get_prec(mpfr_srcptr())); if(0 == mpfr_set_str(t, s, 10, mpreal::get_default_rnd())) { - mpfr_set(mp, t, mpreal::get_default_rnd()); + mpfr_set(mpfr_ptr(), t, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } - mpfr_clear(t); + clear(t); return *this; } @@ -989,15 +1089,15 @@ inline mpreal& mpreal::operator=(const std::string& s) mpfr_t t; - mpfr_init2(t, mpfr_get_prec(mp)); + mpfr_init2(t, mpfr_get_prec(mpfr_srcptr())); if(0 == mpfr_set_str(t, s.c_str(), 10, mpreal::get_default_rnd())) { - mpfr_set(mp, t, mpreal::get_default_rnd()); + mpfr_set(mpfr_ptr(), t, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } - mpfr_clear(t); + clear(t); return *this; } @@ -1006,7 +1106,7 @@ inline mpreal& mpreal::operator=(const std::string& s) // + Addition inline mpreal& mpreal::operator+=(const mpreal& v) { - mpfr_add(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_add(mpfr_ptr(), mpfr_srcptr(), v.mpfr_srcptr(), mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1020,14 +1120,14 @@ inline mpreal& mpreal::operator+=(const mpf_t u) inline mpreal& mpreal::operator+=(const mpz_t u) { - mpfr_add_z(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_z(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const mpq_t u) { - mpfr_add_q(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_q(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1042,7 +1142,7 @@ inline mpreal& mpreal::operator+= (const long double u) inline mpreal& mpreal::operator+= (const double u) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_add_d(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_d(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); #else *this += mpreal(u); #endif @@ -1053,28 +1153,28 @@ inline mpreal& mpreal::operator+= (const double u) inline mpreal& mpreal::operator+=(const unsigned long int u) { - mpfr_add_ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const unsigned int u) { - mpfr_add_ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const long int u) { - mpfr_add_si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const int u) { - mpfr_add_si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1094,9 +1194,9 @@ inline const mpreal mpreal::operator+()const { return mpreal(*this); } inline const mpreal operator+(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_add(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); + mpfr_add(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } inline mpreal& mpreal::operator++() @@ -1127,21 +1227,21 @@ inline const mpreal mpreal::operator-- (int) // - Subtraction inline mpreal& mpreal::operator-=(const mpreal& v) { - mpfr_sub(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_sub(mpfr_ptr(),mpfr_srcptr(),v.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const mpz_t v) { - mpfr_sub_z(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_z(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const mpq_t v) { - mpfr_sub_q(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_q(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1156,7 +1256,7 @@ inline mpreal& mpreal::operator-=(const long double v) inline mpreal& mpreal::operator-=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_sub_d(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_d(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); #else *this -= mpreal(v); #endif @@ -1167,28 +1267,28 @@ inline mpreal& mpreal::operator-=(const double v) inline mpreal& mpreal::operator-=(const unsigned long int v) { - mpfr_sub_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const unsigned int v) { - mpfr_sub_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const long int v) { - mpfr_sub_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const int v) { - mpfr_sub_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1196,15 +1296,15 @@ inline mpreal& mpreal::operator-=(const int v) inline const mpreal mpreal::operator-()const { mpreal u(*this); - mpfr_neg(u.mp,u.mp,mpreal::get_default_rnd()); + mpfr_neg(u.mpfr_ptr(),u.mpfr_srcptr(),mpreal::get_default_rnd()); return u; } inline const mpreal operator-(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_sub(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); + mpfr_sub(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } inline const mpreal operator-(const double b, const mpreal& a) @@ -1252,21 +1352,21 @@ inline const mpreal operator-(const int b, const mpreal& a) // * Multiplication inline mpreal& mpreal::operator*= (const mpreal& v) { - mpfr_mul(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_mul(mpfr_ptr(),mpfr_srcptr(),v.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const mpz_t v) { - mpfr_mul_z(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_z(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const mpq_t v) { - mpfr_mul_q(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_q(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1281,7 +1381,7 @@ inline mpreal& mpreal::operator*=(const long double v) inline mpreal& mpreal::operator*=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_mul_d(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_d(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); #else *this *= mpreal(v); #endif @@ -1291,58 +1391,58 @@ inline mpreal& mpreal::operator*=(const double v) inline mpreal& mpreal::operator*=(const unsigned long int v) { - mpfr_mul_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const unsigned int v) { - mpfr_mul_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const long int v) { - mpfr_mul_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const int v) { - mpfr_mul_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline const mpreal operator*(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_mul(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); + mpfr_mul(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } ////////////////////////////////////////////////////////////////////////// // / Division inline mpreal& mpreal::operator/=(const mpreal& v) { - mpfr_div(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_div(mpfr_ptr(),mpfr_srcptr(),v.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const mpz_t v) { - mpfr_div_z(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_z(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const mpq_t v) { - mpfr_div_q(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_q(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1357,7 +1457,7 @@ inline mpreal& mpreal::operator/=(const long double v) inline mpreal& mpreal::operator/=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_div_d(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_d(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); #else *this /= mpreal(v); #endif @@ -1367,72 +1467,72 @@ inline mpreal& mpreal::operator/=(const double v) inline mpreal& mpreal::operator/=(const unsigned long int v) { - mpfr_div_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const unsigned int v) { - mpfr_div_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const long int v) { - mpfr_div_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const int v) { - mpfr_div_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline const mpreal operator/(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_div(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_srcptr()), mpfr_get_prec(b.mpfr_srcptr()))); + mpfr_div(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } inline const mpreal operator/(const unsigned long int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); mpfr_ui_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const unsigned int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); mpfr_ui_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const long int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); - mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(),mpreal::get_default_rnd()); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); + mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); - mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(),mpreal::get_default_rnd()); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); + mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const double b, const mpreal& a) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); - mpfr_d_div(x.mpfr_ptr(), b, a.mpfr_srcptr(),mpreal::get_default_rnd()); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); + mpfr_d_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; #else mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); @@ -1445,56 +1545,56 @@ inline const mpreal operator/(const double b, const mpreal& a) // Shifts operators - Multiplication/Division by power of 2 inline mpreal& mpreal::operator<<=(const unsigned long int u) { - mpfr_mul_2ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_mul_2ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator<<=(const unsigned int u) { - mpfr_mul_2ui(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_mul_2ui(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator<<=(const long int u) { - mpfr_mul_2si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_mul_2si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator<<=(const int u) { - mpfr_mul_2si(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_mul_2si(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const unsigned long int u) { - mpfr_div_2ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_div_2ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const unsigned int u) { - mpfr_div_2ui(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_div_2ui(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const long int u) { - mpfr_div_2si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_div_2si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const int u) { - mpfr_div_2si(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_div_2si(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1543,7 +1643,7 @@ inline const mpreal operator>>(const mpreal& v, const int k) inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_mul_2ui(x.mp,v.mp,k,rnd_mode); + mpfr_mul_2ui(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } @@ -1551,61 +1651,63 @@ inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_m inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_mul_2si(x.mp,v.mp,k,rnd_mode); + mpfr_mul_2si(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_div_2ui(x.mp,v.mp,k,rnd_mode); + mpfr_div_2ui(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_div_2si(x.mp,v.mp,k,rnd_mode); + mpfr_div_2si(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } ////////////////////////////////////////////////////////////////////////// //Boolean operators -inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p(a.mp,b.mp) !=0); } -inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p(a.mp,b.mp) !=0); } -inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p(a.mp,b.mp) !=0); } -inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p(a.mp,b.mp) !=0); } -inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p(a.mp,b.mp) !=0); } -inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p(a.mp,b.mp) !=0); } +inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } -inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d (a.mpfr_srcptr(),b) == 0 ); } -inline bool isnan (const mpreal& v){ return (mpfr_nan_p(v.mp) != 0); } -inline bool isinf (const mpreal& v){ return (mpfr_inf_p(v.mp) != 0); } -inline bool isfinite (const mpreal& v){ return (mpfr_number_p(v.mp) != 0); } -inline bool iszero (const mpreal& v){ return (mpfr_zero_p(v.mp) != 0); } -inline bool isint (const mpreal& v){ return (mpfr_integer_p(v.mp) != 0); } +inline bool isnan (const mpreal& op){ return (mpfr_nan_p (op.mpfr_srcptr()) != 0 ); } +inline bool isinf (const mpreal& op){ return (mpfr_inf_p (op.mpfr_srcptr()) != 0 ); } +inline bool isfinite (const mpreal& op){ return (mpfr_number_p (op.mpfr_srcptr()) != 0 ); } +inline bool iszero (const mpreal& op){ return (mpfr_zero_p (op.mpfr_srcptr()) != 0 ); } +inline bool isint (const mpreal& op){ return (mpfr_integer_p(op.mpfr_srcptr()) != 0 ); } #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) -inline bool isregular(const mpreal& v){ return (mpfr_regular_p(v.mp));} +inline bool isregular(const mpreal& op){ return (mpfr_regular_p(op.mpfr_srcptr()));} #endif ////////////////////////////////////////////////////////////////////////// // Type Converters -inline long mpreal::toLong (mp_rnd_t mode) const { return mpfr_get_si(mp, mode); } -inline unsigned long mpreal::toULong (mp_rnd_t mode) const { return mpfr_get_ui(mp, mode); } -inline double mpreal::toDouble (mp_rnd_t mode) const { return mpfr_get_d (mp, mode); } -inline long double mpreal::toLDouble(mp_rnd_t mode) const { return mpfr_get_ld(mp, mode); } +inline bool mpreal::toBool (mp_rnd_t /*mode*/) const { return mpfr_zero_p (mpfr_srcptr()) == 0; } +inline long mpreal::toLong (mp_rnd_t mode) const { return mpfr_get_si (mpfr_srcptr(), mode); } +inline unsigned long mpreal::toULong (mp_rnd_t mode) const { return mpfr_get_ui (mpfr_srcptr(), mode); } +inline float mpreal::toFloat (mp_rnd_t mode) const { return mpfr_get_flt(mpfr_srcptr(), mode); } +inline double mpreal::toDouble (mp_rnd_t mode) const { return mpfr_get_d (mpfr_srcptr(), mode); } +inline long double mpreal::toLDouble(mp_rnd_t mode) const { return mpfr_get_ld (mpfr_srcptr(), mode); } #if defined (MPREAL_HAVE_INT64_SUPPORT) -inline int64_t mpreal::toInt64 (mp_rnd_t mode) const{ return mpfr_get_sj(mp, mode); } -inline uint64_t mpreal::toUInt64(mp_rnd_t mode) const{ return mpfr_get_uj(mp, mode); } +inline int64_t mpreal::toInt64 (mp_rnd_t mode) const{ return mpfr_get_sj(mpfr_srcptr(), mode); } +inline uint64_t mpreal::toUInt64(mp_rnd_t mode) const{ return mpfr_get_uj(mpfr_srcptr(), mode); } #endif inline ::mpfr_ptr mpreal::mpfr_ptr() { return mp; } @@ -1629,7 +1731,7 @@ inline std::string mpreal::toString(const std::string& format) const if( !format.empty() ) { - if(!(mpfr_asprintf(&s,format.c_str(),mp) < 0)) + if(!(mpfr_asprintf(&s, format.c_str(), mpfr_srcptr()) < 0)) { out = std::string(s); @@ -1644,20 +1746,19 @@ inline std::string mpreal::toString(const std::string& format) const inline std::string mpreal::toString(int n, int b, mp_rnd_t mode) const { + // TODO: Add extended format specification (f, e, rounding mode) as it done in output operator (void)b; (void)mode; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - // Use MPFR native function for output - char format[128]; - int digits; + std::ostringstream format; - digits = n > 0 ? n : bits2digits(mpfr_get_prec(mp)); + int digits = (n >= 0) ? n : bits2digits(mpfr_get_prec(mpfr_srcptr())); + + format << "%." << digits << "RNg"; - sprintf(format,"%%.%dRNg",digits); // Default format - - return toString(std::string(format)); + return toString(format.str()); #else @@ -1675,8 +1776,8 @@ inline std::string mpreal::toString(int n, int b, mp_rnd_t mode) const if(mpfr_zero_p(mp)) return "0"; if(mpfr_nan_p(mp)) return "NaN"; - s = mpfr_get_str(NULL,&exp,b,0,mp,mode); - ns = mpfr_get_str(NULL,&exp,b,n,mp,mode); + s = mpfr_get_str(NULL, &exp, b, 0, mp, mode); + ns = mpfr_get_str(NULL, &exp, b, (std::max)(0,n), mp, mode); if(s!=NULL && ns!=NULL) { @@ -1761,17 +1862,43 @@ inline std::string mpreal::toString(int n, int b, mp_rnd_t mode) const ////////////////////////////////////////////////////////////////////////// // I/O +inline std::ostream& mpreal::output(std::ostream& os) const +{ + std::ostringstream format; + const std::ios::fmtflags flags = os.flags(); + + format << ((flags & std::ios::showpos) ? "%+" : "%"); + if (os.precision() >= 0) + format << '.' << os.precision() << "R*" + << ((flags & std::ios::floatfield) == std::ios::fixed ? 'f' : + (flags & std::ios::floatfield) == std::ios::scientific ? 'e' : + 'g'); + else + format << "R*e"; + + char *s = NULL; + if(!(mpfr_asprintf(&s, format.str().c_str(), + mpfr::mpreal::get_default_rnd(), + mpfr_srcptr()) + < 0)) + { + os << std::string(s); + mpfr_free_str(s); + } + return os; +} + inline std::ostream& operator<<(std::ostream& os, const mpreal& v) { - return os<(os.precision())); + return v.output(os); } inline std::istream& operator>>(std::istream &is, mpreal& v) { - // ToDo, use cout::hexfloat and other flags to setup base + // TODO: use cout::hexfloat and other flags to setup base std::string tmp; is >> tmp; - mpfr_set_str(v.mp, tmp.c_str(), 10, mpreal::get_default_rnd()); + mpfr_set_str(v.mpfr_ptr(), tmp.c_str(), 10, mpreal::get_default_rnd()); return is; } @@ -1784,53 +1911,53 @@ inline mp_prec_t digits2bits(int d) { const double LOG2_10 = 3.3219280948873624; - return (mp_prec_t) std::ceil( d * LOG2_10 ); + return mp_prec_t(std::ceil( d * LOG2_10 )); } inline int bits2digits(mp_prec_t b) { const double LOG10_2 = 0.30102999566398119; - return (int) std::floor( b * LOG10_2 ); + return int(std::floor( b * LOG10_2 )); } ////////////////////////////////////////////////////////////////////////// // Set/Get number properties -inline int sgn(const mpreal& v) +inline int sgn(const mpreal& op) { - int r = mpfr_signbit(v.mp); - return (r>0?-1:1); + int r = mpfr_signbit(op.mpfr_srcptr()); + return (r > 0? -1 : 1); } inline mpreal& mpreal::setSign(int sign, mp_rnd_t RoundingMode) { - mpfr_setsign(mp,mp,(sign < 0 ? 1 : 0),RoundingMode); + mpfr_setsign(mpfr_ptr(), mpfr_srcptr(), (sign < 0 ? 1 : 0), RoundingMode); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline int mpreal::getPrecision() const { - return mpfr_get_prec(mp); + return int(mpfr_get_prec(mpfr_srcptr())); } inline mpreal& mpreal::setPrecision(int Precision, mp_rnd_t RoundingMode) { - mpfr_prec_round(mp, Precision, RoundingMode); + mpfr_prec_round(mpfr_ptr(), Precision, RoundingMode); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::setInf(int sign) { - mpfr_set_inf(mp,sign); + mpfr_set_inf(mpfr_ptr(), sign); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::setNan() { - mpfr_set_nan(mp); + mpfr_set_nan(mpfr_ptr()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1839,9 +1966,9 @@ inline mpreal& mpreal::setZero(int sign) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - mpfr_set_zero(mp, sign); + mpfr_set_zero(mpfr_ptr(), sign); #else - mpfr_set_si(mp, 0, (mpfr_get_default_rounding_mode)()); + mpfr_set_si(mpfr_ptr(), 0, (mpfr_get_default_rounding_mode)()); setSign(sign); #endif @@ -1851,23 +1978,23 @@ inline mpreal& mpreal::setZero(int sign) inline mp_prec_t mpreal::get_prec() const { - return mpfr_get_prec(mp); + return mpfr_get_prec(mpfr_srcptr()); } inline void mpreal::set_prec(mp_prec_t prec, mp_rnd_t rnd_mode) { - mpfr_prec_round(mp,prec,rnd_mode); + mpfr_prec_round(mpfr_ptr(),prec,rnd_mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mp_exp_t mpreal::get_exp () { - return mpfr_get_exp(mp); + return mpfr_get_exp(mpfr_srcptr()); } inline int mpreal::set_exp (mp_exp_t e) { - int x = mpfr_set_exp(mp, e); + int x = mpfr_set_exp(mpfr_ptr(), e); MPREAL_MSVC_DEBUGVIEW_CODE; return x; } @@ -1885,7 +2012,7 @@ inline const mpreal ldexp(const mpreal& v, mp_exp_t exp) mpreal x(v); // rounding is not important since we just increasing the exponent - mpfr_mul_2si(x.mp,x.mp,exp,mpreal::get_default_rnd()); + mpfr_mul_2si(x.mpfr_ptr(), x.mpfr_srcptr(), exp, mpreal::get_default_rnd()); return x; } @@ -1900,9 +2027,9 @@ inline mpreal machine_epsilon(const mpreal& x) /* the smallest eps such that x + eps != x */ if( x < 0) { - return nextabove(-x)+x; + return nextabove(-x) + x; }else{ - return nextabove(x)-x; + return nextabove( x) - x; } } @@ -1922,37 +2049,37 @@ inline mpreal maxval(mp_prec_t prec) inline bool isEqualUlps(const mpreal& a, const mpreal& b, int maxUlps) { - return abs(a - b) <= machine_epsilon((max)(abs(a), abs(b))) * maxUlps; + return abs(a - b) <= machine_epsilon((max)(abs(a), abs(b))) * maxUlps; } inline bool isEqualFuzzy(const mpreal& a, const mpreal& b, const mpreal& eps) { - return abs(a - b) <= (min)(abs(a), abs(b)) * eps; + return abs(a - b) <= eps; } inline bool isEqualFuzzy(const mpreal& a, const mpreal& b) { - return isEqualFuzzy(a, b, machine_epsilon((min)(abs(a), abs(b)))); + return isEqualFuzzy(a, b, machine_epsilon((max)(1, (min)(abs(a), abs(b))))); } inline const mpreal modf(const mpreal& v, mpreal& n) { - mpreal frac(v); + mpreal f(v); // rounding is not important since we are using the same number - mpfr_frac(frac.mp,frac.mp,mpreal::get_default_rnd()); - mpfr_trunc(n.mp,v.mp); - return frac; + mpfr_frac (f.mpfr_ptr(),f.mpfr_srcptr(),mpreal::get_default_rnd()); + mpfr_trunc(n.mpfr_ptr(),v.mpfr_srcptr()); + return f; } inline int mpreal::check_range (int t, mp_rnd_t rnd_mode) { - return mpfr_check_range(mp,t,rnd_mode); + return mpfr_check_range(mpfr_ptr(),t,rnd_mode); } inline int mpreal::subnormalize (int t,mp_rnd_t rnd_mode) { - int r = mpfr_subnormalize(mp,t,rnd_mode); + int r = mpfr_subnormalize(mpfr_ptr(),t,rnd_mode); MPREAL_MSVC_DEBUGVIEW_CODE; return r; } @@ -2005,8 +2132,11 @@ inline mp_exp_t mpreal::get_emax_max (void) mpfr_##f(y.mpfr_ptr(), x.mpfr_srcptr(), r); \ return y; -inline const mpreal sqr (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sqr ); } -inline const mpreal sqrt (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sqrt); } +inline const mpreal sqr (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) +{ MPREAL_UNARY_MATH_FUNCTION_BODY(sqr ); } + +inline const mpreal sqrt (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) +{ MPREAL_UNARY_MATH_FUNCTION_BODY(sqrt); } inline const mpreal sqrt(const unsigned long int x, mp_rnd_t r) { @@ -2032,14 +2162,14 @@ inline const mpreal sqrt(const int v, mp_rnd_t rnd_mode) else return mpreal().setNan(); // NaN } -inline const mpreal root(const mpreal& x, unsigned long int k, mp_rnd_t r) +inline const mpreal root(const mpreal& x, unsigned long int k, mp_rnd_t r = mpreal::get_default_rnd()) { mpreal y(0, mpfr_get_prec(x.mpfr_srcptr())); mpfr_root(y.mpfr_ptr(), x.mpfr_srcptr(), k, r); return y; } -inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t r) +inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t r = mpreal::get_default_rnd()) { mpreal y(0, mpfr_get_prec(a.mpfr_srcptr())); mpfr_dim(y.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), r); @@ -2048,161 +2178,130 @@ inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t r) inline int cmpabs(const mpreal& a,const mpreal& b) { - return mpfr_cmpabs(a.mp,b.mp); + return mpfr_cmpabs(a.mpfr_ptr(), b.mpfr_srcptr()); } -inline int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) +inline int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - return mpfr_sin_cos(s.mp,c.mp,v.mp,rnd_mode); + return mpfr_sin_cos(s.mpfr_ptr(), c.mpfr_ptr(), v.mpfr_srcptr(), rnd_mode); } inline const mpreal sqrt (const long double v, mp_rnd_t rnd_mode) { return sqrt(mpreal(v),rnd_mode); } inline const mpreal sqrt (const double v, mp_rnd_t rnd_mode) { return sqrt(mpreal(v),rnd_mode); } -inline const mpreal cbrt (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cbrt ); } -inline const mpreal fabs (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } -inline const mpreal abs (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } -inline const mpreal log (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log ); } -inline const mpreal log2 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log2 ); } -inline const mpreal log10 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log10); } -inline const mpreal exp (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp ); } -inline const mpreal exp2 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp2 ); } -inline const mpreal exp10 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp10); } -inline const mpreal cos (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cos ); } -inline const mpreal sin (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sin ); } -inline const mpreal tan (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(tan ); } -inline const mpreal sec (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sec ); } -inline const mpreal csc (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(csc ); } -inline const mpreal cot (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cot ); } -inline const mpreal acos (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(acos); } -inline const mpreal asin (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(asin); } -inline const mpreal atan (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(atan); } +inline const mpreal cbrt (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cbrt ); } +inline const mpreal fabs (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } +inline const mpreal abs (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } +inline const mpreal log (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log ); } +inline const mpreal log2 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log2 ); } +inline const mpreal log10 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log10); } +inline const mpreal exp (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp ); } +inline const mpreal exp2 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp2 ); } +inline const mpreal exp10 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp10); } +inline const mpreal cos (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cos ); } +inline const mpreal sin (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sin ); } +inline const mpreal tan (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(tan ); } +inline const mpreal sec (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sec ); } +inline const mpreal csc (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(csc ); } +inline const mpreal cot (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cot ); } +inline const mpreal acos (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(acos ); } +inline const mpreal asin (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(asin ); } +inline const mpreal atan (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(atan ); } -inline const mpreal acot (const mpreal& v, mp_rnd_t r) { return atan (1/v, r); } -inline const mpreal asec (const mpreal& v, mp_rnd_t r) { return acos (1/v, r); } -inline const mpreal acsc (const mpreal& v, mp_rnd_t r) { return asin (1/v, r); } -inline const mpreal acoth (const mpreal& v, mp_rnd_t r) { return atanh(1/v, r); } -inline const mpreal asech (const mpreal& v, mp_rnd_t r) { return acosh(1/v, r); } -inline const mpreal acsch (const mpreal& v, mp_rnd_t r) { return asinh(1/v, r); } +inline const mpreal acot (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return atan (1/v, r); } +inline const mpreal asec (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return acos (1/v, r); } +inline const mpreal acsc (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return asin (1/v, r); } +inline const mpreal acoth (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return atanh(1/v, r); } +inline const mpreal asech (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return acosh(1/v, r); } +inline const mpreal acsch (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return asinh(1/v, r); } -inline const mpreal cosh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cosh ); } -inline const mpreal sinh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sinh ); } -inline const mpreal tanh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(tanh ); } -inline const mpreal sech (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sech ); } -inline const mpreal csch (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(csch ); } -inline const mpreal coth (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(coth ); } -inline const mpreal acosh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(acosh); } -inline const mpreal asinh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(asinh); } -inline const mpreal atanh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(atanh); } +inline const mpreal cosh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cosh ); } +inline const mpreal sinh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sinh ); } +inline const mpreal tanh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(tanh ); } +inline const mpreal sech (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sech ); } +inline const mpreal csch (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(csch ); } +inline const mpreal coth (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(coth ); } +inline const mpreal acosh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(acosh); } +inline const mpreal asinh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(asinh); } +inline const mpreal atanh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(atanh); } -inline const mpreal log1p (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log1p ); } -inline const mpreal expm1 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(expm1 ); } -inline const mpreal eint (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(eint ); } -inline const mpreal gamma (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(gamma ); } -inline const mpreal lngamma (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(lngamma); } -inline const mpreal zeta (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(zeta ); } -inline const mpreal erf (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(erf ); } -inline const mpreal erfc (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(erfc ); } -inline const mpreal besselj0(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(j0 ); } -inline const mpreal besselj1(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(j1 ); } -inline const mpreal bessely0(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(y0 ); } -inline const mpreal bessely1(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(y1 ); } +inline const mpreal log1p (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log1p ); } +inline const mpreal expm1 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(expm1 ); } +inline const mpreal eint (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(eint ); } +inline const mpreal gamma (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(gamma ); } +inline const mpreal lngamma (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(lngamma); } +inline const mpreal zeta (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(zeta ); } +inline const mpreal erf (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(erf ); } +inline const mpreal erfc (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(erfc ); } +inline const mpreal besselj0(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(j0 ); } +inline const mpreal besselj1(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(j1 ); } +inline const mpreal bessely0(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(y0 ); } +inline const mpreal bessely1(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(y1 ); } -inline const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode) +inline const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_atan2(a.mp, y.mp, x.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_atan2(a.mpfr_ptr(), y.mpfr_srcptr(), x.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_hypot(a.mp, x.mp, y.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_hypot(a.mpfr_ptr(), x.mpfr_srcptr(), y.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_remainder(a.mp, x.mp, y.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_remainder(a.mpfr_ptr(), x.mpfr_srcptr(), y.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_remquo(a.mp,q, x.mp, y.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_remquo(a.mpfr_ptr(),q, x.mpfr_srcptr(), y.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal fac_ui (unsigned long int v, mp_prec_t prec, mp_rnd_t rnd_mode) +inline const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::get_default_prec(), + mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(0, prec); - mpfr_fac_ui(x.mp,v,rnd_mode); + mpfr_fac_ui(x.mpfr_ptr(),v,rnd_mode); return x; } -inline const mpreal lgamma (const mpreal& v, int *signp, mp_rnd_t rnd_mode) +inline const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(v); int tsignp; - if(signp) mpfr_lgamma(x.mp,signp,v.mp,rnd_mode); - else mpfr_lgamma(x.mp,&tsignp,v.mp,rnd_mode); + if(signp) mpfr_lgamma(x.mpfr_ptr(), signp,v.mpfr_srcptr(),rnd_mode); + else mpfr_lgamma(x.mpfr_ptr(),&tsignp,v.mpfr_srcptr(),rnd_mode); return x; } -inline const mpreal besseljn (long n, const mpreal& x, mp_rnd_t r) +inline const mpreal besseljn (long n, const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { - mpreal y(0, mpfr_get_prec(x.mpfr_srcptr())); + mpreal y(0, x.getPrecision()); mpfr_jn(y.mpfr_ptr(), n, x.mpfr_srcptr(), r); return y; } -inline const mpreal besselyn (long n, const mpreal& x, mp_rnd_t r) +inline const mpreal besselyn (long n, const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { - mpreal y(0, mpfr_get_prec(x.mpfr_srcptr())); + mpreal y(0, x.getPrecision()); mpfr_yn(y.mpfr_ptr(), n, x.mpfr_srcptr(), r); return y; } -inline const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) +inline const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t p1, p2, p3; @@ -2217,7 +2316,7 @@ inline const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, m return a; } -inline const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) +inline const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t p1, p2, p3; @@ -2232,7 +2331,7 @@ inline const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, m return a; } -inline const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode) +inline const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t p1, p2; @@ -2247,7 +2346,7 @@ inline const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode) return a; } -inline const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode) +inline const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x; mpfr_ptr* t; @@ -2264,23 +2363,23 @@ inline const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_m // MPFR 2.4.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) -inline int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) +inline int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { return mpfr_sinh_cosh(s.mp,c.mp,v.mp,rnd_mode); } -inline const mpreal li2 (const mpreal& x, mp_rnd_t r) +inline const mpreal li2 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(li2); } -inline const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { /* R = rem(X,Y) if Y != 0, returns X - n * Y where n = trunc(X/Y). */ return fmod(x, y, rnd_mode); } -inline const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { (void)rnd_mode; @@ -2305,7 +2404,7 @@ inline const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) return m; } -inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t yp, xp; @@ -2320,7 +2419,7 @@ inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) return a; } -inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) +inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(v); mpfr_rec_sqrt(x.mp,v.mp,rnd_mode); @@ -2331,41 +2430,41 @@ inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) ////////////////////////////////////////////////////////////////////////// // MPFR 3.0.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) -inline const mpreal digamma (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(digamma); } -inline const mpreal ai (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(ai); } +inline const mpreal digamma (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(digamma); } +inline const mpreal ai (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(ai); } #endif // MPFR 3.0.0 Specifics ////////////////////////////////////////////////////////////////////////// // Constants -inline const mpreal const_log2 (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_log2 (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_log2(x.mpfr_ptr(), r); return x; } -inline const mpreal const_pi (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_pi (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_pi(x.mpfr_ptr(), r); return x; } -inline const mpreal const_euler (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_euler (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_euler(x.mpfr_ptr(), r); return x; } -inline const mpreal const_catalan (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_catalan (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_catalan(x.mpfr_ptr(), r); return x; } -inline const mpreal const_infinity (int sign, mp_prec_t p, mp_rnd_t /*r*/) +inline const mpreal const_infinity (int sign = 1, mp_prec_t p = mpreal::get_default_prec()) { mpreal x(0, p); mpfr_set_inf(x.mpfr_ptr(), sign); @@ -2402,12 +2501,12 @@ inline const mpreal trunc(const mpreal& v) return x; } -inline const mpreal rint (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint ); } -inline const mpreal rint_ceil (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_ceil ); } -inline const mpreal rint_floor (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_floor); } -inline const mpreal rint_round (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_round); } -inline const mpreal rint_trunc (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_trunc); } -inline const mpreal frac (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(frac ); } +inline const mpreal rint (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint ); } +inline const mpreal rint_ceil (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_ceil ); } +inline const mpreal rint_floor (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_floor); } +inline const mpreal rint_round (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_round); } +inline const mpreal rint_trunc (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_trunc); } +inline const mpreal frac (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(frac ); } ////////////////////////////////////////////////////////////////////////// // Miscellaneous Functions @@ -2415,14 +2514,14 @@ inline void swap (mpreal& a, mpreal& b) { mpfr_swap(a.mp,b inline const mpreal (max)(const mpreal& x, const mpreal& y){ return (x>y?x:y); } inline const mpreal (min)(const mpreal& x, const mpreal& y){ return (x= MPFR_VERSION_NUM(3,0,0)) +#if (MPFR_VERSION >= MPFR_VERSION_NUM(3,1,0)) // use gmp_randinit_default() to init state, gmp_randclear() to clear -inline const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode) +inline const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x; mpfr_urandom(x.mp,state,rnd_mode); return x; } + +inline const mpreal grandom (gmp_randstate_t& state, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) +{ + mpreal x; + mpfr_grandom(x.mp, NULL, state, rnd_mode); + return x; +} + #endif #if (MPFR_VERSION <= MPFR_VERSION_NUM(2,4,2)) @@ -2480,7 +2587,7 @@ inline const mpreal random2 (mp_size_t size, mp_exp_t exp) // a = random(seed); <- initialization & first random number generation // a = random(); <- next random numbers generation // seed != 0 -inline const mpreal random(unsigned int seed) +inline const mpreal random(unsigned int seed = 0) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) @@ -2504,6 +2611,25 @@ inline const mpreal random(unsigned int seed) } +#if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) +inline const mpreal grandom(unsigned int seed = 0) +{ + static gmp_randstate_t state; + static bool isFirstTime = true; + + if(isFirstTime) + { + gmp_randinit_default(state); + gmp_randseed_ui(state,0); + isFirstTime = false; + } + + if(seed != 0) gmp_randseed_ui(state,seed); + + return mpfr::grandom(state); +} +#endif + ////////////////////////////////////////////////////////////////////////// // Set/Get global properties inline void mpreal::set_default_prec(mp_prec_t prec) @@ -2523,21 +2649,21 @@ inline bool mpreal::fits_in_bits(double x, int n) return IsInf(x) || (std::modf ( std::ldexp ( std::frexp ( x, &i ), n ), &t ) == 0.0); } -inline const mpreal pow(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow(x.mp,x.mp,b.mp,rnd_mode); return x; } -inline const mpreal pow(const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow_z(x.mp,x.mp,b,rnd_mode); return x; } -inline const mpreal pow(const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow_ui(x.mp,x.mp,b,rnd_mode); @@ -2549,7 +2675,7 @@ inline const mpreal pow(const mpreal& a, const unsigned int b, mp_rnd_t rnd_mode return pow(a,static_cast(b),rnd_mode); } -inline const mpreal pow(const mpreal& a, const long int b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow_si(x.mp,x.mp,b,rnd_mode); @@ -2571,7 +2697,7 @@ inline const mpreal pow(const mpreal& a, const double b, mp_rnd_t rnd_mode) return pow(a,mpreal(b),rnd_mode); } -inline const mpreal pow(const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode) +inline const mpreal pow(const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_ui_pow(x.mp,a,b.mp,rnd_mode); @@ -2586,13 +2712,13 @@ inline const mpreal pow(const unsigned int a, const mpreal& b, mp_rnd_t rnd_mode inline const mpreal pow(const long int a, const mpreal& b, mp_rnd_t rnd_mode) { if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const int a, const mpreal& b, mp_rnd_t rnd_mode) { if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const long double a, const mpreal& b, mp_rnd_t rnd_mode) @@ -2621,13 +2747,13 @@ inline const mpreal pow(const unsigned long int a, const unsigned int b, mp_rnd_ inline const mpreal pow(const unsigned long int a, const long int b, mp_rnd_t rnd_mode) { if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const int b, mp_rnd_t rnd_mode) { if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const long double b, mp_rnd_t rnd_mode) @@ -2824,7 +2950,7 @@ inline const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode) // Non-throwing swap C++ idiom: http://en.wikibooks.org/wiki/More_C%2B%2B_Idioms/Non-throwing_swap namespace std { - // only allowed to extend namespace std with specializations + // we are allowed to extend namespace std with specializations only template <> inline void swap(mpfr::mpreal& x, mpfr::mpreal& y) { @@ -2852,20 +2978,6 @@ namespace std static const bool tinyness_before = true; static const float_denorm_style has_denorm = denorm_absent; - - inline static float_round_style round_style() - { - mp_rnd_t r = mpfr::mpreal::get_default_rnd(); - - switch (r) - { - case MPFR_RNDN: return round_to_nearest; - case MPFR_RNDZ: return round_toward_zero; - case MPFR_RNDU: return round_toward_infinity; - case MPFR_RNDD: return round_toward_neg_infinity; - default: return round_indeterminate; - } - } inline static mpfr::mpreal (min) (mp_prec_t precision = mpfr::mpreal::get_default_prec()) { return mpfr::minval(precision); } inline static mpfr::mpreal (max) (mp_prec_t precision = mpfr::mpreal::get_default_prec()) { return mpfr::maxval(precision); } @@ -2873,7 +2985,7 @@ namespace std // Returns smallest eps such that 1 + eps != 1 (classic machine epsilon) inline static mpfr::mpreal epsilon(mp_prec_t precision = mpfr::mpreal::get_default_prec()) { return mpfr::machine_epsilon(precision); } - + // Returns smallest eps such that x + eps != x (relative machine epsilon) inline static mpfr::mpreal epsilon(const mpfr::mpreal& x) { return mpfr::machine_epsilon(x); } @@ -2881,7 +2993,7 @@ namespace std { mp_rnd_t r = mpfr::mpreal::get_default_rnd(); - if(r == MPFR_RNDN) return mpfr::mpreal(0.5, precision); + if(r == GMP_RNDN) return mpfr::mpreal(0.5, precision); else return mpfr::mpreal(1.0, precision); } @@ -2896,10 +3008,30 @@ namespace std MPREAL_PERMISSIVE_EXPR static const int min_exponent10 = (int) (MPFR_EMIN_DEFAULT * 0.3010299956639811); MPREAL_PERMISSIVE_EXPR static const int max_exponent10 = (int) (MPFR_EMAX_DEFAULT * 0.3010299956639811); - // Should be constant according to standard, but 'digits' depends on precision in MPFR +#ifdef MPREAL_HAVE_DYNAMIC_STD_NUMERIC_LIMITS - inline static int digits() { return mpfr::mpreal::get_default_prec(); } - inline static int digits(const mpfr::mpreal& x) { return x.getPrecision(); } + // Following members should be constant according to standard, but they can be variable in MPFR + // So we define them as functions here. + // + // This is preferable way for std::numeric_limits specialization. + // But it is incompatible with standard std::numeric_limits and might not work with other libraries, e.g. boost. + // See below for compatible implementation. + inline static float_round_style round_style() + { + mp_rnd_t r = mpfr::mpreal::get_default_rnd(); + + switch (r) + { + case GMP_RNDN: return round_to_nearest; + case GMP_RNDZ: return round_toward_zero; + case GMP_RNDU: return round_toward_infinity; + case GMP_RNDD: return round_toward_neg_infinity; + default: return round_indeterminate; + } + } + + inline static int digits() { return int(mpfr::mpreal::get_default_prec()); } + inline static int digits(const mpfr::mpreal& x) { return x.getPrecision(); } inline static int digits10(mp_prec_t precision = mpfr::mpreal::get_default_prec()) { @@ -2915,6 +3047,25 @@ namespace std { return digits10(precision); } +#else + // Digits and round_style are NOT constants when it comes to mpreal. + // If possible, please use functions digits() and round_style() defined above. + // + // These (default) values are preserved for compatibility with existing libraries, e.g. boost. + // Change them accordingly to your application. + // + // For example, if you use 256 bits of precision uniformly in your program, then: + // digits = 256 + // digits10 = 77 + // max_digits10 = 78 + // + // Approximate formula for decimal digits is: digits10 = floor(log10(2) * digits). See bits2digits() for more details. + + static const std::float_round_style round_style = round_to_nearest; + static const int digits = 53; + static const int digits10 = 15; + static const int max_digits10 = 16; +#endif }; } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp index 13f92169e..de79f1538 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp @@ -104,9 +104,7 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const // 1) the roots found are correct // 2) the roots have distinct moduli - typedef typename POLYNOMIAL::Scalar Scalar; typedef typename REAL_ROOTS::Scalar Real; - typedef PolynomialSolver PolynomialSolverType; //Test realRoots std::vector< Real > calc_realRoots; 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/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..7cd6e28b8 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -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 69e841e57..73df17b0d 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -19,15 +19,15 @@ #pragma once #include - +#include +#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include BOOST_MPL_HAS_XXX_TRAIT_DEF(print) @@ -112,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 c67f7dd61..e83a64ba9 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include // operator typeid namespace gtsam { @@ -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 7bcd32b9f..9e56dfb6c 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -180,6 +182,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec } /* ************************************************************************* */ +//3 argument call void print(const Matrix& A, const string &s, ostream& stream) { size_t m = A.rows(), n = A.cols(); @@ -198,6 +201,12 @@ void print(const Matrix& A, const string &s, ostream& stream) { stream << "];" << endl; } +/* ************************************************************************* */ +//1 or 2 argument call +void print(const Matrix& A, const string &s){ + print(A, s, cout); +} + /* ************************************************************************* */ void save(const Matrix& A, const string &s, const string& filename) { fstream stream(filename.c_str(), fstream::out | fstream::app); @@ -697,6 +706,26 @@ 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(); + size_t size = std::min(rows,cols); + typedef Eigen::internal::plain_diag_type::type HCoeffsType; + typedef Eigen::internal::plain_row_type::type RowVectorType; + 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); +} } // namespace gtsam diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index c3cbfa341..caee2071c 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -21,11 +21,14 @@ // \callgraph #pragma once - #include +#include +#include +#include +#include #include #include -#include + /** * Matrix is a typedef in the gtsam namespace @@ -198,9 +201,14 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) { } /** - * print a matrix + * print without optional string, must specify cout yourself */ -GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout); +GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream); + +/** + * print with optional string to cout + */ +GTSAM_EXPORT void print(const Matrix& A, const std::string& s = ""); /** * save a matrix to file, which can be loaded by matlab @@ -367,21 +375,7 @@ GTSAM_EXPORT std::pair qr(const Matrix& A); * @param A is the input matrix, and is the output * @param clear_below_diagonal enables zeroing out below diagonal */ -template -void inplace_QR(MATRIX& A) { - size_t rows = A.rows(); - size_t cols = A.cols(); - size_t size = std::min(rows,cols); - - typedef Eigen::internal::plain_diag_type::type HCoeffsType; - typedef Eigen::internal::plain_row_type::type RowVectorType; - HCoeffsType hCoeffs(size); - RowVectorType temp(cols); - - Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data()); - - zeroBelowDiagonal(A); -} +void inplace_QR(Matrix& A); /** * Imperative algorithm for in-place full elimination with @@ -541,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); @@ -549,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.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 a12f453f4..9537baa31 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -18,9 +18,10 @@ #pragma once -#include -#include #include +#include +#include + namespace gtsam { @@ -120,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; } @@ -165,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 a9ef8fe10..ed6373f5b 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -16,20 +16,18 @@ * @author Frank Dellaert */ +#include +#include +#include +#include #include #include -#include #include #include #include #include -#include -#include -#include #include - -#include - +#include using namespace std; @@ -55,6 +53,7 @@ Vector delta(size_t n, size_t i, double value) { } /* ************************************************************************* */ +//3 argument call void print(const Vector& v, const string& s, ostream& stream) { size_t n = v.size(); @@ -65,6 +64,12 @@ void print(const Vector& v, const string& s, ostream& stream) { stream << "];" << endl; } +/* ************************************************************************* */ +//1 or 2 argument call +void print(const Vector& v, const string& s) { + print(v, s, cout); +} + /* ************************************************************************* */ void save(const Vector& v, const string &s, const string& filename) { fstream stream(filename.c_str(), fstream::out | fstream::app); @@ -217,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 74cd248a1..20477a205 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -18,13 +18,17 @@ // \callgraph + #pragma once -#include -#include -#include +#ifndef MKL_BLAS +#define MKL_BLAS MKL_DOMAIN_BLAS +#endif + #include -#include +#include +#include +#include namespace gtsam { @@ -97,9 +101,14 @@ GTSAM_EXPORT bool zero(const Vector& v); inline size_t dim(const Vector& v) { return v.size(); } /** - * print with optional string + * print without optional string, must specify cout yourself */ -GTSAM_EXPORT void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout); +GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream); + +/** + * print with optional string to cout + */ +GTSAM_EXPORT void print(const Vector& v, const std::string& s = ""); /** * save a vector to file, which can be loaded by matlab @@ -346,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); @@ -362,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/types.cpp b/gtsam/base/types.cpp index 3f86dc0c1..03e7fd120 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -17,10 +17,9 @@ * @addtogroup base */ -#include - #include #include +#include namespace gtsam { diff --git a/gtsam/base/types.h b/gtsam/base/types.h index aca04a14b..a66db13c8 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -21,18 +21,18 @@ #include #include - -#include -#include -#include #include #include #include +#include +#include +#include #ifdef GTSAM_USE_TBB #include #include #include +#include #endif #ifdef GTSAM_USE_EIGEN_MKL_OPENMP 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 e0d57feff..33f2c84eb 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -23,7 +23,6 @@ using namespace std; namespace gtsam { -#ifndef PINHOLEBASE_LINKING_FIX /* ************************************************************************* */ Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { // optimized version of derivatives, see CalibratedCamera.nb @@ -130,8 +129,6 @@ Point3 PinholeBase::backproject_from_camera(const Point2& p, return Point3(p.x() * depth, p.y() * depth, depth); } -#endif - /* ************************************************************************* */ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { return CalibratedCamera(LevelPose(pose2, height)); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 0e6f83fdf..f17cc6441 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -19,10 +19,7 @@ #pragma once #include -#define PINHOLEBASE_LINKING_FIX -#ifdef PINHOLEBASE_LINKING_FIX -#include -#endif + namespace gtsam { class Point2; @@ -46,8 +43,6 @@ private: Pose3 pose_; ///< 3D pose of camera -#ifndef PINHOLEBASE_LINKING_FIX - protected: /// @name Derivatives @@ -165,139 +160,12 @@ public: /// backproject a 2-dimensional point to a 3-dimensional point at given depth static Point3 backproject_from_camera(const Point2& p, const double depth); -#else - -public: - - PinholeBase() { - } - - explicit PinholeBase(const Pose3& pose) : - pose_(pose) { - } - - explicit PinholeBase(const Vector &v) : - pose_(Pose3::Expmap(v)) { - } - - const Pose3& pose() const { - return pose_; - } - - /* ************************************************************************* */ - static Matrix26 Dpose(const Point2& pn, double d) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - double uv = u * v, uu = u * u, vv = v * v; - Matrix26 Dpn_pose; - Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; - return Dpn_pose; - } - - /* ************************************************************************* */ - static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - Matrix23 Dpn_point; - Dpn_point << // - Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), // - /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2); - Dpn_point *= d; - return Dpn_point; - } - - /* ************************************************************************* */ - static Pose3 LevelPose(const Pose2& pose2, double height) { - const double st = sin(pose2.theta()), ct = cos(pose2.theta()); - const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - const Rot3 wRc(x, y, z); - const Point3 t(pose2.x(), pose2.y(), height); - return Pose3(wRc, t); - } - - /* ************************************************************************* */ - static Pose3 LookatPose(const Point3& eye, const Point3& target, - const Point3& upVector) { - Point3 zc = target - eye; - zc = zc / zc.norm(); - Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down - xc = xc / xc.norm(); - Point3 yc = zc.cross(xc); - return Pose3(Rot3(xc, yc, zc), eye); - } - - /* ************************************************************************* */ - bool equals(const PinholeBase &camera, double tol=1e-9) const { - return pose_.equals(camera.pose(), tol); - } - - /* ************************************************************************* */ - void print(const std::string& s) const { - pose_.print(s + ".pose"); - } - - /* ************************************************************************* */ - const Pose3& getPose(OptionalJacobian<6, 6> H) const { - if (H) { - H->setZero(); - H->block(0, 0, 6, 6) = I_6x6; - } - return pose_; - } - - /* ************************************************************************* */ - static Point2 project_to_camera(const Point3& pc, - OptionalJacobian<2, 3> Dpoint = boost::none) { - double d = 1.0 / pc.z(); - const double u = pc.x() * d, v = pc.y() * d; - if (Dpoint) - *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Point2(u, v); - } - - /* ************************************************************************* */ - std::pair projectSafe(const Point3& pw) const { - const Point3 pc = pose().transform_to(pw); - const Point2 pn = project_to_camera(pc); - return std::make_pair(pn, pc.z() > 0); - } - - /* ************************************************************************* */ - Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose, - OptionalJacobian<2, 3> Dpoint) const { - - Matrix3 Rt; // calculated by transform_to if needed - const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); - #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (q.z() <= 0) - throw CheiralityException(); - #endif - const Point2 pn = project_to_camera(q); - - if (Dpose || Dpoint) { - const double d = 1.0 / q.z(); - if (Dpose) - *Dpose = PinholeBase::Dpose(pn, d); - if (Dpoint) - *Dpoint = PinholeBase::Dpoint(pn, d, Rt); - } - return pn; - } - - /* ************************************************************************* */ - static Point3 backproject_from_camera(const Point2& p, - const double depth) { - return Point3(p.x() * depth, p.y() * depth, depth); - } - -#endif - 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_); } @@ -449,7 +317,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 3a34ca1fd..3e40d11a0 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -104,7 +104,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 606f62f87..697bd462d 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,7 +10,8 @@ #include #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 bfc2bbb93..fb57b0a69 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -371,7 +371,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 7588f517e..bfb336f9a 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -37,6 +37,8 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase { public : + typedef Calibration CalibrationType; + /// @name Standard Constructors /// @{ @@ -160,7 +162,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)); @@ -270,7 +272,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"); } /// @} @@ -321,7 +326,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.h b/gtsam/geometry/StereoCamera.h index ea28ecfc7..35cf437e9 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -157,7 +157,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/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index b1e265266..c02a11928 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -90,11 +90,11 @@ TEST( CalibratedCamera, project) /* ************************************************************************* */ TEST( CalibratedCamera, Dproject_to_camera1) { Point3 pp(155,233,131); - Matrix test1; - CalibratedCamera::project_to_camera(pp, test1); - Matrix test2 = numericalDerivative11( + Matrix actual; + CalibratedCamera::project_to_camera(pp, actual); + Matrix expected_numerical = numericalDerivative11( boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp); - CHECK(assert_equal(test1, test2)); + CHECK(assert_equal(expected_numerical, actual)); } /* ************************************************************************* */ 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/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 474689525..c0f69217c 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 @@ -57,7 +51,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)); } @@ -89,6 +92,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 ce83f780b..f4f40ccba 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,7 +18,6 @@ #pragma once - #include #include #include @@ -43,6 +42,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 +62,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); /// /** @@ -92,9 +102,9 @@ std::pair triangulationGraph( * @param initialEstimate * @return graph and initial values */ -template +template std::pair triangulationGraph( - const std::vector >& cameras, + const std::vector& cameras, const std::vector& measurements, Key landmarkKey, const Point3& initialEstimate) { Values values; @@ -103,8 +113,8 @@ std::pair triangulationGraph( static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { - const PinholeCamera& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // + const CAMERA& camera_i = cameras[i]; + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); @@ -150,9 +160,9 @@ Point3 triangulateNonlinear(const std::vector& poses, * @param initialEstimate * @return refined Point3 */ -template +template Point3 triangulateNonlinear( - const std::vector >& cameras, + const std::vector& cameras, const std::vector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values @@ -164,6 +174,25 @@ Point3 triangulateNonlinear( return optimize(graph, values, Symbol('p', 0)); } +/** + * 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 @@ -188,10 +217,10 @@ 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)); - } + 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); @@ -204,7 +233,7 @@ Point3 triangulatePoint3(const std::vector& poses, BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -223,25 +252,25 @@ Point3 triangulatePoint3(const std::vector& poses, * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ -template +template Point3 triangulatePoint3( - const std::vector >& cameras, + 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 - typedef PinholeCamera Camera; + typedef PinholeCamera Camera; std::vector projection_matrices; - BOOST_FOREACH(const Camera& camera, cameras) { - Matrix P_ = (camera.pose().inverse().matrix()); - projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) ); - } + BOOST_FOREACH(const Camera& camera, cameras) + projection_matrices.push_back( + CameraProjectionMatrix(camera.calibration())( + camera.pose())); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization @@ -253,7 +282,7 @@ Point3 triangulatePoint3( BOOST_FOREACH(const Camera& camera, cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif 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/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/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..274d5681c 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -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/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..6a7d91bc9 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.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(Base); } 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.h b/gtsam/linear/HessianFactor.h index 553978e37..dbec5bb59 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -434,7 +434,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..597925eea 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -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; } @@ -515,6 +533,51 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, transposeMultiplyAdd(alpha, Ax, y); } +/* ************************************************************************* */ +/** 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 JacobianFactor::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; + } +} + /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 02ae21566..319d9faba 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -283,6 +283,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 +360,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 2031a4b73..a8b177b43 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using namespace std; @@ -70,6 +71,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(); @@ -79,24 +85,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)); } } @@ -111,13 +118,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"); } /* ************************************************************************* */ @@ -129,6 +138,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; @@ -221,9 +236,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()) { } /* ************************************************************************* */ @@ -262,12 +279,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_); } /* ************************************************************************* */ @@ -293,7 +310,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; @@ -342,7 +359,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; @@ -404,8 +421,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 @@ -542,16 +559,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 */ @@ -560,8 +567,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(); } } @@ -579,7 +585,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); } } @@ -593,7 +599,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); } } @@ -609,7 +615,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); } } @@ -627,7 +633,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); } } @@ -641,7 +647,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; @@ -653,26 +659,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; @@ -680,7 +686,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)); } @@ -688,13 +694,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)); } @@ -702,7 +708,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; @@ -710,7 +716,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); } @@ -718,24 +724,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); @@ -748,24 +754,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); } @@ -774,13 +780,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 098af8b6d..923e7c7e9 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -26,7 +26,6 @@ #include #include #include -#include namespace gtsam { @@ -59,7 +58,7 @@ 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() {} @@ -75,14 +74,13 @@ 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. - */ + /// Unwhiten an error vector. virtual Vector unwhiten(const Vector& v) const = 0; virtual double distance(const Vector& v) const = 0; @@ -116,7 +114,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 +187,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 +219,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 +233,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 +306,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 +316,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 +341,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_); @@ -489,7 +492,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_); } @@ -556,7 +559,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_); @@ -592,18 +595,18 @@ namespace gtsam { 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); } }; @@ -629,20 +632,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; @@ -655,7 +661,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_); } }; @@ -667,16 +673,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); } }; @@ -686,12 +692,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_; @@ -700,7 +706,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_); } @@ -712,11 +718,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_; @@ -725,7 +731,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_); } @@ -741,11 +747,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_; @@ -754,7 +760,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_); } @@ -765,12 +771,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_; @@ -779,7 +785,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_); } @@ -790,12 +796,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_; @@ -804,7 +810,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_); } @@ -848,7 +854,7 @@ 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 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(); } @@ -867,7 +873,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/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/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/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 a7c6ecd39..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,42 +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, + 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; + } 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 b91c76ade..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,41 +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; - private: + /** @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, + 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; + } + +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 3f7109543..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)); - Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); + 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 9c82a7dfa..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,33 +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; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, 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, poseVelocity.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + // 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 4769e5048..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,8 +69,8 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if (H) { - const T value = expression_.value(x, keys_, dims_, *H); + if (H) { + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); return traits::Local(measurement_, value); } else { const T value = expression_.value(x); @@ -89,25 +93,30 @@ public: // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); - JacobianMap jacobianMap(keys_, Ab); + 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 value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! + T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here ! // Evaluate error and set RHS vector b Ab(size()).col(0) = -traits::Local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS - Vector dummy(Dim); - noiseModel_->WhitenSystem(Ab.matrix(), dummy); + Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models + noiseModel_->WhitenSystem(Ab.matrix(), b); 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 473caa35e..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; @@ -42,6 +43,8 @@ LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTrans boost::algorithm::to_upper(s); if (s == "SILENT") return LevenbergMarquardtParams::SILENT; + if (s == "SUMMARY") + return LevenbergMarquardtParams::SUMMARY; if (s == "LAMBDA") return LevenbergMarquardtParams::LAMBDA; if (s == "TRYLAMBDA") @@ -65,6 +68,9 @@ std::string LevenbergMarquardtParams::verbosityLMTranslator( case LevenbergMarquardtParams::SILENT: s = "SILENT"; break; + case LevenbergMarquardtParams::SUMMARY: + s = "SUMMARY"; + break; case LevenbergMarquardtParams::TERMINATION: s = "TERMINATION"; break; @@ -173,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; } @@ -219,9 +225,15 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); - if(state_.totalNumberInnerIterations==0) // write initial error + if(state_.totalNumberInnerIterations==0) { // write initial error writeLogFile(state_.error); + if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { + cout << "Initial error: " << state_.error << ", values: " << state_.values.size() + << std::endl; + } + } + // Keep increasing lambda until we make make progress while (true) { @@ -236,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; @@ -244,10 +256,12 @@ void LevenbergMarquardtOptimizer::iterate() { try { delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - } catch (IndeterminantLinearSystemException) { + } catch (const IndeterminantLinearSystemException& e) { systemSolvedSuccessfully = false; } + double linearizedCostChange = 0, + newlinearizedError = 0; if (systemSolvedSuccessfully) { params_.reuse_diagonal_ = true; @@ -257,9 +271,9 @@ void LevenbergMarquardtOptimizer::iterate() { delta.print("delta"); // cost change in the linearized system (old - new) - double newlinearizedError = linear->error(delta); + newlinearizedError = linear->error(delta); - double linearizedCostChange = state_.error - newlinearizedError; + linearizedCostChange = state_.error - newlinearizedError; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "newlinearizedError = " << newlinearizedError << " linearizedCostChange = " << linearizedCostChange << endl; @@ -304,6 +318,12 @@ void LevenbergMarquardtOptimizer::iterate() { } } + if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { + cout << "[" << state_.iterations << "]: " << "new error = " << newlinearizedError + << ", delta = " << linearizedCostChange << ", lambda = " << state_.lambda + << ", success = " << systemSolvedSuccessfully << std::endl; + } + ++state_.totalNumberInnerIterations; if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity @@ -320,7 +340,8 @@ void LevenbergMarquardtOptimizer::iterate() { // check if lambda is too big if (state_.lambda >= params_.lambdaUpperBound) { - if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION) + if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION || + lmVerbosity == LevenbergMarquardtParams::SUMMARY) cout << "Warning: Levenberg-Marquardt giving up because " "cannot decrease error with maximum lambda" << endl; break; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 20f9dec3c..009b480f2 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -38,7 +38,7 @@ class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { public: /** See LevenbergMarquardtParams::lmVerbosity */ enum VerbosityLM { - SILENT = 0, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA + SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA }; static VerbosityLM verbosityLMTranslator(const std::string &s); 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/Values.h b/gtsam/nonlinear/Values.h index 73d0711be..4eb89b084 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -26,14 +26,9 @@ #include #include -#include #include - -#include -#include #include #include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -43,7 +38,6 @@ #pragma GCC diagnostic pop #endif #include -#include #include #include @@ -404,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 80100af5e..3e86bcb8c 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()); } @@ -180,17 +189,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()); } /* ************************************************************************* */ @@ -234,7 +252,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/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp index 99b8120e3..2fb544edf 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -169,7 +170,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 +185,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 +202,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 +217,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 +254,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 +281,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); @@ -327,7 +331,7 @@ TEST(ExpressionFactor, tree) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - TernaryExpression::Function fff = project6; + Expression::TernaryFunction::type fff = project6; // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); @@ -493,6 +497,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/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..2516b2bcc 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -20,12 +20,31 @@ #pragma once -#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 { @@ -120,7 +139,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_); @@ -222,7 +241,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("NoiseModelFactor3", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); 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/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 38e1407f0..1531258cb 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -182,50 +182,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/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index a2bdfc059..cdbe71718 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -771,7 +771,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(body_P_sensor_); diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 7fb43152a..a28482583 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -243,7 +243,7 @@ public: // We triangulate the 3D position of the landmark try { // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - point_ = triangulatePoint3(cameras, this->measured_, + point_ = triangulatePoint3(cameras, this->measured_, rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; @@ -687,7 +687,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/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 70476ba3e..127bf284f 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -203,7 +203,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/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 b7f6a20dc..d0371d1f8 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -184,7 +184,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/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 474009cfb..07c49008d 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -296,8 +296,12 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -306,7 +310,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); @@ -316,8 +320,6 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { // VectorValues delta = GFG->optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); if (isDebugTest) tictoc_print_(); @@ -416,8 +418,6 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { result = optimizer.optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(bodyPose3, result.at(x3))); // Check derivatives @@ -447,15 +447,16 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Factors ){ +TEST( SmartProjectionPoseFactor, Factors ) { // Default cameras for simple derivatives Rot3 R; static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); - SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K); + SimpleCamera cam1(Pose3(R, Point3(0, 0, 0)), *K), cam2( + Pose3(R, Point3(1, 0, 0)), *K); // one landmarks 1m in front of camera - Point3 landmark1(0,0,10); + Point3 landmark1(0, 0, 10); vector measurements_cam1; @@ -476,12 +477,12 @@ TEST( SmartProjectionPoseFactor, Factors ){ cameras.push_back(cam2); // Make sure triangulation works - LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras)); + LONGS_EQUAL(2, smartFactor1->triangulateSafe(cameras)); CHECK(!smartFactor1->isDegenerate()); CHECK(!smartFactor1->isPointBehindCamera()); boost::optional p = smartFactor1->point(); CHECK(p); - EXPECT(assert_equal(landmark1,*p)); + EXPECT(assert_equal(landmark1, *p)); // After eliminating the point, A1 and A2 contain 2-rank information on cameras: Matrix16 A1, A2; @@ -489,12 +490,14 @@ TEST( SmartProjectionPoseFactor, Factors ){ A2 << 1000, 0, 100, 0, -100, 0; { // createHessianFactor - Matrix66 G11 = 0.5*A1.transpose()*A1; - Matrix66 G12 = 0.5*A1.transpose()*A2; - Matrix66 G22 = 0.5*A2.transpose()*A2; + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; - Vector6 g1; g1.setZero(); - Vector6 g2; g2.setZero(); + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); double f = 0; @@ -507,13 +510,32 @@ TEST( SmartProjectionPoseFactor, Factors ){ } { - Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; - Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; - Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; + Matrix26 F1; + F1.setZero(); + F1(0, 1) = -100; + F1(0, 3) = -10; + F1(1, 0) = 100; + F1(1, 4) = -10; + Matrix26 F2; + F2.setZero(); + F2(0, 1) = -101; + F2(0, 3) = -10; + F2(0, 5) = -1; + F2(1, 0) = 100; + F2(1, 2) = 10; + F2(1, 4) = -10; + Matrix43 E; + E.setZero(); + E(0, 0) = 100; + E(1, 1) = 100; + E(2, 0) = 100; + E(2, 2) = 10; + E(3, 1) = 100; const vector > Fblocks = list_of > // - (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); + (make_pair(x1, 10 * F1))(make_pair(x2, 10 * F2)); Matrix3 P = (E.transpose() * E).inverse(); - Vector4 b; b.setZero(); + Vector4 b; + b.setZero(); // createRegularImplicitSchurFactor RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); @@ -524,7 +546,7 @@ TEST( SmartProjectionPoseFactor, Factors ){ CHECK(assert_equal(expected, *actual)); // createJacobianQFactor - JacobianFactorQ < 6, 2 > expectedQ(Fblocks, E, P, b); + JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b); boost::shared_ptr > actualQ = smartFactor1->createJacobianQFactor(cameras, 0.0); @@ -534,9 +556,10 @@ TEST( SmartProjectionPoseFactor, Factors ){ { // createJacobianSVDFactor - Vector1 b; b.setZero(); + Vector1 b; + b.setZero(); double s = sin(M_PI_4); - JacobianFactor expected(x1, s*A1, x2, s*A2, b); + JacobianFactor expected(x1, s * A1, x2, s * A2, b); boost::shared_ptr actual = smartFactor1->createJacobianSVDFactor(cameras, 0.0); @@ -604,8 +627,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), Point3(0.1, -0.1, 1.9)), + values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -614,15 +642,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); if (isDebugTest) tictoc_print_(); @@ -1050,8 +1076,14 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); boost::shared_ptr hessianFactor1 = smartFactor1->linearize( values); @@ -1143,8 +1175,14 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac values.insert(x2, pose2 * noise_pose); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, + -0.572308662, -0.324093872, 0.639358349, -0.521617766, + -0.564921063), + Point3(0.145118171, -0.252907438, 0.819956033)), + values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -1153,19 +1191,24 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; - // EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT( + assert_equal( + Pose3( + Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, + -0.572308662, -0.324093872, 0.639358349, -0.521617766, + -0.564921063), + Point3(0.145118171, -0.252907438, 0.819956033)), + result.at(x3))); if (isDebugTest) tictoc_print_(); } @@ -1240,8 +1283,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -1250,19 +1299,24 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; - // EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + result.at(x3))); if (isDebugTest) tictoc_print_(); } @@ -1543,9 +1597,12 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); - + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -1553,15 +1610,12 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); - // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(pose3, result.at(x3), 1e-6)); if (isDebugTest) tictoc_print_(); @@ -1653,8 +1707,14 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -1663,15 +1723,21 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; 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_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 8ea6c2bcd..f59721a1e 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -74,6 +74,7 @@ Point3 Similarity3::transform_from(const Point3& p, // const Matrix3 R = R_.matrix(); Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z()); *H1 << DR, R, R * p.vector(); + print("From Derivative"); } if (H2) *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() @@ -88,6 +89,8 @@ Point3 Similarity3::operator*(const Point3& p) const { } Matrix7 Similarity3::AdjointMap() const { +// ToDo: This adjoint might not be correct, it is based on delta = [u, w, lambda] +// However, we use the convention delta = [w, u, lambda] const Matrix3 R = R_.matrix(); const Vector3 t = t_.vector(); Matrix3 A = s_ * skewSymmetric(t) * R; @@ -217,8 +220,14 @@ Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { return Similarity3(Rot3::Expmap(w), Point3(V*u), 1.0/exp(-lambda)); } -Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, - ChartJacobian H) { + +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. // Rot3 r; //Create a zero rotation to do our retraction. diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 81f7e4d1c..a03201d57 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -73,6 +73,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 /// @{ @@ -171,6 +173,6 @@ private: }; 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 11513d19f..1358e1349 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -267,7 +267,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, rankTolerance_, enableEPI_); // // // End temporary hack @@ -736,7 +736,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 3e0c6476f..3db1c883e 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -203,7 +203,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/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4cc8e66ff..eac63006d 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -1026,7 +1026,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { // Hessian is invariant to rotations and translations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), - hessianFactorRotTran->information(), 1e-7)); + hessianFactorRotTran->information(), 1e-6)); } /* *************************************************************************/ diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index c7b7d6441..9abd4e31d 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -25,7 +25,7 @@ set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_ma if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - if("${build_type_upper}" STREQUAL "RELEASE") + if(${build_type_upper} STREQUAL "RELEASE") set(build_type_tag "") # Don't create release mode tag on installed directory else() set(build_type_tag "${build_type}") 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/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/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;