From 0372a959ee8756a17b232c3d675d4cfd26dbbd75 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 24 Feb 2016 11:01:19 -0800 Subject: [PATCH] Many small improvements, bug-fixes, and tests --- CMakeLists.txt | 36 ++- GTSAM-Concepts.md | 8 +- cmake/GtsamBuildTypes.cmake | 135 +++++---- cmake/GtsamTesting.cmake | 6 +- gtsam.h | 3 +- gtsam/3rdparty/CMakeLists.txt | 4 +- gtsam/3rdparty/ceres/fixed_array.h | 2 +- gtsam/3rdparty/ceres/jet.h | 2 +- gtsam/3rdparty/metis/CMakeLists.txt | 6 +- gtsam/CMakeLists.txt | 4 +- gtsam/base/Lie.h | 9 + gtsam/base/Matrix.h | 52 ++-- gtsam/base/Testable.h | 12 +- gtsam/base/Vector.cpp | 2 +- gtsam/base/Vector.h | 30 +- gtsam/base/VectorSpace.h | 37 ++- gtsam/base/VerticalBlockMatrix.h | 18 +- gtsam/base/numericalDerivative.h | 14 +- gtsam/base/tests/testFastContainers.cpp | 6 +- gtsam/base/types.h | 5 +- gtsam/config.h.in | 9 +- .../tests/testAlgebraicDecisionTree.cpp | 10 +- gtsam/geometry/Cal3_S2.h | 5 + gtsam/geometry/EssentialMatrix.cpp | 11 + gtsam/geometry/EssentialMatrix.h | 5 + gtsam/geometry/OrientedPlane3.cpp | 20 +- gtsam/geometry/OrientedPlane3.h | 9 + gtsam/geometry/PinholePose.h | 5 + gtsam/geometry/Point2.cpp | 6 + gtsam/geometry/Point2.h | 11 +- gtsam/geometry/Point3.cpp | 2 +- gtsam/geometry/Point3.h | 14 +- gtsam/geometry/Pose2.cpp | 4 +- gtsam/geometry/Pose3.cpp | 84 ++++-- gtsam/geometry/Pose3.h | 35 ++- gtsam/geometry/Rot3.cpp | 57 +++- gtsam/geometry/Rot3.h | 10 +- gtsam/geometry/Rot3M.cpp | 6 +- gtsam/geometry/SO3.cpp | 13 +- gtsam/geometry/StereoPoint2.h | 7 +- gtsam/geometry/Unit3.cpp | 52 ++-- gtsam/geometry/Unit3.h | 7 - gtsam/geometry/tests/testEssentialMatrix.cpp | 49 ++- gtsam/geometry/tests/testOrientedPlane3.cpp | 24 ++ gtsam/geometry/tests/testPoint3.cpp | 76 ++++- gtsam/geometry/tests/testPose3.cpp | 123 ++++++-- gtsam/geometry/tests/testRot3.cpp | 5 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/geometry/triangulation.cpp | 8 +- .../inference/EliminateableFactorGraph-inst.h | 46 ++- gtsam/inference/FactorGraph-inst.h | 25 +- gtsam/inference/FactorGraph.h | 13 +- gtsam/inference/Key.h | 19 +- gtsam/inference/LabeledSymbol.cpp | 2 +- gtsam/inference/LabeledSymbol.h | 6 +- gtsam/inference/Ordering.cpp | 10 +- gtsam/inference/Symbol.cpp | 8 +- gtsam/inference/Symbol.h | 67 ++-- gtsam/inference/VariableIndex.h | 8 +- gtsam/inference/inference-inst.h | 2 +- gtsam/inference/tests/testOrdering.cpp | 2 +- gtsam/linear/GaussianBayesNet.cpp | 15 +- gtsam/linear/GaussianConditional.cpp | 20 +- gtsam/linear/GaussianFactorGraph.cpp | 28 +- gtsam/linear/GaussianFactorGraph.h | 6 +- gtsam/linear/HessianFactor.cpp | 4 +- gtsam/linear/IterativeSolver.h | 2 +- gtsam/linear/JacobianFactor.cpp | 101 ++++--- gtsam/linear/JacobianFactor.h | 6 +- gtsam/linear/NoiseModel.cpp | 257 +++++++++------- gtsam/linear/NoiseModel.h | 130 +++++--- gtsam/linear/SubgraphPreconditioner.cpp | 11 +- gtsam/linear/VectorValues.h | 4 +- gtsam/linear/tests/testHessianFactor.cpp | 14 +- gtsam/linear/tests/testJacobianFactor.cpp | 120 +++++--- gtsam/linear/tests/testNoiseModel.cpp | 155 +++++++++- gtsam/navigation/GPSFactor.cpp | 28 +- gtsam/navigation/GPSFactor.h | 88 +++++- gtsam/navigation/ImuBias.h | 7 +- gtsam/navigation/ImuFactor.cpp | 78 ++++- gtsam/navigation/ImuFactor.h | 86 +++++- gtsam/navigation/NavState.cpp | 2 + gtsam/navigation/NavState.h | 2 + gtsam/navigation/PreintegratedRotation.cpp | 3 +- gtsam/navigation/PreintegrationBase.cpp | 43 ++- gtsam/navigation/PreintegrationBase.h | 18 +- gtsam/navigation/tests/imuFactorTesting.h | 2 +- .../tests/testCombinedImuFactor.cpp | 15 +- gtsam/navigation/tests/testGPSFactor.cpp | 50 ++- gtsam/navigation/tests/testImuFactor.cpp | 54 +++- gtsam/navigation/tests/testNavState.cpp | 2 + .../tests/testPreintegrationBase.cpp | 19 +- gtsam/navigation/tests/testScenarios.cpp | 2 +- gtsam/nonlinear/Expression-inl.h | 40 ++- gtsam/nonlinear/Expression.h | 97 +++++- gtsam/nonlinear/ExpressionFactor.h | 9 +- gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 55 ++-- gtsam/nonlinear/ExtendedKalmanFilter.h | 141 +++++---- gtsam/nonlinear/GaussNewtonOptimizer.cpp | 7 +- gtsam/nonlinear/ISAM2.cpp | 33 +- gtsam/nonlinear/ISAM2.h | 2 +- gtsam/nonlinear/LinearContainerFactor.h | 8 +- gtsam/nonlinear/Marginals.cpp | 45 +-- gtsam/nonlinear/Marginals.h | 23 +- gtsam/nonlinear/NonlinearFactor.h | 7 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 36 ++- gtsam/nonlinear/NonlinearFactorGraph.h | 20 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 6 +- gtsam/nonlinear/NonlinearOptimizer.h | 7 +- gtsam/nonlinear/Values-inl.h | 8 +- gtsam/nonlinear/expressionTesting.h | 6 +- gtsam/nonlinear/factorTesting.h | 19 +- gtsam/nonlinear/internal/CallRecord.h | 38 ++- gtsam/nonlinear/internal/ExecutionTrace.h | 19 +- gtsam/nonlinear/internal/ExpressionNode.h | 213 ++++++++++++- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 3 - gtsam/nonlinear/tests/testExpression.cpp | 285 +++++++++++++++--- gtsam/slam/InitializePose3.cpp | 4 +- gtsam/slam/JacobianFactorSVD.h | 17 +- gtsam/slam/OrientedPlane3Factor.h | 1 + gtsam/slam/RotateFactor.h | 34 ++- gtsam/slam/SmartProjectionFactor.h | 6 +- gtsam/slam/dataset.cpp | 4 +- gtsam/slam/lago.cpp | 6 +- gtsam/slam/tests/testDataset.cpp | 6 +- .../slam/tests/testEssentialMatrixFactor.cpp | 65 +++- gtsam/slam/tests/testRotateFactor.cpp | 54 +++- .../symbolic/tests/testSymbolicBayesTree.cpp | 16 +- .../tests/testSymbolicFactorGraph.cpp | 20 +- gtsam/symbolic/tests/testSymbolicISAM.cpp | 3 +- .../nonlinear/BatchFixedLagSmoother.cpp | 6 + gtsam_unstable/slam/tests/testTOAFactor.cpp | 6 +- python/gtsam/__init__.py | 2 +- python/gtsam_examples/ImuFactorExample.py | 82 +++-- python/gtsam_examples/Pose2SLAMExample.py | 68 +++++ .../gtsam_examples/PreintegrationExample.py | 4 +- python/gtsam_utils/plot.py | 27 +- python/handwritten/CMakeLists.txt | 16 +- python/handwritten/exportgtsam.cpp | 8 +- python/handwritten/geometry/Point3.cpp | 3 +- python/handwritten/geometry/Pose3.cpp | 74 ++--- python/handwritten/geometry/Rot3.cpp | 2 +- .../handwritten/geometry/export_geometry.cpp | 35 +++ python/handwritten/linear/NoiseModel.cpp | 6 +- python/handwritten/navigation/ImuFactor.cpp | 49 ++- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 45 ++- python/handwritten/nonlinear/Values.cpp | 4 + python/handwritten/slam/PriorFactor.cpp | 2 + python/handwritten/slam/export_slam.cpp | 36 +++ tests/testExpressionFactor.cpp | 2 +- tests/testGaussianBayesTreeB.cpp | 3 +- tests/testNonlinearEquality.cpp | 72 ++--- 152 files changed, 3223 insertions(+), 1332 deletions(-) create mode 100644 python/gtsam_examples/Pose2SLAMExample.py create mode 100644 python/handwritten/geometry/export_geometry.cpp create mode 100644 python/handwritten/slam/export_slam.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 95cfa9d41..8c4229ed5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,8 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) -option(GTSAM_USE_VECTOR3_POINTS "Symply typdef Point3 to eigen::Vector3d" OFF) +option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF) +option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -190,10 +191,10 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL include_directories(${MKL_INCLUDE_DIR}) list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) - + # --no-as-needed is required with gcc according to the MKL link advisor if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") endif() else() set(GTSAM_USE_EIGEN_MKL 0) @@ -224,17 +225,17 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us if(GTSAM_USE_SYSTEM_EIGEN) 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! # Note: Eigen >= v3.2.5 includes our patches if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5)) message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL") endif() else() - # Use bundled Eigen include path. + # Use bundled Eigen include path. # Clear any variables set by FindEigen3 if(EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) @@ -242,11 +243,11 @@ else() # 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() ############################################################################### @@ -289,18 +290,24 @@ endif() # Include boost - use 'BEFORE' so that a specific boost specified to CMake # takes precedence over a system-installed one. -# Use 'SYSTEM' to ignore compiler warnings in Boost headers include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR}) +if(GTSAM_SUPPORT_NESTED_DISSECTION) + set(METIS_INCLUDE_DIRECTORIES + gtsam/3rdparty/metis/include + gtsam/3rdparty/metis/libmetis + gtsam/3rdparty/metis/GKlib) +else() + set(METIS_INCLUDE_DIRECTORIES) +endif() + # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. include_directories(BEFORE gtsam/3rdparty/UFconfig gtsam/3rdparty/CCOLAMD/Include - gtsam/3rdparty/metis/include - gtsam/3rdparty/metis/libmetis - gtsam/3rdparty/metis/GKlib + ${METIS_INCLUDE_DIRECTORIES} ${PROJECT_SOURCE_DIR} ${PROJECT_BINARY_DIR} # So we can include generated config header files CppUnitLite) @@ -362,9 +369,9 @@ if (GTSAM_BUILD_PYTHON) # NOTE: The automatic generation of python wrapper from the gtsampy.h interface is # not working yet, so we're using a handwritten wrapper files on python/handwritten. - # Once the python wrapping from the interface file is working, you can _swap_ the + # Once the python wrapping from the interface file is working, you can _swap_ the # comments on the next lines - + # wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "") add_subdirectory(python) @@ -484,6 +491,7 @@ print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full Exp print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") +print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 5c7434a8d..366a58a09 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -18,14 +18,14 @@ To optimize over continuous types, we assume they are manifolds. This is central [Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space. -In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented. -In detail, we ask the following are defined in the traits object (although, not all are needed for optimization): +In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented. +In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization): * values: * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. -* types: +* types: * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. - * `ChartJacobian`, a typedef for `OptionalJacobian`. + * `ChartJacobian`, a typedef for `OptionalJacobian`. * `ManifoldType`, a pointer back to the type. * `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following: * `gtsam::traits::manifold_tag` -- Everything in this list is expected diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 43ae36929..838d5eb09 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -3,56 +3,76 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}") # Default to Release mode -if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION) - set(CMAKE_BUILD_TYPE "Release" CACHE STRING - "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." - FORCE) +if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION) + set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING + "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.") + set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE}) endif() # Add option for using build type postfixes to allow installing multiple build modes option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) -# Add debugging flags but only on the first pass -if(NOT FIRST_PASS_DONE) - if(MSVC) - set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) - set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) - set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - set(CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING CMAKE_MODULE_LINKER_FLAGS_TIMING) - set(CMAKE_C_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_CXX_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING) - else() - set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -std=c11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -std=c++11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_C_FLAGS_RELEASE "-std=c11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) - set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) - set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING) - set(CMAKE_C_FLAGS_PROFILING "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_CXX_FLAGS_PROFILING "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING) - endif() +# Set custom compilation flags. +# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below +# so that we don't "pollute" the global variable namespace in the cmake cache. +if(MSVC) + set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds.") + set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") + set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") +else() + set(GTSAM_CMAKE_C_FLAGS_DEBUG "-std=c11 -Wall -g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-std=c++11 -Wall -g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_C_FLAGS_RELEASE "-std=c11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.") + set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") + set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") endif() +set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") +set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") +set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") + +set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") +set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") +set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") + +mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING + GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING + GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING + GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING) + +# Apply the gtsam specific build flags as normal variables. This makes it so that they only +# apply to the gtsam part of the build if gtsam is built as a subproject +set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG}) +set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}) +set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}) +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}) +set(CMAKE_C_FLAGS_RELEASE ${GTSAM_CMAKE_C_FLAGS_RELEASE}) +set(CMAKE_CXX_FLAGS_RELEASE ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}) +set(CMAKE_C_FLAGS_PROFILING ${GTSAM_CMAKE_C_FLAGS_PROFILING}) +set(CMAKE_CXX_FLAGS_PROFILING ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}) +set(CMAKE_C_FLAGS_TIMING ${GTSAM_CMAKE_C_FLAGS_TIMING}) +set(CMAKE_CXX_FLAGS_TIMING ${GTSAM_CMAKE_CXX_FLAGS_TIMING}) + +set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING}) +set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING}) +set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING}) + +set(CMAKE_SHARED_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING}) +set(CMAKE_MODULE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING}) +set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING}) + # Clang uses a template depth that is less than standard and is too small if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # Apple Clang before 5.0 does not support -ftemplate-depth. @@ -63,10 +83,10 @@ endif() # Set up build type library postfixes if(GTSAM_BUILD_TYPE_POSTFIXES) - foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel) - string(TOUPPER "${build_type}" build_type_toupper) - set(CMAKE_${build_type_toupper}_POSTFIX ${build_type}) - endforeach() + foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel) + string(TOUPPER "${build_type}" build_type_toupper) + set(CMAKE_${build_type_toupper}_POSTFIX ${build_type}) + endforeach() endif() # Make common binary output directory when on Windows @@ -78,17 +98,16 @@ endif() # Set up build type list for cmake-gui if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "") - if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8) - set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel) - endif() + if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel) + endif() endif() # Set up build types for MSVC and XCode -if(NOT FIRST_PASS_DONE) - set(CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel - CACHE STRING "Build types available to MSVC and XCode" FORCE) - mark_as_advanced(FORCE CMAKE_CONFIGURATION_TYPES) -endif() +set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel + CACHE STRING "Build types available to MSVC and XCode") +mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES) +set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES}) # Check build types string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower) @@ -100,13 +119,9 @@ if( NOT cmake_build_type_tolower STREQUAL "" AND NOT cmake_build_type_tolower STREQUAL "profiling" 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).") + message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo, MinSizeRel (case-insensitive).") endif() -# Mark that first pass is done -set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times") -mark_as_advanced(FIRST_PASS_DONE) - # Enable Visual Studio solution folders set_property(GLOBAL PROPERTY USE_FOLDERS On) diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 3e5cadd32..e13888c00 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -164,9 +164,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_test(NAME ${script_name} COMMAND ${script_name}) add_dependencies(check.${groupName} ${script_name}) add_dependencies(check ${script_name}) - add_dependencies(all.tests ${script_name}) + add_dependencies(all.tests ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) - add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) + add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name}) endif() # Add TOPSRCDIR @@ -254,7 +254,7 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b # Add target dependencies add_dependencies(${groupName} ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) - add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) + add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name}) endif() # Add TOPSRCDIR diff --git a/gtsam.h b/gtsam.h index c42cf7031..3d225529e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1742,7 +1742,8 @@ virtual class NonlinearFactor { virtual class NoiseModelFactor: gtsam::NonlinearFactor { void equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* get_noiseModel() const; + gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below + gtsam::noiseModel::Base* noiseModel() const; Vector unwhitenedError(const gtsam::Values& x) const; Vector whitenedError(const gtsam::Values& x) const; }; diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 4adbfb250..8534a8d7e 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -43,7 +43,9 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) -add_subdirectory(metis) +if(GTSAM_SUPPORT_NESTED_DISSECTION) + add_subdirectory(metis) +endif() add_subdirectory(ceres) diff --git a/gtsam/3rdparty/ceres/fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h index 455fce383..d85021dbd 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 4a7683f50..d45dab9e5 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/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index 6ba17787f..dd21338a4 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -20,7 +20,7 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") endif() set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib") -set(SHARED FALSE CACHE BOOL "build a shared library") +set(METIS_SHARED TRUE CACHE BOOL "build a shared library") if(MSVC) set(METIS_INSTALL FALSE) @@ -29,11 +29,11 @@ else() endif() # Configure libmetis library. -if(SHARED) +if(METIS_SHARED) set(METIS_LIBRARY_TYPE SHARED) else() set(METIS_LIBRARY_TYPE STATIC) -endif(SHARED) +endif(METIS_SHARED) include(${GKLIB_PATH}/GKlibSystem.cmake) # Add include directories. diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index cab5e8639..d3229427e 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -88,7 +88,9 @@ configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h") install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam) -list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) +if(GTSAM_SUPPORT_NESTED_DISSECTION) + list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) +endif() # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 23ac0f4df..bb49a84d6 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -327,6 +327,15 @@ T expm(const Vector& x, int K=7) { return T(expm(xhat,K)); } +/** + * Linear interpolation between X and Y by coefficient t in [0, 1]. + */ +template +T interpolate(const T& X, const T& Y, double t) { + assert(t >= 0 && t <= 1); + return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); +} + } // namespace gtsam /** diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 37a0d28b8..b0b292c56 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -46,29 +46,29 @@ typedef Eigen::Matrix M // Create handy typedefs and constants for square-size matrices // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 -#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \ -typedef Eigen::Matrix Matrix##SUFFIX; \ -typedef Eigen::Matrix Matrix1##SUFFIX; \ -typedef Eigen::Matrix Matrix2##SUFFIX; \ -typedef Eigen::Matrix Matrix3##SUFFIX; \ -typedef Eigen::Matrix Matrix4##SUFFIX; \ -typedef Eigen::Matrix Matrix5##SUFFIX; \ -typedef Eigen::Matrix Matrix6##SUFFIX; \ -typedef Eigen::Matrix Matrix7##SUFFIX; \ -typedef Eigen::Matrix Matrix8##SUFFIX; \ -typedef Eigen::Matrix Matrix9##SUFFIX; \ -static const Eigen::MatrixBase::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \ -static const Eigen::MatrixBase::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero(); +#define GTSAM_MAKE_MATRIX_DEFS(N) \ +typedef Eigen::Matrix Matrix##N; \ +typedef Eigen::Matrix Matrix1##N; \ +typedef Eigen::Matrix Matrix2##N; \ +typedef Eigen::Matrix Matrix3##N; \ +typedef Eigen::Matrix Matrix4##N; \ +typedef Eigen::Matrix Matrix5##N; \ +typedef Eigen::Matrix Matrix6##N; \ +typedef Eigen::Matrix Matrix7##N; \ +typedef Eigen::Matrix Matrix8##N; \ +typedef Eigen::Matrix Matrix9##N; \ +static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \ +static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero(); -GTSAM_MAKE_TYPEDEFS(1,1); -GTSAM_MAKE_TYPEDEFS(2,2); -GTSAM_MAKE_TYPEDEFS(3,3); -GTSAM_MAKE_TYPEDEFS(4,4); -GTSAM_MAKE_TYPEDEFS(5,5); -GTSAM_MAKE_TYPEDEFS(6,6); -GTSAM_MAKE_TYPEDEFS(7,7); -GTSAM_MAKE_TYPEDEFS(8,8); -GTSAM_MAKE_TYPEDEFS(9,9); +GTSAM_MAKE_MATRIX_DEFS(1); +GTSAM_MAKE_MATRIX_DEFS(2); +GTSAM_MAKE_MATRIX_DEFS(3); +GTSAM_MAKE_MATRIX_DEFS(4); +GTSAM_MAKE_MATRIX_DEFS(5); +GTSAM_MAKE_MATRIX_DEFS(6); +GTSAM_MAKE_MATRIX_DEFS(7); +GTSAM_MAKE_MATRIX_DEFS(8); +GTSAM_MAKE_MATRIX_DEFS(9); // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; @@ -135,7 +135,7 @@ inline bool operator==(const Matrix& A, const Matrix& B) { } /** - * inequality + * inequality */ inline bool operator!=(const Matrix& A, const Matrix& B) { return !(A==B); @@ -371,7 +371,7 @@ GTSAM_EXPORT Matrix inverse(const Matrix& A); * m*n matrix -> m*m Q, m*n R * @param A a matrix * @return rotation matrix Q, upper triangular R - */ + */ GTSAM_EXPORT std::pair qr(const Matrix& A); /** @@ -434,7 +434,7 @@ GTSAM_EXPORT Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool u * @param b an RHS vector * @param unit, set true if unit triangular * @return the solution x of L*x=b - */ + */ GTSAM_EXPORT Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit=false); /** diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 92ccb9156..73fb320e1 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -13,20 +13,20 @@ * @file Testable.h * @brief Concept check for values that can be used in unit tests * @author Frank Dellaert - * + * * The necessary functions to implement for Testable are defined * below with additional details as to the interface. * The concept checking function will check whether or not * the function exists in derived class and throw compile-time errors. - * + * * print with optional string naming the object * void print(const std::string& name) const = 0; - * + * * equality up to tolerance * tricky to implement, see NoiseModelFactor1 for an example * equals is not supposed to print out *anything*, just return true|false * bool equals(const Derived& expected, double tol) const = 0; - * + * */ // \callgraph @@ -123,7 +123,7 @@ namespace gtsam { double tol_; equals_star(double tol = 1e-9) : tol_(tol) {} bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { - if (!actual || !expected) return true; + if (!actual && !expected) return true; return actual && expected && traits::Equals(*actual,*expected, tol_); } }; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index ed6373f5b..61a42fead 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -292,7 +292,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, // Basically, instead of doing a normal QR step with the weighted // pseudoinverse, we enforce the constraint by turning // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0 - pseudo = delta(m, i, 1 / a[i]); + pseudo = delta(m, i, 1.0 / a[i]); return inf; } } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 20477a205..f87703e2b 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -41,13 +41,25 @@ typedef Eigen::VectorXd Vector; typedef Eigen::Matrix Vector1; typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; -typedef Eigen::Matrix Vector4; -typedef Eigen::Matrix Vector5; -typedef Eigen::Matrix Vector6; -typedef Eigen::Matrix Vector7; -typedef Eigen::Matrix Vector8; -typedef Eigen::Matrix Vector9; -typedef Eigen::Matrix Vector10; + +static const Eigen::MatrixBase::ConstantReturnType Z_2x1 = Vector2::Zero(); +static const Eigen::MatrixBase::ConstantReturnType Z_3x1 = Vector3::Zero(); + +// Create handy typedefs and constants for vectors with N>3 +// VectorN and Z_Nx1, for N=1..9 +#define GTSAM_MAKE_VECTOR_DEFS(N) \ + typedef Eigen::Matrix Vector##N; \ + static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero(); + +GTSAM_MAKE_VECTOR_DEFS(4); +GTSAM_MAKE_VECTOR_DEFS(5); +GTSAM_MAKE_VECTOR_DEFS(6); +GTSAM_MAKE_VECTOR_DEFS(7); +GTSAM_MAKE_VECTOR_DEFS(8); +GTSAM_MAKE_VECTOR_DEFS(9); +GTSAM_MAKE_VECTOR_DEFS(10); +GTSAM_MAKE_VECTOR_DEFS(11); +GTSAM_MAKE_VECTOR_DEFS(12); typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; @@ -89,7 +101,7 @@ inline Vector zero(size_t n) { return Vector::Zero(n);} * @param n size */ inline Vector ones(size_t n) { return Vector::Ones(n); } - + /** * check if all zero */ diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index c356dcc07..cc28ac893 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -43,7 +43,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); - return origin + (Class)v; + return origin + v; } /// @} @@ -117,7 +117,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) *H1 = Eye(origin); if (H2) *H2 = Eye(origin); - return origin + Class(v); + return origin + v; } /// @} @@ -159,12 +159,34 @@ struct VectorSpaceImpl { /// @} }; -/// A helper that implements the traits interface for GTSAM vector space types. -/// To use this for your gtsam type, define: -/// template<> struct traits : public VectorSpaceTraits { }; +/// Requirements on type to pass it to Manifold template below +template +struct HasVectorSpacePrereqs { + + enum { dim = Class::dimension }; + + Class p, q; + Vector v; + + BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) { + p = Class::identity(); // identity + q = p + p; // addition + q = p - p; // subtraction + v = p.vector(); // conversion to vector + q = p + v; // addition of a vector on the right + } +}; + +/// A helper that implements the traits interface for *classes* that define vector spaces +/// To use this for your class, define: +/// template<> struct traits : public VectorSpaceTraits {}; +/// The class needs to support the requirements defined by HasVectorSpacePrereqs above template struct VectorSpaceTraits: VectorSpaceImpl { + // Check that Class has the necessary machinery + BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs)); + typedef vector_space_tag structure_category; /// @name Group @@ -180,12 +202,11 @@ struct VectorSpaceTraits: VectorSpaceImpl { /// @} }; -/// Both VectorSpaceTRaits and Testable +/// VectorSpace provides both Testable and VectorSpaceTraits template struct VectorSpace: Testable, VectorSpaceTraits {}; -/// A helper that implements the traits interface for GTSAM lie groups. -/// To use this for your gtsam type, define: +/// A helper that implements the traits interface for scalar vector spaces. Usage: /// template<> struct traits : public ScalarTraits { }; template struct ScalarTraits : VectorSpaceImpl { diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index c6a3eb034..9cd566ecf 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -165,24 +165,24 @@ namespace gtsam { return variableColOffsets_[actualBlock]; } + /** Get the apparent first row of the underlying matrix for all operations */ + const DenseIndex& rowStart() const { return rowStart_; } + /** Get or set the apparent first row of the underlying matrix for all operations */ DenseIndex& rowStart() { return rowStart_; } + /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ + const DenseIndex& rowEnd() const { return rowEnd_; } + /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ DenseIndex& rowEnd() { return rowEnd_; } + /** Get the apparent first block for all operations */ + const DenseIndex& firstBlock() const { return blockStart_; } + /** Get or set the apparent first block for all operations */ DenseIndex& firstBlock() { return blockStart_; } - /** Get the apparent first row of the underlying matrix for all operations */ - DenseIndex rowStart() const { return rowStart_; } - - /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ - DenseIndex rowEnd() const { return rowEnd_; } - - /** Get the apparent first block for all operations */ - DenseIndex firstBlock() const { return blockStart_; } - /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ const Matrix& matrix() const { return matrix_; } diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index a7dc37d55..9a42db3ce 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -131,11 +131,11 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(boost::funct typedef typename TraitsX::TangentVector TangentX; // get value at x, and corresponding chart - Y hx = h(x); + const Y hx = h(x); // Bit of a hack for now to find number of rows - TangentY zeroY = TraitsY::Local(hx, hx); - size_t m = zeroY.size(); + const TangentY zeroY = TraitsY::Local(hx, hx); + const size_t m = zeroY.size(); // Prepare a tangent vector to perturb x with, only works for fixed size TangentX dx; @@ -143,12 +143,12 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(boost::funct // Fill in Jacobian H Matrix H = zeros(m, N); - double factor = 1.0 / (2.0 * delta); + const double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; - TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); + const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = -delta; - TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); + const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = 0; H.col(j) << (dy1 - dy2) * factor; } diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp index 19870fdeb..4909f4ecb 100644 --- a/gtsam/base/tests/testFastContainers.cpp +++ b/gtsam/base/tests/testFastContainers.cpp @@ -22,11 +22,11 @@ using namespace gtsam; /* ************************************************************************* */ TEST( testFastContainers, KeySet ) { - FastVector init_vector; + KeyVector init_vector; init_vector += 2, 3, 4, 5; - FastSet actSet(init_vector); - FastSet expSet; expSet += 2, 3, 4, 5; + KeySet actSet(init_vector); + KeySet expSet; expSet += 2, 3, 4, 5; EXPECT(actSet == expSet); } diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 84c94e73d..c4775b672 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -25,6 +25,7 @@ #include // for GTSAM_USE_TBB #include +#include #ifdef GTSAM_USE_TBB #include @@ -53,7 +54,7 @@ namespace gtsam { /// Integer nonlinear key type - typedef size_t Key; + typedef std::uint64_t Key; /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; diff --git a/gtsam/config.h.in b/gtsam/config.h.in index cb195dc03..f9a576d14 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -32,11 +32,11 @@ #cmakedefine GTSAM_USE_QUATERNIONS // Whether GTSAM is compiled to use Pose3::EXPMAP as the default coordinates mode for Pose3's retract and localCoordinates (otherwise, Pose3::FIRST_ORDER will be used) -#cmakedefine GTSAM_POSE3_EXPMAP +#cmakedefine GTSAM_POSE3_EXPMAP // Whether GTSAM is compiled to use Rot3::EXPMAP as the default coordinates mode for Rot3's retract and localCoordinates (otherwise, Pose3::CAYLEY will be used) #ifndef GTSAM_USE_QUATERNIONS - #cmakedefine GTSAM_ROT3_EXPMAP + #cmakedefine GTSAM_ROT3_EXPMAP #endif // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake) @@ -65,3 +65,6 @@ // Publish flag about Eigen typedef #cmakedefine GTSAM_USE_VECTOR3_POINTS + +// Support Metis-based nested dissection +#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 970a18b42..d6902bbef 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -421,16 +421,16 @@ TEST(ADT, conversion) ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6"); dot(fDiscreteKey, "conversion-f1"); - std::map ordering; - ordering[0] = 5; - ordering[1] = 2; + std::map keyMap; + keyMap[0] = 5; + keyMap[1] = 2; - AlgebraicDecisionTree fIndexKey(fDiscreteKey, ordering); + AlgebraicDecisionTree fIndexKey(fDiscreteKey, keyMap); // f1.print("f1"); // f2.print("f2"); dot(fIndexKey, "conversion-f2"); - Assignment x00, x01, x02, x10, x11, x12; + Assignment x00, x01, x02, x10, x11, x12; x00[5] = 0, x00[2] = 0; x01[5] = 0, x01[2] = 1; x10[5] = 1, x10[2] = 0; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 4e62ca7e9..ac4b68ccd 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -95,6 +95,11 @@ public: return fy_; } + /// aspect ratio + inline double aspectRatio() const { + return fx_/fy_; + } + /// skew inline double skew() const { return s_; diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 699705fa5..02ede9228 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -12,6 +12,17 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb, + OptionalJacobian<5, 3> H1, + OptionalJacobian<5, 2> H2) { + if (H1) + *H1 << I_3x3, Matrix23::Zero(); + if (H2) + *H2 << Matrix32::Zero(), I_2x2; + return EssentialMatrix(aRb, aTb); +} + /* ************************************************************************* */ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb, OptionalJacobian<5, 6> H) { diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 697bd462d..9dec574eb 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -52,6 +52,11 @@ public: Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) { } + /// Named constructor with derivatives + static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb, + OptionalJacobian<5, 3> H1 = boost::none, + OptionalJacobian<5, 2> H2 = boost::none); + /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) static EssentialMatrix FromPose3(const Pose3& _1P2_, OptionalJacobian<5, 6> H = boost::none); diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index bb9925e23..aa023a09a 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -57,13 +57,31 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d); } - /* ************************************************************************* */ Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { Vector2 n_error = -n_.localCoordinates(plane.n_); return Vector3(n_error(0), n_error(1), d_ - plane.d_); } +/* ************************************************************************* */ +Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + Matrix22 H_n_error_this, H_n_error_other; + Vector2 n_error = n_.errorVector(other.normal(), H1 ? &H_n_error_this : 0, + H2 ? &H_n_error_other : 0); + + double d_error = d_ - other.d_; + + if (H1) { + *H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1; + } + if (H2) { + *H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1; + } + + return Vector3(n_error(0), n_error(1), d_error); +} + /* ************************************************************************* */ OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2)); diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 5c9a5cdef..e425a4b81 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -114,6 +114,15 @@ public: */ Vector3 error(const OrientedPlane3& plane) const; + /** Computes the error between the two planes, with derivatives. + * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses + * Unit3::localCoordinates. This one has correct derivatives. + * NOTE(hayk): The derivatives are zero when normals are exactly orthogonal. + * @param the other plane + */ + Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, // + OptionalJacobian<3, 3> H2 = boost::none) const; + /// Dimensionality of tangent space = 3 DOF inline static size_t Dim() { return 3; diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index ac453e048..ac889c9d7 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -328,6 +328,11 @@ public: virtual ~PinholePose() { } + /// return shared pointer to calibration + const boost::shared_ptr& sharedCalibration() const { + return K_; + } + /// return calibration virtual const CALIBRATION& calibration() const { return *K_; diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 4b2111efc..7066e527b 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -140,4 +140,10 @@ ostream &operator<<(ostream &os, const Point2& p) { return os; } +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) { + os << p.first << " <-> " << p.second; + return os; +} + } // namespace gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 56809ae53..3099a8bb3 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -50,7 +50,7 @@ public: /// @{ /// construct from 2D vector - Point2(const Vector2& v) { + explicit Point2(const Vector2& v) { x_ = v(0); y_ = v(1); } @@ -112,6 +112,11 @@ public: /// inverse inline Point2 operator- () const {return Point2(-x_,-y_);} + /// add vector on right + inline Point2 operator +(const Vector2& v) const { + return Point2(x_ + v[0], y_ + v[1]); + } + /// add inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} @@ -194,6 +199,10 @@ private: /// @} }; +// Convenience typedef +typedef std::pair Point2Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); + // For MATLAB wrapper typedef std::vector Point2Vector; diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 01b5ed132..df0f78283 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -73,7 +73,7 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = I_3x3; - if (H2) *H2 = I_3x3; + if (H2) *H2 = -I_3x3; return *this - q; } #endif diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 8b31316c7..fd254e51c 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -51,17 +51,12 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @name Standard Constructors /// @{ -#ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - /// Default constructor no longer initializes, just like Vector3 - Point3() {} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + // Deprecated default constructor initializes to zero, in contrast to new behavior below + Point3() { setZero(); } #endif - /// Construct from x, y, and z coordinates - Point3(double x, double y, double z): Vector3(x,y, z) {} - - /// Construct from other vector - template - inline Point3(const Eigen::MatrixBase& v): Vector3(v) {} + using Vector3::Vector3; /// @} /// @name Testable @@ -126,7 +121,6 @@ class GTSAM_EXPORT Point3 : public Vector3 { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ - Point3() { setZero(); } // initializes to zero, in contrast to new behavior Point3 inverse() const { return -(*this);} Point3 compose(const Point3& q) const { return (*this)+q;} Point3 between(const Point3& q) const { return q-(*this);} diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 89f6b3754..1aa8f060a 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -216,7 +216,7 @@ Point2 Pose2::transform_from(const Point2& point, OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2); const Point2 q = r_.rotate(point, Hrotation, Hpoint); - if (Htranslation) *Htranslation = Hpoint ? *Hpoint : r_.matrix(); + if (Htranslation) *Htranslation = (Hpoint ? *Hpoint : r_.matrix()); return q + t_; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f1cb482bb..d170282fe 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -36,6 +36,14 @@ Pose3::Pose3(const Pose2& pose2) : Point3(pose2.x(), pose2.y(), 0)) { } +/* ************************************************************************* */ +Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> H1, + OptionalJacobian<6, 3> H2) { + if (H1) *H1 << I_3x3, Z_3x3; + if (H2) *H2 << Z_3x3, R.transpose(); + return Pose3(R, t); +} + /* ************************************************************************* */ Pose3 Pose3::inverse() const { Rot3 Rt = R_.inverse(); @@ -193,8 +201,8 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { * The closed-form formula is similar to formula 102 in Barfoot14tro) */ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { - const Vector3 w = xi.head<3>(); - const Vector3 v = xi.tail<3>(); + const auto w = xi.head<3>(); + const auto v = xi.tail<3>(); const Matrix3 V = skewSymmetric(v); const Matrix3 W = skewSymmetric(w); @@ -260,9 +268,18 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { return t_; } +/* ************************************************************************* */ + +const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const { + if (H) { + *H << I_3x3, Z_3x3; + } + return R_; +} + /* ************************************************************************* */ Matrix4 Pose3::matrix() const { - static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished(); + static const auto A14 = Eigen::RowVector4d(0,0,0,1); Matrix4 mat; mat << R_.matrix(), t_, A14; return mat; @@ -275,6 +292,14 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { return Pose3(cRv, t); } +/* ************************************************************************* */ +Pose3 Pose3::transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1, + OptionalJacobian<6, 6> H2) const { + if (H1) *H1 = -pose.inverse().AdjointMap() * AdjointMap(); + if (H2) *H2 = I_6x6; + return inverse() * pose; +} + /* ************************************************************************* */ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, OptionalJacobian<3,3> Dpoint) const { @@ -355,41 +380,54 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, } /* ************************************************************************* */ -boost::optional align(const vector& pairs) { - const size_t n = pairs.size(); +boost::optional Pose3::Align(const std::vector& abPointPairs) { + const size_t n = abPointPairs.size(); if (n < 3) - return boost::none; // we need at least three pairs + return boost::none; // we need at least three pairs // calculate centroids - Point3 cp(0,0,0), cq(0,0,0); - BOOST_FOREACH(const Point3Pair& pair, pairs) { - cp += pair.first; - cq += pair.second; + Point3 aCentroid(0,0,0), bCentroid(0,0,0); + for(const Point3Pair& abPair: abPointPairs) { + aCentroid += abPair.first; + bCentroid += abPair.second; } double f = 1.0 / n; - cp *= f; - cq *= f; + aCentroid *= f; + bCentroid *= f; // Add to form H matrix Matrix3 H = Z_3x3; - BOOST_FOREACH(const Point3Pair& pair, pairs) { - Point3 dp = pair.first - cp; - Point3 dq = pair.second - cq; - H += dp * dq.transpose(); + for(const Point3Pair& abPair: abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + H += db * da.transpose(); } -// Compute SVD - Matrix U, V; - Vector S; - svd(H, U, S, V); + // Compute SVD + Eigen::JacobiSVD svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV); + Matrix U = svd.matrixU(); + Vector S = svd.singularValues(); + Matrix V = svd.matrixV(); + + // Check rank + if (S[1] < 1e-10) + return boost::none; // Recover transform with correction from Eggert97machinevisionandapplications Matrix3 UVtranspose = U * V.transpose(); Matrix3 detWeighting = I_3x3; detWeighting(2, 2) = UVtranspose.determinant(); - Rot3 R(Matrix3(V * detWeighting * U.transpose())); - Point3 t = Point3(cq) - R * Point3(cp); - return Pose3(R, t); + Rot3 aRb(Matrix(V * detWeighting * U.transpose())); + Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); + return Pose3(aRb, aTb); +} + +boost::optional align(const vector& baPointPairs) { + vector abPointPairs; + BOOST_FOREACH (const Point3Pair& baPair, baPointPairs) { + abPointPairs.push_back(make_pair(baPair.second, baPair.first)); + } + return Pose3::Align(abPointPairs); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ba4b9737b..ee1206119 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -73,6 +73,19 @@ public: T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { } + /// Named constructor with derivatives + static Pose3 Create(const Rot3& R, const Point3& t, + OptionalJacobian<6, 3> H1 = boost::none, + OptionalJacobian<6, 3> H2 = boost::none); + + /** + * Create Pose3 by aligning two point pairs + * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point + * Meant to replace the deprecated function 'align', which orders the pairs the opposite way. + * Note this allows for noise on the points but in that case the mapping will not be exact. + */ + static boost::optional Align(const std::vector& abPointPairs); + /// @} /// @name Testable /// @{ @@ -213,9 +226,7 @@ public: /// @{ /// get rotation - const Rot3& rotation() const { - return R_; - } + const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const; /// get translation const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; @@ -238,9 +249,15 @@ public: /** convert to 4*4 matrix */ Matrix4 matrix() const; - /** receives a pose in world coordinates and transforms it to local coordinates */ + /** receives a pose in local coordinates and transforms it to world coordinates + * @deprecated: This is actually equivalent to transform_from, so it is WRONG! Use + * transform_pose_to instead. */ Pose3 transform_to(const Pose3& pose) const; + /** receives a pose in world coordinates and transforms it to local coordinates */ + Pose3 transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; + /** * Calculate range to a landmark * @param point 3D location of landmark @@ -291,7 +308,7 @@ public: GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); -private: + private: /** Serialization function */ friend class boost::serialization::access; template @@ -317,11 +334,11 @@ inline Matrix wedge(const Vector& xi) { } /** - * Calculate pose between a vector of 3D point correspondences (p,q) - * where q = Pose3::transform_from(p) = t + R*p + * Calculate pose between a vector of 3D point correspondences (b_point, a_point) + * where a_point = Pose3::transform_from(b_point) = t + R*b_point + * @deprecated: use Pose3::Align with point pairs ordered the opposite way */ -typedef std::pair Point3Pair; -GTSAM_EXPORT boost::optional align(const std::vector& pairs); +GTSAM_EXPORT boost::optional align(const std::vector& baPointPairs); // For MATLAB wrapper typedef std::vector Pose3Vector; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 696c9df68..c2e711a17 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -42,6 +42,58 @@ Rot3 Rot3::Random(boost::mt19937& rng) { return AxisAngle(axis, angle); } + + +/* ************************************************************************* */ +Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) { + // if a_p is already aligned with b_p, return the identity rotation + if (std::abs(a_p.dot(b_p)) > 0.999999999) { + return Rot3(); + } + + // Check axis was not degenerate cross product + const Vector3 z = axis.unitVector(); + if (z.hasNaN()) + throw std::runtime_error("AlignSinglePair: axis has Nans"); + + // Now, calculate rotation that takes b_p to a_p + const Matrix3 P = I_3x3 - z * z.transpose(); // orthogonal projector + const Vector3 a_po = P * a_p.unitVector(); // point in a orthogonal to axis + const Vector3 b_po = P * b_p.unitVector(); // point in b orthogonal to axis + const Vector3 x = a_po.normalized(); // x-axis in axis-orthogonal plane, along a_p vector + const Vector3 y = z.cross(x); // y-axis in axis-orthogonal plane + const double u = x.dot(b_po); // x-coordinate for b_po + const double v = y.dot(b_po); // y-coordinate for b_po + double angle = std::atan2(v, u); + return Rot3::AxisAngle(z, -angle); +} + +/* ************************************************************************* */ +Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // + const Unit3& a_q, const Unit3& b_q) { + // there are three frames in play: + // a: the first frame in which p and q are measured + // b: the second frame in which p and q are measured + // i: intermediate, after aligning first pair + + // First, find rotation around that aligns a_p and b_p + Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p); + + // Rotate points in frame b to the intermediate frame, + // in which we expect the point p to be aligned now + Unit3 i_q = i_R_b * b_q; + assert(assert_equal(a_p, i_R_b * b_p, 1e-6)); + + // Now align second pair: we need to align i_q to a_q + Rot3 a_R_i = AlignPair(a_p, a_q, i_q); + assert(assert_equal(a_p, a_R_i * a_p, 1e-6)); + assert(assert_equal(a_q, a_R_i * i_q, 1e-6)); + + // The desired rotation is the product of both + Rot3 a_R_b = a_R_i * i_R_b; + return a_R_b; +} + /* ************************************************************************* */ bool Rot3::equals(const Rot3 & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); @@ -172,10 +224,7 @@ ostream &operator<<(ostream &os, const Rot3& R) { /* ************************************************************************* */ Rot3 Rot3::slerp(double t, const Rot3& other) const { - // amazingly simple in GTSAM :-) - assert(t>=0 && t<=1); - Vector3 omega = Logmap(between(other)); - return compose(Expmap(t * omega)); + return interpolate(*this, other, t); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 057fdf558..79b13b93e 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -215,6 +215,13 @@ namespace gtsam { return Rodrigues(Vector3(wx, wy, wz)); } + /// Determine a rotation to bring two vectors into alignment, using the rotation axis provided + static Rot3 AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p); + + /// Calculate rotation from two pairs of homogeneous points using two successive rotations + static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // + const Unit3& a_q, const Unit3& b_q); + /// @} /// @name Testable /// @{ @@ -456,9 +463,6 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ -#ifndef GTSAM_USE_VECTOR3_POINTS - static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } -#endif static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 03cde1a01..529c64973 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -36,9 +36,9 @@ Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = (Vector3)col1; - rot_.col(1) = (Vector3)col2; - rot_.col(2) = (Vector3)col3; + rot_.col(0) = col1; + rot_.col(1) = col2; + rot_.col(2) = col3; } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 459f15561..bd111d9b1 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -29,11 +29,12 @@ namespace so3 { void ExpmapFunctor::init(bool nearZeroApprox) { nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); - if (nearZero) return; - theta = std::sqrt(theta2); // rotation angle - sin_theta = std::sin(theta); - const double s2 = std::sin(theta / 2.0); - one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + if (!nearZero) { + theta = std::sqrt(theta2); // rotation angle + sin_theta = std::sin(theta); + const double s2 = std::sin(theta / 2.0); + one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + } } ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) @@ -42,7 +43,6 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; init(nearZeroApprox); if (!nearZero) { - theta = std::sqrt(theta2); K = W / theta; KK = K * K; } @@ -55,7 +55,6 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApp W = K * angle; init(nearZeroApprox); if (!nearZero) { - theta = angle; KK = K * K; } } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 1b9559e67..961cc041b 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -50,7 +50,7 @@ public: } /// construct from 3D vector - StereoPoint2(const Vector3& v) : + explicit StereoPoint2(const Vector3& v) : uL_(v(0)), uR_(v(1)), v_(v(2)) {} /// @} @@ -80,6 +80,11 @@ public: return StereoPoint2(-uL_, -uR_, -v_); } + /// add vector on right + inline StereoPoint2 operator +(const Vector3& v) const { + return StereoPoint2(uL_ + v[0], uR_ + v[1], v_ + v[2]); + } + /// add inline StereoPoint2 operator +(const StereoPoint2& b) const { return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index a64f99eb1..dacb5c3fd 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -60,7 +60,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). boost::variate_generator > generator( rng, randomDirection); - vector d = generator(); + const vector d = generator(); return Unit3(d[0], d[1], d[2]); } @@ -100,16 +100,16 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with // the chosen axis. Matrix33 H_B1_n; - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : 0); + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); // Normalize result to get a unit vector: b1 = B1 / |B1|. Matrix33 H_b1_B1; - Point3 b1 = normalize(B1, H ? &H_b1_B1 : 0); + Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // No need to normalize this, p and b1 are orthogonal unit vectors. Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); + Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); // Create the basis by stacking b1 and b2. B_.reset(Matrix32()); @@ -118,8 +118,8 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { if (H) { // Chain rule tomfoolery to compute the derivative. const Matrix32& H_n_p = *B_; - Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; // Cache the derivative and fill the result. H_B_.reset(Matrix62()); @@ -164,14 +164,14 @@ Matrix3 Unit3::skew() const { double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const { // Get the unit vectors of each, and the derivative. Matrix32 H_pn_p; - Point3 pn = point3(H_p ? &H_pn_p : 0); + Point3 pn = point3(H_p ? &H_pn_p : nullptr); Matrix32 H_qn_q; - Point3 qn = q.point3(H_q ? &H_qn_q : 0); + const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr); // Compute the dot product of the Point3s. Matrix13 H_dot_pn, H_dot_qn; - double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); + double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : nullptr, H_q ? &H_dot_qn : nullptr); if (H_p) { (*H_p) << H_dot_pn * H_pn_p; @@ -187,10 +187,9 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1, /* ************************************************************************* */ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) 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_; + const Vector2 xi = basis().transpose() * q.p_; if (H_q) { - *H_q = Bt * q.basis(); + *H_q = basis().transpose() * q.basis(); } return xi; } @@ -199,11 +198,11 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { // Get the point3 of this, and the derivative. Matrix32 H_qn_q; - Point3 qn = q.point3(H_q ? &H_qn_q : 0); + const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr); // 2D error here is projecting q into the tangent plane of this (p). Matrix62 H_B_p; - Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose(); + Matrix23 Bt = basis(H_p ? &H_B_p : nullptr).transpose(); Vector2 xi = Bt * qn; if (H_p) { @@ -212,18 +211,18 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); // Derivatives of the two entries of xi wrt the basis vectors. - Matrix13 H_xi1_b1 = qn.transpose(); - Matrix13 H_xi2_b2 = qn.transpose(); + const Matrix13 H_xi1_b1 = qn.transpose(); + const Matrix13 H_xi2_b2 = qn.transpose(); // Assemble dxi/dp = dxi/dB * dB/dp. - Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; - Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p; + const Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; + const Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p; *H_p << H_xi1_p, H_xi2_p; } if (H_q) { // dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q. - Matrix23 H_xi_qu = Bt; + const Matrix23 H_xi_qu = Bt; *H_q = H_xi_qu * H_qn_q; } @@ -232,26 +231,27 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ /* ************************************************************************* */ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { - Matrix2 H_; - Vector2 xi = error(q, H_); - double theta = xi.norm(); + Matrix2 H_xi_q; + const Vector2 xi = error(q, H ? &H_xi_q : nullptr); + const double theta = xi.norm(); if (H) - *H = (xi.transpose() / theta) * H_; + *H = (xi.transpose() / theta) * H_xi_q; return theta; } /* ************************************************************************* */ Unit3 Unit3::retract(const Vector2& v) const { // Compute the 3D xi_hat vector - Vector3 xi_hat = basis() * v; - double theta = xi_hat.norm(); + const Vector3 xi_hat = basis() * v; + const double theta = xi_hat.norm(); // Treat case of very small v differently if (theta < std::numeric_limits::epsilon()) { return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); } - Vector3 exp_p_xi_hat = std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); + const Vector3 exp_p_xi_hat = + std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 1ce790a1b..9f21bb82a 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -65,14 +65,7 @@ public: p_(1.0, 0.0, 0.0) { } -#ifndef GTSAM_USE_VECTOR3_POINTS /// Construct from point - explicit Unit3(const Point3& p) : - p_(p.vector().normalized()) { - } -#endif - - /// Construct from a vector3 explicit Unit3(const Vector3& p) : p_(p.normalized()) { } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index bfff0a182..79759678d 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -20,20 +20,41 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix) //************************************************************************* // Create two cameras and corresponding essential matrix E -Rot3 c1Rc2 = Rot3::Yaw(M_PI_2); -Point3 c1Tc2(0.1, 0, 0); -EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); +Rot3 trueRotation = Rot3::Yaw(M_PI_2); +Point3 trueTranslation(0.1, 0, 0); +Unit3 trueDirection(trueTranslation); +EssentialMatrix trueE(trueRotation, trueDirection); //************************************************************************* TEST (EssentialMatrix, equality) { - EssentialMatrix actual(c1Rc2, Unit3(c1Tc2)), expected(c1Rc2, Unit3(c1Tc2)); + EssentialMatrix actual(trueRotation, trueDirection), expected(trueRotation, trueDirection); EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST(EssentialMatrix, FromRotationAndDirection) { + Matrix actualH1, actualH2; + EXPECT(assert_equal( + trueE, EssentialMatrix::FromRotationAndDirection(trueRotation, trueDirection, actualH1, actualH2), + 1e-8)); + + Matrix expectedH1 = numericalDerivative11( + boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none, + boost::none), + trueRotation); + EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); + + Matrix expectedH2 = numericalDerivative11( + boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none, + boost::none), + trueDirection); + EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); +} + //************************************************************************* TEST (EssentialMatrix, FromPose3) { - EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)); - Pose3 pose(c1Rc2, c1Tc2); + EssentialMatrix expected(trueRotation, trueDirection); + Pose3 pose(trueRotation, trueTranslation); EssentialMatrix actual = EssentialMatrix::FromPose3(pose); EXPECT(assert_equal(expected, actual)); } @@ -50,7 +71,7 @@ TEST(EssentialMatrix, localCoordinates0) { TEST (EssentialMatrix, localCoordinates) { // Pose between two cameras - Pose3 pose(c1Rc2, c1Tc2); + Pose3 pose(trueRotation, trueTranslation); EssentialMatrix hx = EssentialMatrix::FromPose3(pose); Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); EXPECT(assert_equal(zero(5), actual, 1e-8)); @@ -70,15 +91,15 @@ TEST (EssentialMatrix, retract0) { //************************************************************************* TEST (EssentialMatrix, retract1) { - EssentialMatrix expected(c1Rc2.retract(Vector3(0.1, 0, 0)), Unit3(c1Tc2)); + EssentialMatrix expected(trueRotation.retract(Vector3(0.1, 0, 0)), trueDirection); EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0).finished()); EXPECT(assert_equal(expected, actual)); } //************************************************************************* TEST (EssentialMatrix, retract2) { - EssentialMatrix expected(c1Rc2, - Unit3(c1Tc2).retract(Vector2(0.1, 0))); + EssentialMatrix expected(trueRotation, + trueDirection.retract(Vector2(0.1, 0))); EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished()); EXPECT(assert_equal(expected, actual)); } @@ -124,8 +145,8 @@ TEST (EssentialMatrix, rotate) { Rot3 bRc(bX, bZ, -bY), cRb = bRc.inverse(); // Let's compute the ground truth E in body frame: - Rot3 b1Rb2 = bRc * c1Rc2 * cRb; - Point3 b1Tb2 = bRc * c1Tc2; + Rot3 b1Rb2 = bRc * trueRotation * cRb; + Point3 b1Tb2 = bRc * trueTranslation; EssentialMatrix bodyE(b1Rb2, Unit3(b1Tb2)); EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8)); EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8)); @@ -149,7 +170,7 @@ TEST (EssentialMatrix, rotate) { //************************************************************************* TEST (EssentialMatrix, FromPose3_a) { Matrix actualH; - Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); @@ -171,7 +192,7 @@ TEST (EssentialMatrix, FromPose3_b) { //************************************************************************* TEST (EssentialMatrix, streaming) { - EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)), actual; + EssentialMatrix expected(trueRotation, trueDirection), actual; stringstream ss; ss << expected; ss >> actual; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 180abb0d6..d3a107570 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -139,6 +139,30 @@ TEST(OrientedPlane3, localCoordinates_retract) { } } +//******************************************************************************* +TEST (OrientedPlane3, error2) { + OrientedPlane3 plane1(-1, 0.1, 0.2, 5); + OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); + + // Hard-coded regression values, to ensure the result doesn't change. + EXPECT(assert_equal(zero(3), plane1.errorVector(plane1), 1e-8)); + EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5)); + + // Test the jacobians of transform + Matrix33 actualH1, expectedH1, actualH2, expectedH2; + + Vector3 actual = plane1.errorVector(plane2, actualH1, actualH2); + EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), Vector2(actual[0], actual[1]))); + EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); + + boost::function f = // + boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none); + expectedH1 = numericalDerivative21(f, plane1, plane2); + expectedH2 = numericalDerivative22(f, plane1, plane2); + EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); +} + /* ************************************************************************* */ int main() { srand(time(NULL)); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a4e2b139e..e457da887 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -81,6 +81,71 @@ TEST( Point3, dot) { Point3 origin(0,0,0), ones(1, 1, 1); CHECK(origin.dot(Point3(1, 1, 0)) == 0); CHECK(ones.dot(Point3(1, 1, 0)) == 2); + + Point3 p(1, 0.2, 0.3); + Point3 q = p + Point3(0.5, 0.2, -3.0); + Point3 r = p + Point3(0.8, 0, 0); + Point3 t = p + Point3(0, 0.3, -0.4); + EXPECT(assert_equal(1.130000, p.dot(p), 1e-8)); + EXPECT(assert_equal(0.770000, p.dot(q), 1e-5)); + EXPECT(assert_equal(1.930000, p.dot(r), 1e-5)); + EXPECT(assert_equal(1.070000, p.dot(t), 1e-5)); + + // Use numerical derivatives to calculate the expected Jacobians + Matrix H1, H2; + boost::function f = boost::bind(&Point3::dot, _1, _2, // + boost::none, boost::none); + { + p.dot(q, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); + } + { + p.dot(r, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); + } + { + p.dot(t, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, t), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, t), H2, 1e-9)); + } +} + +/* ************************************************************************* */ +TEST(Point3, cross) { + Matrix aH1, aH2; + boost::function f = + boost::bind(>sam::cross, _1, _2, boost::none, boost::none); + const Point3 omega(0, 1, 0), theta(4, 6, 8); + cross(omega, theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + +/* ************************************************************************* */ +TEST( Point3, cross2) { + Point3 p(1, 0.2, 0.3); + Point3 q = p + Point3(0.5, 0.2, -3.0); + Point3 r = p + Point3(0.8, 0, 0); + EXPECT(assert_equal(Point3(0, 0, 0), p.cross(p), 1e-8)); + EXPECT(assert_equal(Point3(-0.66, 3.15, 0.1), p.cross(q), 1e-5)); + EXPECT(assert_equal(Point3(0, 0.24, -0.16), p.cross(r), 1e-5)); + + // Use numerical derivatives to calculate the expected Jacobians + Matrix H1, H2; + boost::function f = boost::bind(&Point3::cross, _1, _2, // + boost::none, boost::none); + { + p.cross(q, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); + } + { + p.cross(r, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); + } } /* ************************************************************************* */ @@ -135,17 +200,6 @@ TEST (Point3, distance) { EXPECT(assert_equal(numH2, H2, 1e-8)); } -/* ************************************************************************* */ -TEST(Point3, cross) { - Matrix aH1, aH2; - boost::function f = - boost::bind(>sam::cross, _1, _2, boost::none, boost::none); - const Point3 omega(0, 1, 0), theta(4, 6, 8); - cross(omega, theta, aH1, aH2); - EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index af8e7c6a0..1aba34bd6 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -31,12 +31,13 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3) -static Point3 P(0.2,0.7,-2); -static Rot3 R = Rot3::Rodrigues(0.3,0,0); -static Pose3 T(R,Point3(3.5,-8.2,4.2)); -static Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); -static Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3)); -const double tol=1e-5; +static const Point3 P(0.2,0.7,-2); +static const Rot3 R = Rot3::Rodrigues(0.3,0,0); +static const Point3 P2(3.5,-8.2,4.2); +static const Pose3 T(R,P2); +static const Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),P2); +static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3)); +static const double tol=1e-5; /* ************************************************************************* */ TEST( Pose3, equals) @@ -197,6 +198,17 @@ TEST(Pose3, translation) { EXPECT(assert_equal(numericalH, actualH, 1e-6)); } +/* ************************************************************************* */ +// Check rotation and its pushforward +TEST(Pose3, rotation) { + Matrix actualH; + EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); + + Matrix numericalH = numericalDerivative11( + boost::bind(&Pose3::rotation, _1, boost::none), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + /* ************************************************************************* */ TEST(Pose3, Adjoint_compose_full) { @@ -403,6 +415,41 @@ TEST( Pose3, transform_to) EXPECT(assert_equal(expected, actual, 0.001)); } +Pose3 transform_pose_to_(const Pose3& pose, const Pose3& pose2) { return pose.transform_pose_to(pose2); } + +/* ************************************************************************* */ +TEST( Pose3, transform_pose_to) +{ + Pose3 origin = T.transform_pose_to(T); + EXPECT(assert_equal(Pose3{}, origin)); +} + +/* ************************************************************************* */ +TEST( Pose3, transform_pose_to_with_derivatives) +{ + Matrix actH1, actH2; + Pose3 res = T.transform_pose_to(T2,actH1,actH2); + EXPECT(assert_equal(res, T.inverse().compose(T2))); + + Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T2), + expH2 = numericalDerivative22(transform_pose_to_, T, T2); + EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH2, actH2, 1e-8)); +} + +/* ************************************************************************* */ +TEST( Pose3, transform_pose_to_with_derivatives2) +{ + Matrix actH1, actH2; + Pose3 res = T.transform_pose_to(T3,actH1,actH2); + EXPECT(assert_equal(res, T.inverse().compose(T3))); + + Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T3), + expH2 = numericalDerivative22(transform_pose_to_, T, T3); + EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH2, actH2, 1e-8)); +} + /* ************************************************************************* */ TEST( Pose3, transform_from) { @@ -476,7 +523,7 @@ TEST(Pose3, Retract_LocalCoordinates) { Vector6 d; d << 1,2,3,4,5,6; d/=10; - R = Rot3::Retract(d.head<3>()); + const Rot3 R = Rot3::Retract(d.head<3>()); Pose3 t = Pose3::Retract(d); EXPECT(assert_equal(d, Pose3::LocalCoordinates(t))); } @@ -506,6 +553,8 @@ TEST(Pose3, retract_localCoordinates2) EXPECT(assert_equal(t2, t1.retract(d12))); Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); + // TODO(hayk): This currently fails! + // EXPECT(assert_equal(d12, -d21)); } /* ************************************************************************* */ TEST(Pose3, manifold_expmap) @@ -637,16 +686,26 @@ TEST( Pose3, range_pose ) Unit3 bearing_proxy(const Pose3& pose, const Point3& point) { return pose.bearing(point); } -TEST( Pose3, bearing ) -{ +TEST(Pose3, Bearing) { Matrix expectedH1, actualH1, expectedH2, actualH2; - EXPECT(assert_equal(Unit3(1,0,0),x1.bearing(l1, actualH1, actualH2),1e-9)); + EXPECT(assert_equal(Unit3(1, 0, 0), x1.bearing(l1, actualH1, actualH2), 1e-9)); // Check numerical derivatives expectedH1 = numericalDerivative21(bearing_proxy, x1, l1); expectedH2 = numericalDerivative22(bearing_proxy, x1, l1); - EXPECT(assert_equal(expectedH1,actualH1)); - EXPECT(assert_equal(expectedH2,actualH2)); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + +TEST(Pose3, Bearing2) { + Matrix expectedH1, actualH1, expectedH2, actualH2; + EXPECT(assert_equal(Unit3(0,0.6,-0.8), x2.bearing(l4, actualH1, actualH2), 1e-9)); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(bearing_proxy, x2, l4); + expectedH2 = numericalDerivative22(bearing_proxy, x2, l4); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); } /* ************************************************************************* */ @@ -671,21 +730,21 @@ TEST( Pose3, adjointMap) { } /* ************************************************************************* */ -TEST(Pose3, align_1) { +TEST(Pose3, Align1) { Pose3 expected(Rot3(), Point3(10,10,0)); vector correspondences; - Point3Pair pq1(make_pair(Point3(0,0,0), Point3(10,10,0))); - Point3Pair pq2(make_pair(Point3(20,10,0), Point3(30,20,0))); - Point3Pair pq3(make_pair(Point3(10,20,0), Point3(20,30,0))); - correspondences += pq1, pq2, pq3; + Point3Pair ab1(make_pair(Point3(10,10,0), Point3(0,0,0))); + Point3Pair ab2(make_pair(Point3(30,20,0), Point3(20,10,0))); + Point3Pair ab3(make_pair(Point3(20,30,0), Point3(10,20,0))); + correspondences += ab1, ab2, ab3; - boost::optional actual = align(correspondences); + boost::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual)); } /* ************************************************************************* */ -TEST(Pose3, align_2) { +TEST(Pose3, Align2) { Point3 t(20,10,5); Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1); Pose3 expected(R, t); @@ -695,12 +754,12 @@ TEST(Pose3, align_2) { Point3 q1 = expected.transform_from(p1), q2 = expected.transform_from(p2), q3 = expected.transform_from(p3); - Point3Pair pq1(make_pair(p1, q1)); - Point3Pair pq2(make_pair(p2, q2)); - Point3Pair pq3(make_pair(p3, q3)); - correspondences += pq1, pq2, pq3; + Point3Pair ab1(make_pair(q1, p1)); + Point3Pair ab2(make_pair(q2, p2)); + Point3Pair ab3(make_pair(q3, p3)); + correspondences += ab1, ab2, ab3; - boost::optional actual = align(correspondences); + boost::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual, 1e-5)); } @@ -843,6 +902,22 @@ TEST(Pose3 , ChartDerivatives) { } } +/* ************************************************************************* */ +TEST(Pose3, interpolate) { + EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0))); + EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0))); +} + +/* ************************************************************************* */ +TEST(Pose3, Create) { + Matrix63 actualH1, actualH2; + Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2); + EXPECT(assert_equal(T, actual)); + boost::function create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none); + EXPECT(assert_equal(numericalDerivative21(create, R, P2), actualH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(create, R, P2), actualH2, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index f96198b94..5364d72f6 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -117,11 +117,13 @@ TEST( Rot3, Rodrigues2) { Vector axis = Vector3(0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; - Rot3 actual = Rot3::AxisAngle(axis, angle); Rot3 expected(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388); + Rot3 actual = Rot3::AxisAngle(axis, angle); CHECK(assert_equal(expected,actual,1e-5)); + Rot3 actual2 = Rot3::Rodrigues(angle*axis); + CHECK(assert_equal(expected,actual2,1e-5)); } /* ************************************************************************* */ @@ -242,6 +244,7 @@ TEST(Rot3, retract_localCoordinates2) EXPECT(assert_equal(t2, t1.retract(d12))); Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); + EXPECT(assert_equal(d12, -d21)); } /* ************************************************************************* */ TEST(Rot3, manifold_expmap) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 07789acaa..396c8b0f3 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -303,7 +303,7 @@ TEST(Unit3, localCoordinates) { } //******************************************************************************* -// Wrapper to make basis return a vector6 so we can test numerical derivatives. +// Wrapper to make basis return a Vector6 so we can test numerical derivatives. Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) { Matrix32 B = p.basis(H); Vector6 B_vec; diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c92aa8237..7ec9edbb3 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -56,11 +56,10 @@ Vector4 triangulateHomogeneousDLT( Point3 triangulateDLT(const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - rank_tol); + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates - return Point3(sub((v / v(3)), 0, 3)); + return Point3(v.head<3>() / v[3]); } /// @@ -91,5 +90,4 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } -} // \namespace gtsam - +} // \namespace gtsam diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 98a3545f6..43bcffb09 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -48,16 +48,20 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateSequential(ordering, function, VariableIndex(asDerived()), orderingType); + VariableIndex computedVariableIndex(asDerived()); + return eliminateSequential(ordering, function, computedVariableIndex, orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == Ordering::METIS) - return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) { + Ordering computedOrdering = Ordering::Metis(asDerived()); + return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + } else { + Ordering computedOrdering = Ordering::Colamd(*variableIndex); + return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + } } } @@ -86,16 +90,20 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); + VariableIndex computedVariableIndex(asDerived()); + return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == Ordering::METIS) - return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) { + Ordering computedOrdering = Ordering::Metis(asDerived()); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + } else { + Ordering computedOrdering = Ordering::Colamd(*variableIndex); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + } } } @@ -112,7 +120,8 @@ namespace gtsam { return etree.eliminate(function); } else { // If no variable index is provided, compute one and call this function again - return eliminatePartialSequential(ordering, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return eliminatePartialSequential(ordering, function, computedVariableIndex); } } @@ -132,7 +141,8 @@ namespace gtsam { return eliminatePartialSequential(ordering, function, variableIndex); } else { // If no variable index is provided, compute one and call this function again - return eliminatePartialSequential(variables, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return eliminatePartialSequential(variables, function, computedVariableIndex); } } @@ -150,7 +160,8 @@ namespace gtsam { return junctionTree.eliminate(function); } else { // If no variable index is provided, compute one and call this function again - return eliminatePartialMultifrontal(ordering, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return eliminatePartialMultifrontal(ordering, function, computedVariableIndex); } } @@ -170,7 +181,8 @@ namespace gtsam { return eliminatePartialMultifrontal(ordering, function, variableIndex); } else { // If no variable index is provided, compute one and call this function again - return eliminatePartialMultifrontal(variables, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return eliminatePartialMultifrontal(variables, function, computedVariableIndex); } } @@ -287,7 +299,8 @@ namespace gtsam { } } else { // If no variable index is provided, compute one and call this function again - return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex); } } @@ -312,7 +325,8 @@ namespace gtsam { else { // If no variable index is provided, compute one and call this function again - return marginal(variables, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return marginal(variables, function, computedVariableIndex); } } diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index c629d336a..52ba23155 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -74,11 +74,26 @@ namespace gtsam { /* ************************************************************************* */ template KeySet FactorGraph::keys() const { - KeySet allKeys; - BOOST_FOREACH(const sharedFactor& factor, factors_) + KeySet keys; + BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + if(factor) + keys.insert(factor->begin(), factor->end()); + } + return keys; + } + + /* ************************************************************************* */ + template + KeyVector FactorGraph::keyVector() const { + KeyVector keys; + keys.reserve(2 * size()); // guess at size + BOOST_FOREACH (const sharedFactor& factor, factors_) if (factor) - allKeys.insert(factor->begin(), factor->end()); - return allKeys; + keys.insert(keys.end(), factor->begin(), factor->end()); + std::sort(keys.begin(), keys.end()); + auto last = std::unique(keys.begin(), keys.end()); + keys.erase(last, keys.end()); + return keys; } /* ************************************************************************* */ diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index e97860eaa..fcafc9b42 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -114,7 +114,7 @@ namespace gtsam { /// @} /// @name Advanced Constructors /// @{ - + // TODO: are these needed? ///** @@ -319,10 +319,10 @@ namespace gtsam { void replace(size_t index, sharedFactor factor) { at(index) = factor; } /** Erase factor and rearrange other factors to take up the empty space */ - void erase(iterator item) { factors_.erase(item); } + iterator erase(iterator item) { return factors_.erase(item); } /** Erase factors and rearrange other factors to take up the empty space */ - void erase(iterator first, iterator last) { factors_.erase(first, last); } + iterator erase(iterator first, iterator last) { return factors_.erase(first, last); } /// @} /// @name Advanced Interface @@ -331,9 +331,12 @@ namespace gtsam { /** return the number of non-null factors */ size_t nrFactors() const; - /** Potentially very slow function to return all keys involved */ + /** Potentially slow function to return all keys involved, sorted, as a set */ KeySet keys() const; + /** Potentially slow function to return all keys involved, sorted, as a vector */ + KeyVector keyVector() const; + /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */ inline bool exists(size_t idx) const { return idx < size() && at(idx); } diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index d2b342575..2c3eb84c6 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -11,7 +11,7 @@ /** * @file Key.h - * @brief + * @brief * @author Richard Roberts * @date Feb 20, 2012 */ @@ -78,14 +78,19 @@ GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "", // Define Key to be Testable by specializing gtsam::traits template struct traits; -template<> struct traits { - static void Print(const Key& key, const std::string& str = "") { - PrintKey(key, str); + +template <> +struct traits { + static void Print(const Key& val, const std::string& str = "") { + PrintKey(val, str); } - static bool Equals(const Key& key1, const Key& key2, double tol = 1e-8) { - return key1 == key2; + static bool Equals(const Key& val1, const Key& val2, double tol = 1e-8) { + return val1 == val2; } }; } // namespace gtsam + + + diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index b9e93ceb1..c82062246 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -37,7 +37,7 @@ LabeledSymbol::LabeledSymbol(const LabeledSymbol& key) : c_(key.c_), label_(key.label_), j_(key.j_) {} /* ************************************************************************* */ -LabeledSymbol::LabeledSymbol(unsigned char c, unsigned char label, size_t j) +LabeledSymbol::LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j) : c_(c), label_(label), j_(j) {} /* ************************************************************************* */ diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 452c98434..8c521b067 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -34,7 +34,7 @@ namespace gtsam { class GTSAM_EXPORT LabeledSymbol { protected: unsigned char c_, label_; - size_t j_; + std::uint64_t j_; public: /** Default constructor */ @@ -44,7 +44,7 @@ public: LabeledSymbol(const LabeledSymbol& key); /** Constructor */ - LabeledSymbol(unsigned char c, unsigned char label, size_t j); + LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j); /** Constructor that decodes an integer gtsam::Key */ LabeledSymbol(gtsam::Key key); diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 9f4a81d05..3e3b40f8f 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -23,7 +23,10 @@ #include #include + +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION #include +#endif using namespace std; @@ -198,6 +201,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, /* ************************************************************************* */ Ordering Ordering::Metis(const MetisIndex& met) { +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION gttic(Ordering_METIS); vector xadj = met.xadj(); @@ -227,6 +231,10 @@ Ordering Ordering::Metis(const MetisIndex& met) { result[j] = met.intToKey(perm[j]); } return result; +#else + throw runtime_error("GTSAM was built without support for Metis-based " + "nested dissection"); +#endif } /* ************************************************************************* */ diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index f8b37d429..ccabcb07e 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -33,9 +33,9 @@ static const size_t indexBits = keyBits - chrBits; static const Key chrMask = Key(UCHAR_MAX) << indexBits; // For some reason, std::numeric_limits::max() fails static const Key indexMask = ~chrMask; -Symbol::Symbol(Key key) { - c_ = (unsigned char) ((key & chrMask) >> indexBits); - j_ = key & indexMask; +Symbol::Symbol(Key key) : + c_((unsigned char) ((key & chrMask) >> indexBits)), + j_ (key & indexMask) { } Key Symbol::key() const { diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 68d927baf..102761273 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -21,6 +21,7 @@ #include #include #include +#include namespace gtsam { @@ -32,8 +33,8 @@ namespace gtsam { */ class GTSAM_EXPORT Symbol { protected: - unsigned char c_; - size_t j_; + const unsigned char c_; + const std::uint64_t j_; public: @@ -48,7 +49,7 @@ public: } /** Constructor */ - Symbol(unsigned char c, size_t j) : + Symbol(unsigned char c, std::uint64_t j) : c_(c), j_(j) { } @@ -73,7 +74,7 @@ public: } /** Retrieve key index */ - size_t index() const { + std::uint64_t index() const { return j_; } @@ -124,41 +125,41 @@ private: }; /** Create a symbol key from a character and index, i.e. x5. */ -inline Key symbol(unsigned char c, size_t j) { return (Key)Symbol(c,j); } +inline Key symbol(unsigned char c, std::uint64_t j) { return (Key)Symbol(c,j); } /** Return the character portion of a symbol key. */ inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); } /** Return the index portion of a symbol key. */ -inline size_t symbolIndex(Key key) { return Symbol(key).index(); } +inline std::uint64_t symbolIndex(Key key) { return Symbol(key).index(); } namespace symbol_shorthand { -inline Key A(size_t j) { return Symbol('a', j); } -inline Key B(size_t j) { return Symbol('b', j); } -inline Key C(size_t j) { return Symbol('c', j); } -inline Key D(size_t j) { return Symbol('d', j); } -inline Key E(size_t j) { return Symbol('e', j); } -inline Key F(size_t j) { return Symbol('f', j); } -inline Key G(size_t j) { return Symbol('g', j); } -inline Key H(size_t j) { return Symbol('h', j); } -inline Key I(size_t j) { return Symbol('i', j); } -inline Key J(size_t j) { return Symbol('j', j); } -inline Key K(size_t j) { return Symbol('k', j); } -inline Key L(size_t j) { return Symbol('l', j); } -inline Key M(size_t j) { return Symbol('m', j); } -inline Key N(size_t j) { return Symbol('n', j); } -inline Key O(size_t j) { return Symbol('o', j); } -inline Key P(size_t j) { return Symbol('p', j); } -inline Key Q(size_t j) { return Symbol('q', j); } -inline Key R(size_t j) { return Symbol('r', j); } -inline Key S(size_t j) { return Symbol('s', j); } -inline Key T(size_t j) { return Symbol('t', j); } -inline Key U(size_t j) { return Symbol('u', j); } -inline Key V(size_t j) { return Symbol('v', j); } -inline Key W(size_t j) { return Symbol('w', j); } -inline Key X(size_t j) { return Symbol('x', j); } -inline Key Y(size_t j) { return Symbol('y', j); } -inline Key Z(size_t j) { return Symbol('z', j); } +inline Key A(std::uint64_t j) { return Symbol('a', j); } +inline Key B(std::uint64_t j) { return Symbol('b', j); } +inline Key C(std::uint64_t j) { return Symbol('c', j); } +inline Key D(std::uint64_t j) { return Symbol('d', j); } +inline Key E(std::uint64_t j) { return Symbol('e', j); } +inline Key F(std::uint64_t j) { return Symbol('f', j); } +inline Key G(std::uint64_t j) { return Symbol('g', j); } +inline Key H(std::uint64_t j) { return Symbol('h', j); } +inline Key I(std::uint64_t j) { return Symbol('i', j); } +inline Key J(std::uint64_t j) { return Symbol('j', j); } +inline Key K(std::uint64_t j) { return Symbol('k', j); } +inline Key L(std::uint64_t j) { return Symbol('l', j); } +inline Key M(std::uint64_t j) { return Symbol('m', j); } +inline Key N(std::uint64_t j) { return Symbol('n', j); } +inline Key O(std::uint64_t j) { return Symbol('o', j); } +inline Key P(std::uint64_t j) { return Symbol('p', j); } +inline Key Q(std::uint64_t j) { return Symbol('q', j); } +inline Key R(std::uint64_t j) { return Symbol('r', j); } +inline Key S(std::uint64_t j) { return Symbol('s', j); } +inline Key T(std::uint64_t j) { return Symbol('t', j); } +inline Key U(std::uint64_t j) { return Symbol('u', j); } +inline Key V(std::uint64_t j) { return Symbol('v', j); } +inline Key W(std::uint64_t j) { return Symbol('w', j); } +inline Key X(std::uint64_t j) { return Symbol('x', j); } +inline Key Y(std::uint64_t j) { return Symbol('y', j); } +inline Key Z(std::uint64_t j) { return Symbol('z', j); } } /// traits diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 3b80f0d01..ad8d6720b 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -32,7 +32,7 @@ namespace gtsam { /** * The VariableIndex class computes and stores the block column structure of a - * factor graph. The factor graph stores a collection of factors, each of + * factor graph. The factor graph stores a collection of factors, each of * which involves a set of variables. In contrast, the VariableIndex is built * from a factor graph prior to elimination, and stores the list of factors * that involve each variable. This information is stored as a deque of @@ -45,7 +45,7 @@ public: typedef boost::shared_ptr shared_ptr; typedef FastVector Factors; typedef Factors::iterator Factor_iterator; - typedef Factors::const_iterator Factor_const_iterator; + typedef Factors::const_iterator Factor_const_iterator; protected: typedef FastMap KeyMap; @@ -81,7 +81,7 @@ public: * The number of variable entries. This is one greater than the variable * with the highest index. */ - Key size() const { return index_.size(); } + size_t size() const { return index_.size(); } /** The number of factors in the original factor graph */ size_t nFactors() const { return nFactors_; } diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index 434b705f2..1e3f4cdfc 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -61,7 +61,7 @@ namespace gtsam { { // Call eliminate on the node and add the result to the parent's gathered factors typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors); - if(!childFactor->empty()) + if(childFactor && !childFactor->empty()) myData.parentData->childFactors.push_back(childFactor); } }; diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index d789da9b0..c209d086b 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -71,7 +71,7 @@ TEST(Ordering, grouped_constrained_ordering) { SymbolicFactorGraph sfg = example::symbolicChain(); // constrained version - push one set to the end - FastMap constraints; + FastMap constraints; constraints[2] = 1; constraints[4] = 1; constraints[5] = 2; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 6db13adb6..948daf2ad 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -86,6 +86,8 @@ namespace gtsam { VectorValues GaussianBayesNet::backSubstitute(const VectorValues& rhs) const { VectorValues result; + // TODO this looks pretty sketchy. result is passed as the parents argument + // as it's filled up by solving the gaussian conditionals. BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { result.insert(cg->solveOtherRHS(result, rhs)); } @@ -138,17 +140,18 @@ namespace gtsam { // return grad; //} - /* ************************************************************************* */ - pair GaussianBayesNet::matrix() const - { + /* ************************************************************************* */ + pair GaussianBayesNet::matrix() const { GaussianFactorGraph factorGraph(*this); KeySet keys = factorGraph.keys(); // add frontal keys in order Ordering ordering; BOOST_FOREACH (const sharedConditional& cg, *this) - BOOST_FOREACH (Key key, cg->frontals()) { - ordering.push_back(key); - keys.erase(key); + if (cg) { + BOOST_FOREACH (Key key, cg->frontals()) { + ordering.push_back(key); + keys.erase(key); + } } // add remaining keys in case Bayes net is incomplete BOOST_FOREACH (Key key, keys) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 6bd853764..feb477a4e 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -71,7 +71,7 @@ namespace gtsam { model_->print(" Noise model: "); else cout << " No noise model" << endl; - } + } /* ************************************************************************* */ bool GaussianConditional::equals(const GaussianFactor& f, double tol) const @@ -120,24 +120,24 @@ namespace gtsam { VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables - Vector xS = x.vector(FastVector(beginParents(), endParents())); + const Vector xS = x.vector(FastVector(beginParents(), endParents())); // Update right-hand-side - xS = getb() - get_S() * xS; + const Vector rhs = get_d() - get_S() * xS; // Solve matrix - Vector soln = get_R().triangularView().solve(xS); + const Vector solution = get_R().triangularView().solve(rhs); // Check for indeterminant solution - if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); - - // TODO, do we not have to scale by sigmas here? Copy/paste bug + if (solution.hasNaN()) { + throw IndeterminantLinearSystemException(keys().front()); + } // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index a39b1d91e..19d6bd607 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -30,6 +30,8 @@ #include #include +#include + using namespace std; using namespace gtsam; @@ -68,27 +70,6 @@ namespace gtsam { return spec; } - /* ************************************************************************* */ - vector GaussianFactorGraph::getkeydim() const { - // First find dimensions of each variable - vector dims; - BOOST_FOREACH(const sharedFactor& factor, *this) { - for (GaussianFactor::const_iterator pos = factor->begin(); - pos != factor->end(); ++pos) { - if (dims.size() <= *pos) - dims.resize(*pos + 1, 0); - dims[*pos] = factor->getDim(pos); - } - } - // Find accumulated dimensions for variables - vector dims_accumulated; - dims_accumulated.resize(dims.size()+1,0); - dims_accumulated[0]=0; - for (size_t i=1; i optionalOrdering) const { // combine all factors and get upper-triangular part of Hessian - HessianFactor combined(*this, Scatter(*this, optionalOrdering)); + Scatter scatter(*this, optionalOrdering); + HessianFactor combined(*this, scatter); Matrix result = combined.info(); // Fill in lower-triangular part of Hessian result.triangularView() = result.transpose(); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 02bc95511..ae552adcd 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -17,7 +17,7 @@ * @author Alireza Fathi * @author Richard Roberts * @author Frank Dellaert - */ + */ #pragma once @@ -141,8 +141,6 @@ namespace gtsam { /* return a map of (Key, dimension) */ std::map getKeyDimMap() const; - std::vector getkeydim() const; - /** unnormalized error */ double error(const VectorValues& x) const { double total_error = 0.; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 2b275b60f..9277f3903 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -455,8 +455,8 @@ std::pair, // Build joint factor HessianFactor::shared_ptr jointFactor; try { - jointFactor = boost::make_shared(factors, - Scatter(factors, keys)); + Scatter scatter(factors, keys); + jointFactor = boost::make_shared(factors, scatter); } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateCholesky was called with a request to eliminate variables that are not\n" diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 442a5fc6b..92dcbfa07 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -111,7 +111,7 @@ public: * Handy data structure for iterative solvers * key to (index, dimension, colstart) */ -class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { +class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { public: diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 47b6ec90b..fc1a7c841 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -527,15 +527,16 @@ void JacobianFactor::updateHessian(const FastVector& infoKeys, // Loop over blocks of A, including RHS with j==n vector slots(n+1); for (DenseIndex j = 0; j <= n; ++j) { + Eigen::Block Ab_j = Ab_(j); const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); slots[j] = J; // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { const DenseIndex I = slots[i]; // because i, - boost::shared_ptr > JacobianFactor::eliminate( +std::pair JacobianFactor::eliminate( const Ordering& keys) { GaussianFactorGraph graph; graph.add(*this); @@ -710,8 +711,7 @@ void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) { } /* ************************************************************************* */ -std::pair, - boost::shared_ptr > EliminateQR( +std::pair EliminateQR( const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminateQR); // Combine and sort variable blocks in elimination order @@ -721,77 +721,84 @@ std::pair, } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateQR was called with a request to eliminate variables that are not\n" - "involved in the provided factors."); + "involved in the provided factors."); } // Do dense elimination + // The following QR variants eliminate to fully triangular or trapezoidal SharedDiagonal noiseModel; - if (jointFactor->model_) - jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix()); - else - inplace_QR(jointFactor->Ab_.matrix()); - - // Zero below the diagonal - jointFactor->Ab_.matrix().triangularView().setZero(); + VerticalBlockMatrix& Ab = jointFactor->Ab_; + if (jointFactor->model_) { + // The noiseModel QR can, in the case of constraints, yield a "staggered" QR where + // some rows have more leading zeros than in an upper triangular matrix. + // In either case, QR will put zeros below the "diagonal". + jointFactor->model_ = jointFactor->model_->QR(Ab.matrix()); + } else { + // The inplace variant will have no valid rows anymore below m==n + // and only entries above the diagonal are valid. + inplace_QR(Ab.matrix()); + // We zero below the diagonal to agree with the result from noieModel QR + Ab.matrix().triangularView().setZero(); + size_t m = Ab.rows(), n = Ab.cols() - 1; + size_t maxRank = min(m, n); + jointFactor->model_ = noiseModel::Unit::Create(maxRank); + } // Split elimination result into conditional and remaining factor - GaussianConditional::shared_ptr conditional = // + GaussianConditional::shared_ptr conditional = // jointFactor->splitConditional(keys.size()); return make_pair(conditional, jointFactor); } /* ************************************************************************* */ -GaussianConditional::shared_ptr JacobianFactor::splitConditional( - size_t nrFrontals) { +GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals) { gttic(JacobianFactor_splitConditional); - if (nrFrontals > size()) + if (!model_) { throw std::invalid_argument( - "Requesting to split more variables than exist using JacobianFactor::splitConditional"); + "JacobianFactor::splitConditional cannot be given a NULL noise model"); + } + if (nrFrontals > size()) { + throw std::invalid_argument( + "JacobianFactor::splitConditional was requested to split off more variables than exist."); + } + + // Convert nr of keys to number of scalar columns DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols(); + // Check that the noise model has at least this dimension + // If this is *not* the case, we do not have enough information on the frontal variables. + if ((DenseIndex)model_->dim() < frontalDim) + throw IndeterminantLinearSystemException(this->keys().front()); + // Restrict the matrix to be in the first nrFrontals variables and create the conditional - gttic(cond_Rd); const DenseIndex originalRowEnd = Ab_.rowEnd(); Ab_.rowEnd() = Ab_.rowStart() + frontalDim; SharedDiagonal conditionalNoiseModel; - if (model_) { - if ((DenseIndex) model_->dim() < frontalDim) - throw IndeterminantLinearSystemException(this->keys().front()); - conditionalNoiseModel = noiseModel::Diagonal::Sigmas( - model_->sigmas().segment(Ab_.rowStart(), Ab_.rows())); - } - GaussianConditional::shared_ptr conditional = boost::make_shared< - GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); - const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd) - - Ab_.rowStart() - frontalDim; - const DenseIndex remainingRows = - model_ ? std::min(model_->sigmas().size() - frontalDim, - maxRemainingRows) : - maxRemainingRows; + conditionalNoiseModel = + noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows())); + GaussianConditional::shared_ptr conditional = + boost::make_shared(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); + + const DenseIndex maxRemainingRows = + std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim; + const DenseIndex remainingRows = std::min(model_->sigmas().size() - frontalDim, maxRemainingRows); Ab_.rowStart() += frontalDim; Ab_.rowEnd() = Ab_.rowStart() + remainingRows; Ab_.firstBlock() += nrFrontals; - gttoc(cond_Rd); // Take lower-right block of Ab to get the new factor - gttic(remaining_factor); keys_.erase(begin(), begin() + nrFrontals); // Set sigmas with the right model - if (model_) { - if (model_->isConstrained()) - model_ = noiseModel::Constrained::MixedSigmas( - model_->sigmas().tail(remainingRows)); - else - model_ = noiseModel::Diagonal::Sigmas( - model_->sigmas().tail(remainingRows)); - assert(model_->dim() == (size_t)Ab_.rows()); - } - gttoc(remaining_factor); + if (model_->isConstrained()) + model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(remainingRows)); + else + model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(remainingRows)); + assert(model_->dim() == (size_t)Ab_.rows()); return conditional; } -} +} // namespace gtsam diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index ea9958474..3795f096e 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -314,7 +314,7 @@ namespace gtsam { JacobianFactor whiten() const; /** Eliminate the requested variables. */ - std::pair, boost::shared_ptr > + std::pair, shared_ptr> eliminate(const Ordering& keys); /** set noiseModel correctly */ @@ -331,13 +331,15 @@ namespace gtsam { * @return The conditional and remaining factor * * \addtogroup LinearSolving */ - friend GTSAM_EXPORT std::pair, boost::shared_ptr > + friend GTSAM_EXPORT std::pair, shared_ptr> EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); /** * splits a pre-factorized factor into a conditional, and changes the current * factor to be the remaining component. Performs same operation as eliminate(), * but without running QR. + * NOTE: looks at dimension of noise model to determine how many rows to keep. + * @param nrFrontals number of keys to eliminate */ boost::shared_ptr splitConditional(size_t nrFrontals); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index dc12216e3..15b2dcf81 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -58,7 +59,7 @@ boost::optional checkIfDiagonal(const Matrix M) { for (i = 0; i < m; i++) if (!full) for (j = i + 1; j < n; j++) - if (fabs(M(i, j)) > 1e-9) { + if (std::abs(M(i, j)) > 1e-9) { full = true; break; } @@ -87,7 +88,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { if (diagonal) return Diagonal::Sigmas(diagonal->array().inverse(), true); } - // NOTE(frank): only reaches here if !smart && !diagonal + // NOTE(frank): only reaches here if !(smart && diagonal) return shared_ptr(new Gaussian(R.rows(), R)); } @@ -190,8 +191,8 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { // get size(A) and maxRank // TODO: really no rank problems ? - // size_t m = Ab.rows(), n = Ab.cols()-1; - // size_t maxRank = min(m,n); + size_t m = Ab.rows(), n = Ab.cols()-1; + size_t maxRank = min(m,n); // pre-whiten everything (cheaply if possible) WhitenInPlace(Ab); @@ -200,12 +201,13 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { // Eigen QR - much faster than older householder approach inplace_QR(Ab); + Ab.triangularView().setZero(); // hand-coded householder implementation // TODO: necessary to isolate last column? // householder(Ab, maxRank); - return SharedDiagonal(); + return noiseModel::Unit::Create(maxRank); } void Gaussian::WhitenSystem(vector& A, Vector& b) const { @@ -414,81 +416,141 @@ Constrained::shared_ptr Constrained::unit() const { // Special version of QR for Constrained calls slower but smarter code // that deals with possibly zero sigmas // It is Gram-Schmidt orthogonalization rather than Householder -// Previously Diagonal::QR + +// Check whether column a triggers a constraint and corresponding variable is deterministic +// Return constraint_row with maximum element in case variable plays in multiple constraints +template +boost::optional check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) { + boost::optional constraint_row; + // not zero, so roundoff errors will not be counted + // TODO(frank): that's a fairly crude way of dealing with roundoff errors :-( + double max_element = 1e-9; + for (size_t i = 0; i < m; i++) { + if (!std::isinf(invsigmas[i])) + continue; + double abs_ai = std::abs(a(i,0)); + if (abs_ai > max_element) { + max_element = abs_ai; + constraint_row.reset(i); + } + } + return constraint_row; +} + SharedDiagonal Constrained::QR(Matrix& Ab) const { - bool verbose = false; - if (verbose) cout << "\nStarting Constrained::QR" << endl; + static const double kInfinity = std::numeric_limits::infinity(); // get size(A) and maxRank - size_t m = Ab.rows(), n = Ab.cols()-1; - size_t maxRank = min(m,n); + size_t m = Ab.rows(); + const size_t n = Ab.cols() - 1; + const size_t maxRank = min(m, n); // create storage for [R d] - typedef boost::tuple Triple; + typedef boost::tuple Triple; list Rd; - Vector pseudo(m); // allocate storage for pseudo-inverse + Matrix rd(1, n + 1); // and for row of R Vector invsigmas = sigmas_.array().inverse(); - Vector weights = invsigmas.array().square(); // calculate weights once + 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 // scalar variable x as d-rS, with S the separator (remaining columns). // Then update A and b by substituting x with d-rS, zero-ing out x's column. - for (size_t j=0; j a = Ab.block(0, j, m, 1); - // Calculate weighted pseudo-inverse and corresponding precision - gttic(constrained_QR_weightedPseudoinverse); - double precision = weightedPseudoinverse(a, weights, pseudo); - gttoc(constrained_QR_weightedPseudoinverse); + // Check whether we need to handle as a constraint + boost::optional constraint_row = check_if_constraint(a, invsigmas, m); - // If precision is zero, no information on this column - // This is actually not limited to constraints, could happen in Gaussian::QR - // In that case, we're probably hosed. TODO: make sure Householder is rank-revealing - if (precision < 1e-8) continue; + if (constraint_row) { + // Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j) - gttic(constrained_QR_create_rd); - // create solution [r d], rhs is automatically r(n) - Vector rd(n+1); // uninitialized ! - rd(j)=1.0; // put 1 on diagonal - for (size_t j2=j+1; j2=maxRank) break; + // exit after rank exhausted + if (Rd.size() >= maxRank) + break; - // update Ab, expensive, using outer product - gttic(constrained_QR_update_Ab); - Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); - gttoc(constrained_QR_update_Ab); + // The constraint row will be zeroed out, so we can save work by swapping in the + // last valid row and decreasing m. This will save work on subsequent down-dates, too. + m -= 1; + if (*constraint_row != m) { + Ab.row(*constraint_row) = Ab.row(m); + weights(*constraint_row) = weights(m); + invsigmas(*constraint_row) = invsigmas(m); + } + + // get a reduced a-column which is now shorter + Eigen::Block a_reduced = Ab.block(0, j, m, 1); + a_reduced *= (1.0/rd(0, j)); // NOTE(frank): this is the 1/a[i] = 1/rd(0,j) factor we need! + + // Rank-1 down-date of Ab, expensive, using outer product + Ab.block(0, j + 1, m, n - j).noalias() -= a_reduced * rd.middleCols(j + 1, n - j); + } else { + // Treat in normal Gram-Schmidt way + // Calculate weighted pseudo-inverse and corresponding precision + + // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma) + // For diagonal Sigma, inv(Sigma) = diag(precisions) + double precision = 0; + Vector pseudo(m); // allocate storage for pseudo-inverse + for (size_t i = 0; i < m; i++) { + double ai = a(i, 0); + if (std::abs(ai) > 1e-9) { // also catches remaining sigma==0 rows + pseudo[i] = weights[i] * ai; + precision += pseudo[i] * ai; + } else + pseudo[i] = 0; + } + + if (precision > 1e-8) { + pseudo /= precision; + + // create solution [r d], rhs is automatically r(n) + rd(0, j) = 1.0; // put 1 on diagonal + rd.block(0, j + 1, 1, n - j) = pseudo.transpose() * Ab.block(0, j + 1, m, n - j); + + // construct solution (r, d, sigma) + Rd.push_back(boost::make_tuple(j, rd, precision)); + } else { + // If precision is zero, no information on this column + // This is actually not limited to constraints, could happen in Gaussian::QR + // In that case, we're probably hosed. TODO: make sure Householder is rank-revealing + continue; // but even if not, no need to update if a==zeros + } + + // exit after rank exhausted + if (Rd.size() >= maxRank) + break; + + // Rank-1 down-date of Ab, expensive, using outer product + Ab.block(0, j + 1, m, n - j).noalias() -= a * rd.middleCols(j + 1, n - j); + } } // Create storage for precisions Vector precisions(Rd.size()); - gttic(constrained_QR_write_back_into_Ab); // Write back result in Ab, imperative as we are - // TODO: test that is correct if a column was skipped !!!! - size_t i = 0; // start with first row + size_t i = 0; // start with first row bool mixed = false; - BOOST_FOREACH(const Triple& t, Rd) { - const size_t& j = t.get<0>(); - const Vector& rd = t.get<1>(); - precisions(i) = t.get<2>(); - if (constrained(i)) mixed = true; - for (size_t j2=0; j2(); + const Matrix& rd = t.get<1>(); + precisions(i) = t.get<2>(); + if (std::isinf(precisions(i))) + mixed = true; + Ab.block(i, j, 1, n + 1 - j) = rd.block(0, j, 1, n + 1 - j); + i += 1; } - gttoc(constrained_QR_write_back_into_Ab); // Must include mu, as the defaults might be higher, resulting in non-convergence return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions); @@ -498,13 +560,13 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { // Isotropic /* ************************************************************************* */ Isotropic::shared_ptr Isotropic::Sigma(size_t dim, double sigma, bool smart) { - if (smart && fabs(sigma-1.0)<1e-9) return Unit::Create(dim); + if (smart && std::abs(sigma-1.0)<1e-9) return Unit::Create(dim); return shared_ptr(new Isotropic(dim, sigma)); } /* ************************************************************************* */ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart) { - if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim); + if (smart && std::abs(variance-1.0)<1e-9) return Unit::Create(dim); return shared_ptr(new Isotropic(dim, sqrt(variance))); } @@ -561,21 +623,19 @@ void Unit::print(const std::string& name) const { namespace mEstimator { -/** produce a weight vector according to an error vector and the implemented - * robust function */ -Vector Base::weight(const Vector &error) const { +Vector Base::weight(const Vector& error) const { const size_t n = error.rows(); Vector w(n); - for ( size_t i = 0 ; i < n ; ++i ) + for (size_t i = 0; i < n; ++i) w(i) = weight(error(i)); return w; } -/** The following three functions reweight block matrices and a vector - * according to their weight implementation */ +// The following three functions re-weight block matrices and a vector +// according to their weight implementation void Base::reweight(Vector& error) const { - if(reweight_ == Block) { + if (reweight_ == Block) { const double w = sqrtWeight(error.norm()); error *= w; } else { @@ -583,7 +643,7 @@ void Base::reweight(Vector& error) const { } } -/** Reweight n block matrices with one error vector */ +// Reweight n block matrices with one error vector void Base::reweight(vector &A, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); @@ -601,7 +661,7 @@ void Base::reweight(vector &A, Vector &error) const { } } -/** Reweight one block matrix with one error vector */ +// Reweight one block matrix with one error vector void Base::reweight(Matrix &A, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); @@ -615,7 +675,7 @@ void Base::reweight(Matrix &A, Vector &error) const { } } -/** Reweight two block matrix with one error vector */ +// Reweight two block matrix with one error vector void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); @@ -631,7 +691,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { } } -/** Reweight three block matrix with one error vector */ +// Reweight three block matrix with one error vector void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); @@ -659,11 +719,9 @@ void Null::print(const std::string &s="") const Null::shared_ptr Null::Create() { return shared_ptr(new Null()); } -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; - c_ = 1.0; +Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { + if (c_ <= 0) { + throw runtime_error("mEstimator Fair takes only positive double in constructor."); } } @@ -671,16 +729,13 @@ Fair::Fair(double c, const ReweightScheme reweight) // Fair /* ************************************************************************* */ -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, double tol) const { const Fair* p = dynamic_cast (&expected); if (p == NULL) return false; - return fabs(c_ - p->c_ ) < tol; + return std::abs(c_ - p->c_ ) < tol; } Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) @@ -690,18 +745,12 @@ Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) // Huber /* ************************************************************************* */ -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; - k_ = 1.0; +Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { + if (k_ <= 0) { + throw runtime_error("mEstimator Huber takes only positive double in constructor."); } } -double Huber::weight(double error) const { - return (fabs(error) > k_) ? k_ / fabs(error) : 1.0; -} - void Huber::print(const std::string &s="") const { cout << s << "huber (" << k_ << ")" << endl; } @@ -709,7 +758,7 @@ void Huber::print(const std::string &s="") 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; + return std::abs(k_ - p->k_) < tol; } Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { @@ -720,18 +769,12 @@ Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { // Cauchy /* ************************************************************************* */ -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; - k_ = 1.0; +Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) { + if (k <= 0) { + throw runtime_error("mEstimator Cauchy takes only positive double in constructor."); } } -double Cauchy::weight(double error) const { - return k_*k_ / (k_*k_ + error*error); -} - void Cauchy::print(const std::string &s="") const { cout << s << "cauchy (" << k_ << ")" << endl; } @@ -739,7 +782,7 @@ void Cauchy::print(const std::string &s="") 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; + return std::abs(ksquared_ - p->ksquared_) < tol; } Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { @@ -749,18 +792,7 @@ Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { /* ************************************************************************* */ // Tukey /* ************************************************************************* */ -Tukey::Tukey(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { -} - -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); - return one_xc22; - } - return 0.0; -} +Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} void Tukey::print(const std::string &s="") const { std::cout << s << ": Tukey (" << c_ << ")" << std::endl; @@ -769,7 +801,7 @@ void Tukey::print(const std::string &s="") 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; + return std::abs(c_ - p->c_) < tol; } Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { @@ -779,14 +811,7 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { /* ************************************************************************* */ // Welsh /* ************************************************************************* */ -Welsh::Welsh(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { -} - -double Welsh::weight(double error) const { - double xc2 = (error/c_)*(error/c_); - return std::exp(-xc2); -} +Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} void Welsh::print(const std::string &s="") const { std::cout << s << ": Welsh (" << c_ << ")" << std::endl; @@ -795,7 +820,7 @@ void Welsh::print(const std::string &s="") 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; + return std::abs(c_ - p->c_) < tol; } Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index db01152f6..c76db1b55 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -232,9 +232,11 @@ namespace gtsam { /** * Apply appropriately weighted QR factorization to the system [A b] * Q' * [A b] = [R d] - * Dimensions: (r*m) * m*(n+1) = r*(n+1) + * Dimensions: (r*m) * m*(n+1) = r*(n+1), where r = min(m,n). + * This routine performs an in-place factorization on Ab. + * Below-diagonal elements are set to zero by this routine. * @param Ab is the m*(n+1) augmented system matrix [A b] - * @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!! + * @return Empty SharedDiagonal() noise model: R,d are whitened */ virtual boost::shared_ptr QR(Matrix& Ab) const; @@ -482,6 +484,12 @@ namespace gtsam { /** * Apply QR factorization to the system [A b], taking into account constraints + * Q' * [A b] = [R d] + * Dimensions: (r*m) * m*(n+1) = r*(n+1), where r = min(m,n). + * This routine performs an in-place factorization on Ab. + * Below-diagonal elements are set to zero by this routine. + * @param Ab is the m*(n+1) augmented system matrix [A b] + * @return diagonal noise model can be all zeros, mixed, or not-constrained */ virtual Diagonal::shared_ptr QR(Matrix& Ab) const; @@ -618,8 +626,24 @@ namespace gtsam { } }; - // TODO: should not really exist - /// The mEstimator namespace contains all robust error functions (not models) + /** + * The mEstimator name space contains all robust error functions. + * It mirrors the exposition at + * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. + * + * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: + * + * Name Symbol Least-Squares L1-norm Huber + * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if x shared_ptr; Fair(double c = 1.3998, const ReweightScheme reweight = Block); - virtual ~Fair() {} - 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; + double weight(double error) const { + return 1.0 / (1.0 + fabs(error) / c_); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; - protected: - double c_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -721,19 +746,20 @@ namespace gtsam { /// Huber implements the "Huber" robust error model (Zhang97ivc) class GTSAM_EXPORT Huber : public Base { + protected: + double k_; + public: typedef boost::shared_ptr shared_ptr; - virtual ~Huber() {} 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; + double weight(double error) const { + return (error < k_) ? (1.0) : (k_ / fabs(error)); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; - protected: - double k_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -750,19 +776,20 @@ namespace gtsam { /// oberlaender@fzi.de /// Thanks Jan! class GTSAM_EXPORT Cauchy : public Base { + protected: + double k_, ksquared_; + public: typedef boost::shared_ptr shared_ptr; - virtual ~Cauchy() {} 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; + double weight(double error) const { + return ksquared_ / (ksquared_ + error*error); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; - protected: - double k_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -775,19 +802,24 @@ namespace gtsam { /// Tukey implements the "Tukey" robust error model (Zhang97ivc) class GTSAM_EXPORT Tukey : public Base { + protected: + double c_, csquared_; + public: typedef boost::shared_ptr shared_ptr; Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - virtual ~Tukey() {} - 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; + double weight(double error) const { + if (std::fabs(error) <= c_) { + double xc2 = error*error/csquared_; + return (1.0-xc2)*(1.0-xc2); + } + return 0.0; + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; - protected: - double c_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -800,19 +832,21 @@ namespace gtsam { /// Welsh implements the "Welsh" robust error model (Zhang97ivc) class GTSAM_EXPORT Welsh : public Base { + protected: + double c_, csquared_; + public: typedef boost::shared_ptr shared_ptr; Welsh(double c = 2.9846, const ReweightScheme reweight = Block); - virtual ~Welsh() {} - 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; + double weight(double error) const { + double xc2 = (error*error)/csquared_; + return std::exp(-xc2); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; - protected: - double c_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -884,7 +918,23 @@ namespace gtsam { } ///\namespace mEstimator - /// Base class for robust error models + /** + * Base class for robust error models + * The robust M-estimators above simply tell us how to re-weight the residual, and are + * isotropic kernels, in that they do not allow for correlated noise. They also have no way + * to scale the residual values, e.g., dividing by a single standard deviation. + * Hence, the actual robust noise model below does this scaling/whitening in sequence, by + * passing both a standard noise model and a robust estimator. + * + * Taking as an example noise = Isotropic::Create(d, sigma), we first divide the residuals + * uw = |Ax-b| by sigma by "whitening" the system (A,b), obtaining r = |Ax-b|/sigma, and + * then we pass the now whitened residual 'r' through the robust M-estimator. + * This is currently done by multiplying with sqrt(w), because the residuals will be squared + * again in error, yielding 0.5 \sum w(r)*r^2. + * + * In other words, while sigma is expressed in the native residual units, a parameter like + * k in the Huber norm is expressed in whitened units, i.e., "nr of sigmas". + */ class GTSAM_EXPORT Robust : public Base { public: typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index ee2447d2f..ebe5bcc9b 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -508,9 +508,8 @@ Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { Errors::iterator ei = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { + for (size_t i = 0; i < y.size(); ++i, ++ei) *ei = y[i]; - } // Add A2 contribution VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y @@ -523,9 +522,9 @@ VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { Errors::const_iterator it = e.begin(); VectorValues y = zero(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) - y[i] = *it ; - transposeMultiplyAdd2(1.0,it,e.end(),y); + for (size_t i = 0; i < y.size(); ++i, ++it) + y[i] = *it; + transposeMultiplyAdd2(1.0, it, e.end(), y); return y; } @@ -535,7 +534,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd (double alpha, const Errors& e, VectorValues& y) const { Errors::const_iterator it = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { + for (size_t i = 0; i < y.size(); ++i, ++it) { const Vector& ei = *it; axpy(alpha, ei, y[i]); } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index d04d9faac..0c752728a 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -135,7 +135,7 @@ namespace gtsam { /// @{ /** Number of variables stored. */ - Key size() const { return values_.size(); } + size_t size() const { return values_.size(); } /** Return the dimension of variable \c j. */ size_t dim(Key j) const { return at(j).rows(); } diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 10fb34988..61083a926 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -314,22 +314,26 @@ TEST(HessianFactor, CombineAndEliminate1) { Ordering ordering = list_of(1); GaussianConditional::shared_ptr expectedConditional; JacobianFactor::shared_ptr expectedFactor; - boost::tie(expectedConditional, expectedFactor) = // - jacobian.eliminate(ordering); + boost::tie(expectedConditional, expectedFactor) = jacobian.eliminate(ordering); + CHECK(expectedFactor); // Eliminate GaussianConditional::shared_ptr actualConditional; HessianFactor::shared_ptr actualHessian; boost::tie(actualConditional, actualHessian) = // EliminateCholesky(gfg, ordering); + actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); +#ifdef TEST_ERROR_EQUIVALENCE + // these tests disabled because QR cuts off the all zeros + error row EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); EXPECT( assert_equal(expectedFactor->augmentedInformation(), actualHessian->augmentedInformation(), 1e-9)); EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); -} +#endif + } /* ************************************************************************* */ TEST(HessianFactor, CombineAndEliminate2) { @@ -381,8 +385,11 @@ TEST(HessianFactor, CombineAndEliminate2) { HessianFactor::shared_ptr actualHessian; boost::tie(actualConditional, actualHessian) = // EliminateCholesky(gfg, ordering); + actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); +#ifdef TEST_ERROR_EQUIVALENCE + // these tests disabled because QR cuts off the all zeros + error row VectorValues v; v.insert(1, Vector3(1, 2, 3)); EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); @@ -390,6 +397,7 @@ TEST(HessianFactor, CombineAndEliminate2) { assert_equal(expectedFactor->augmentedInformation(), actualHessian->augmentedInformation(), 1e-9)); EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); +#endif } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 17ceaf5f0..d57f896ef 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -161,37 +162,46 @@ TEST(JabobianFactor, Hessian_conversion) { EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); } +/* ************************************************************************* */ +namespace simple_graph { + +Key keyX(10), keyY(8), keyZ(12); + +double sigma1 = 0.1; +Matrix A11 = Matrix::Identity(2, 2); +Vector2 b1(2, -1); +JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1)); + +double sigma2 = 0.5; +Matrix A21 = -2 * Matrix::Identity(2, 2); +Matrix A22 = 3 * Matrix::Identity(2, 2); +Vector2 b2(4, -5); +JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2)); + +double sigma3 = 1.0; +Matrix A32 = -4 * Matrix::Identity(2, 2); +Matrix A33 = 5 * Matrix::Identity(2, 2); +Vector2 b3(3, -6); +JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3)); + +GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3)); +Ordering ordering(list_of(keyX)(keyY)(keyZ)); +} + /* ************************************************************************* */ TEST( JacobianFactor, construct_from_graph) { - GaussianFactorGraph factors; - - double sigma1 = 0.1; - Matrix A11 = Matrix::Identity(2,2); - Vector b1(2); b1 << 2, -1; - factors.add(JacobianFactor(10, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))); - - double sigma2 = 0.5; - Matrix A21 = -2 * Matrix::Identity(2,2); - Matrix A22 = 3 * Matrix::Identity(2,2); - Vector b2(2); b2 << 4, -5; - factors.add(JacobianFactor(10, A21, 8, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))); - - double sigma3 = 1.0; - Matrix A32 = -4 * Matrix::Identity(2,2); - Matrix A33 = 5 * Matrix::Identity(2,2); - Vector b3(2); b3 << 3, -6; - factors.add(JacobianFactor(8, A32, 12, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))); + using namespace simple_graph; Matrix A1(6,2); A1 << A11, A21, Matrix::Zero(2,2); Matrix A2(6,2); A2 << Matrix::Zero(2,2), A22, A32; Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33; Vector b(6); b << b1, b2, b3; Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3; - JacobianFactor expected(10, A1, 8, A2, 12, A3, b, noiseModel::Diagonal::Sigmas(sigmas)); + JacobianFactor expected(keyX, A1, keyY, A2, keyZ, A3, b, noiseModel::Diagonal::Sigmas(sigmas)); // The ordering here specifies the order in which the variables will appear in the combined factor - JacobianFactor actual(factors, Ordering(list_of(10)(8)(12))); + JacobianFactor actual(factors, ordering); EXPECT(assert_equal(expected, actual)); } @@ -466,7 +476,7 @@ TEST(JacobianFactor, eliminate2 ) +0.00,-0.20,+0.00,-0.80 ).finished()/oldSigma; Vector d = Vector2(0.2,-0.14)/oldSigma; - GaussianConditional expectedCG(2, d, R11, 11, S12); + GaussianConditional expectedCG(2, d, R11, 11, S12, noiseModel::Unit::Create(2)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); @@ -478,7 +488,7 @@ TEST(JacobianFactor, eliminate2 ) 0.00, 1.00, +0.00, -1.00 ).finished()/sigma; Vector b1 = Vector2(0.0, 0.894427); - JacobianFactor expectedLF(11, Bl1x1, b1); + JacobianFactor expectedLF(11, Bl1x1, b1, noiseModel::Unit::Create(2)); EXPECT(assert_equal(expectedLF, *actual.second,1e-3)); } @@ -516,7 +526,7 @@ TEST(JacobianFactor, EliminateQR) EXPECT(assert_equal(2.0 * Ab, actualDense)); // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows) - Matrix R = 2.0 * (Matrix(11, 11) << + Matrix R = 2.0 * (Matrix(10, 11) << -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, 0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625, 0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250, @@ -526,24 +536,24 @@ TEST(JacobianFactor, EliminateQR) 0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081, 0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685, 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, - 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090, - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635).finished(); + 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090).finished(); - GaussianConditional expectedFragment( - list_of(3)(5)(7)(9)(11), 3, VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R)); + GaussianConditional expectedFragment(list_of(3)(5)(7)(9)(11), 3, + VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R.topRows(6)), + noiseModel::Unit::Create(6)); // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, list_of(3)(5)(7)); const JacobianFactor &actualJF = dynamic_cast(*actual.second); EXPECT(assert_equal(expectedFragment, *actual.first, 0.001)); - EXPECT(assert_equal(size_t(2), actualJF.keys().size())); + EXPECT_LONGS_EQUAL(size_t(2), actualJF.keys().size()); EXPECT(assert_equal(Key(9), actualJF.keys()[0])); EXPECT(assert_equal(Key(11), actualJF.keys()[1])); - EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001)); - EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); - EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001)); - EXPECT(!actualJF.get_model()); + EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); + EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001)); + EXPECT(assert_equal(*noiseModel::Diagonal::Sigmas(Vector4::Ones()), *actualJF.get_model(), 0.001)); } /* ************************************************************************* */ @@ -557,8 +567,8 @@ TEST ( JacobianFactor, constraint_eliminate1 ) pair actual = lc.eliminate(list_of(1)); - // verify linear factor - EXPECT(actual.second->size() == 0); + // verify linear factor is a null ptr + EXPECT(actual.second->empty()); // verify conditional Gaussian Vector sigmas = Vector2(0.0, 0.0); @@ -574,14 +584,10 @@ TEST ( JacobianFactor, constraint_eliminate2 ) Vector b(2); b(0)=3.0; b(1)=4.0; // A1 - invertible - Matrix A1(2,2); - A1(0,0) = 1.0 ; A1(0,1) = 2.0; - A1(1,0) = 2.0 ; A1(1,1) = 1.0; + Matrix2 A1; A1 << 2,4, 2,1; // A2 - not invertible - Matrix A2(2,2); - A2(0,0) = 1.0 ; A2(0,1) = 2.0; - A2(1,0) = 2.0 ; A2(1,1) = 4.0; + Matrix2 A2; A2 << 2,4, 2,4; JacobianFactor lc(1, A1, 2, A2, b, noiseModel::Constrained::All(2)); @@ -598,18 +604,38 @@ TEST ( JacobianFactor, constraint_eliminate2 ) EXPECT(assert_equal(expectedLF, *actual.second)); // verify CG - Matrix R = (Matrix(2, 2) << - 1.0, 2.0, - 0.0, 1.0).finished(); - Matrix S = (Matrix(2, 2) << - 1.0, 2.0, - 0.0, 0.0).finished(); + Matrix2 R; R << 1,2, 0,1; + Matrix2 S; S << 1,2, 0,0; Vector d = Vector2(3.0, 0.6666); Vector sigmas = Vector2(0.0, 0.0); GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); } +/* ************************************************************************* */ +TEST(JacobianFactor, OverdeterminedEliminate) { + Matrix Ab(9, 4); + Ab << 0, 1, 0, 0, // + 0, 0, 1, 0, // + Matrix74::Ones(); + + // Call Gaussian version + Vector9 sigmas = Vector9::Ones(); + SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); + + JacobianFactor factor(0, Ab.leftCols(3), Ab.col(3), diagonal); + GaussianFactorGraph::EliminationResult actual = factor.eliminate(list_of(0)); + + Matrix expectedRd(3, 4); + expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131, // + 0.0, -1, 0, 0, // + 0.0, 0.0, -1, 0; + GaussianConditional expectedConditional(0, expectedRd.col(3), expectedRd.leftCols(3), + noiseModel::Unit::Create(3)); + EXPECT(assert_equal(expectedConditional, *actual.first, 1e-4)); + EXPECT(actual.second->empty()); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index d6857c6ff..7ac4846f9 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -222,45 +222,172 @@ namespace exampleQR { SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); } +/* ************************************************************************* */ TEST( NoiseModel, QR ) { Matrix Ab1 = exampleQR::Ab; Matrix Ab2 = exampleQR::Ab; // otherwise overwritten ! - // Expected result - Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished(); - SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); - // Call Gaussian version SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1); - EXPECT(!actual1); + EXPECT(actual1->isUnit()); EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!! - // Call Constrained version - SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas); - SharedDiagonal actual2 = constrained->QR(Ab2); - SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas); - EXPECT(assert_equal(*expectedModel2,*actual2,1e-6)); + // Expected result for constrained version + Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished(); + SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); Matrix expectedRd2 = (Matrix(4, 7) << 1., 0., -0.2, 0., -0.8, 0., 0.2, 0., 1., 0.,-0.2, 0., -0.8,-0.14, 0., 0., 1., 0., -1., 0., 0.0, 0., 0., 0., 1., 0., -1., 0.2).finished(); - EXPECT(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!! + + // Call Constrained version + SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas); + SharedDiagonal actual2 = constrained->QR(Ab2); + EXPECT(assert_equal(*expectedModel, *actual2, 1e-6)); + EXPECT(linear_dependent(expectedRd2, Ab2, 1e-6)); // Ab was modified in place !!! } /* ************************************************************************* */ +TEST(NoiseModel, OverdeterminedQR) { + Matrix Ab1(9, 4); + Ab1 << 0, 1, 0, 0, // + 0, 0, 1, 0, // + Matrix74::Ones(); + Matrix Ab2 = Ab1; // otherwise overwritten ! + + // Call Gaussian version + Vector9 sigmas = Vector9::Ones() ; + SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); + SharedDiagonal actual1 = diagonal->QR(Ab1); + EXPECT(actual1->isUnit()); + Matrix expectedRd(9,4); + expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131, // + 0.0, -1, 0, 0, // + 0.0, 0.0, -1, 0, // + Matrix64::Zero(); + EXPECT(assert_equal(expectedRd, Ab1, 1e-4)); // Ab was modified in place !!! + + // Expected result for constrained version + Vector3 expectedSigmas(0.377964473, 1, 1); + SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); + + // Call Constrained version + SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas); + SharedDiagonal actual2 = constrained->QR(Ab2); + EXPECT(assert_equal(*expectedModel, *actual2, 1e-6)); + expectedRd.row(0) *= 0.377964473; // not divided by sigma! + EXPECT(assert_equal(-expectedRd, Ab2, 1e-6)); // Ab was modified in place !!! +} + +/* ************************************************************************* */ +TEST( NoiseModel, MixedQR ) +{ + // Call Constrained version, with first and third row treated as constraints + // Naming the 6 variables u,v,w,x,y,z, we have + // u = -z + // w = -x + // And let's have simple priors on variables + Matrix Ab(5,6+1); + Ab << + 1,0,0,0,0,1, 0, // u+z = 0 + 0,0,0,0,1,0, 0, // y^2 + 0,0,1,1,0,0, 0, // w+x = 0 + 0,1,0,0,0,0, 0, // v^2 + 0,0,0,0,0,1, 0; // z^2 + Vector mixed_sigmas = (Vector(5) << 0, 1, 0, 1, 1).finished(); + SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(mixed_sigmas); + + // Expected result + Vector expectedSigmas = (Vector(5) << 0, 1, 0, 1, 1).finished(); + SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); + Matrix expectedRd(5, 6+1); + expectedRd << 1, 0, 0, 0, 0, 1, 0, // + 0, 1, 0, 0, 0, 0, 0, // + 0, 0, 1, 1, 0, 0, 0, // + 0, 0, 0, 0, 1, 0, 0, // + 0, 0, 0, 0, 0, 1, 0; // + + SharedDiagonal actual = constrained->QR(Ab); + EXPECT(assert_equal(*expectedModel,*actual,1e-6)); + EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!! +} + +/* ************************************************************************* */ +TEST( NoiseModel, MixedQR2 ) +{ + // Let's have three variables x,y,z, but x=z and y=z + // Hence, all non-constraints are really measurements on z + Matrix Ab(11,3+1); + Ab << + 1,0,0, 0, // + 0,1,0, 0, // + 0,0,1, 0, // + -1,0,1, 0, // x=z + 1,0,0, 0, // + 0,1,0, 0, // + 0,0,1, 0, // + 0,-1,1, 0, // y=z + 1,0,0, 0, // + 0,1,0, 0, // + 0,0,1, 0; // + + Vector sigmas(11); + sigmas.setOnes(); + sigmas[3] = 0; + sigmas[7] = 0; + SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas); + + // Expected result + Vector3 expectedSigmas(0,0,1.0/3); + SharedDiagonal expectedModel = noiseModel::Constrained::MixedSigmas(expectedSigmas); + Matrix expectedRd(11, 3+1); + expectedRd.setZero(); + expectedRd.row(0) << -1, 0, 1, 0; // x=z + expectedRd.row(1) << 0, -1, 1, 0; // y=z + expectedRd.row(2) << 0, 0, 1, 0; // z=0 +/- 1/3 + + SharedDiagonal actual = constrained->QR(Ab); + EXPECT(assert_equal(*expectedModel,*actual,1e-6)); + EXPECT(assert_equal(expectedRd,Ab,1e-6)); // Ab was modified in place !!! +} + +/* ************************************************************************* */ +TEST( NoiseModel, FullyConstrained ) +{ + Matrix Ab(3,7); + Ab << + 1,0,0,0,0,1, 2, // u+z = 2 + 0,0,1,1,0,0, 4, // w+x = 4 + 0,1,0,1,1,1, 8; // v+x+y+z=8 + SharedDiagonal constrained = noiseModel::Constrained::All(3); + + // Expected result + SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(Vector3 (0,0,0)); + Matrix expectedRd(3, 7); + expectedRd << 1, 0, 0, 0, 0, 1, 2, // + 0, 1, 0, 1, 1, 1, 8, // + 0, 0, 1, 1, 0, 0, 4; // + + SharedDiagonal actual = constrained->QR(Ab); + EXPECT(assert_equal(*expectedModel,*actual,1e-6)); + EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!! +} + +/* ************************************************************************* */ +// This matches constraint_eliminate2 in testJacobianFactor TEST(NoiseModel, QRNan ) { SharedDiagonal constrained = noiseModel::Constrained::All(2); - Matrix Ab = (Matrix(2, 5) << 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.).finished(); + Matrix Ab = (Matrix25() << 2, 4, 2, 4, 6, 2, 1, 2, 4, 4).finished(); SharedDiagonal expected = noiseModel::Constrained::All(2); - Matrix expectedAb = (Matrix(2, 5) << 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3).finished(); + Matrix expectedAb = (Matrix25() << 1, 2, 1, 2, 3, 0, 1, 0, 0, 2.0/3).finished(); SharedDiagonal actual = constrained->QR(Ab); EXPECT(assert_equal(*expected,*actual)); - EXPECT(assert_equal(expectedAb,Ab)); + EXPECT(linear_dependent(expectedAb,Ab)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index b2cb5ed82..782c98148 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -25,7 +25,7 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; - cout << " prior mean: " << nT_ << "\n"; + cout << " GPS measurement: " << nT_ << "\n"; noiseModel_->print(" noise model: "); } @@ -38,12 +38,7 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { //*************************************************************************** Vector GPSFactor::evaluateError(const Pose3& p, boost::optional H) const { - if (H) { - H->resize(3, 6); - H->block < 3, 3 > (0, 0) << zeros(3, 3); - H->block < 3, 3 > (0, 3) << p.rotation().matrix(); - } - return p.translation() -nT_; + return p.translation(H) -nT_; } //*************************************************************************** @@ -68,6 +63,25 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, return make_pair(nTb, nV); } +//*************************************************************************** +void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; + nT_.print(" GPS measurement: "); + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); +} + +//*************************************************************************** +Vector GPSFactor2::evaluateError(const NavState& p, + boost::optional H) const { + return p.position(H).vector() -nT_.vector(); +} + //*************************************************************************** }/// namespace gtsam diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 46e194460..2b5ea1f2f 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -37,7 +38,7 @@ private: typedef NoiseModelFactor1 Base; - Point3 nT_; ///< Position measurement in + Point3 nT_; ///< Position measurement in cartesian coordinates public: @@ -50,14 +51,13 @@ public: /** default constructor - only use for serialization */ GPSFactor(): nT_(0, 0, 0) {} - virtual ~GPSFactor() { - } + virtual ~GPSFactor() {} /** * @brief Constructor from a measurement in a Cartesian frame. * Use GeographicLib to convert from geographic (latitude and longitude) coordinates * @param key of the Pose3 variable that will be constrained - * @param gpsIn measurement already in coordinates + * @param gpsIn measurement already in correct coordinates * @param model Gaussian noise model */ GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : @@ -70,18 +70,14 @@ public: gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - /** implement functions needed for Testable */ - - /** print */ + /// print virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /** equals */ + /// equals virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; - /** implement functions needed to derive from Factor */ - - /** vector of errors */ + /// vector of errors Vector evaluateError(const Pose3& p, boost::optional H = boost::none) const; @@ -89,8 +85,8 @@ public: return nT_; } - /* - * Convenience funcion to estimate state at time t, given two GPS + /** + * Convenience function to estimate state at time t, given two GPS * readings (in local NED Cartesian frame) bracketing t * Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector. */ @@ -99,7 +95,71 @@ public: private: - /** Serialization function */ + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(nT_); + } +}; + +/** + * Version of GPSFactor for NavState + * @addtogroup Navigation + */ +class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1 { + +private: + + typedef NoiseModelFactor1 Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GPSFactor2 This; + + /// default constructor - only use for serialization + GPSFactor2():nT_(0, 0, 0) {} + + virtual ~GPSFactor2() {} + + /// Constructor from a measurement in a Cartesian frame. + GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn) { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + /// vector of errors + Vector evaluateError(const NavState& p, + boost::optional H = boost::none) const; + + inline const Point3 & measurementIn() const { + return nT_; + } + +private: + + /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 11799f310..2ad3c17dd 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -44,7 +44,7 @@ public: biasAcc_(biasAcc), biasGyro_(biasGyro) { } - ConstantBias(const Vector6& v) : + explicit ConstantBias(const Vector6& v) : biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { } @@ -114,6 +114,11 @@ public: return ConstantBias(-biasAcc_, -biasGyro_); } + /** addition of vector on right */ + ConstantBias operator+(const Vector6& v) const { + return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>()); + } + /** addition */ ConstantBias operator+(const ConstantBias& b) const { return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 145359d91..6d36539e4 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -72,9 +72,30 @@ void PreintegratedImuMeasurements::integrateMeasurement( preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose(); preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); - // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) - static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); - preintMeasCov_.noalias() += Gi * (iCov * dt) * Gi.transpose(); + // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 + preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt; +} + +//------------------------------------------------------------------------------ +void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs, + const Matrix& measuredOmegas, + const Matrix& dts) { + assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); + assert(dts.cols() >= 1); + assert(measuredAccs.cols() == dts.cols()); + assert(measuredOmegas.cols() == dts.cols()); + size_t n = static_cast(dts.cols()); + for (size_t j = 0; j < n; j++) { + integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j)); + } +} + +//------------------------------------------------------------------------------ +void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // + Matrix9* H1, Matrix9* H2) { + PreintegrationBase::mergeWith(pim12, H1, H2); + preintMeasCov_ = + *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); } //------------------------------------------------------------------------------ @@ -120,9 +141,7 @@ NonlinearFactor::shared_ptr ImuFactor::clone() const { //------------------------------------------------------------------------------ std::ostream& operator<<(std::ostream& os, const ImuFactor& f) { - os << " preintegrated measurements:\n" << f._PIM_; - ; - // Print standard deviations on covariance only + f._PIM_.print("preintegrated measurements:\n"); os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); return os; } @@ -172,9 +191,6 @@ PreintegratedImuMeasurements ImuFactor::Merge( Matrix9 H1, H2; pim02.mergeWith(pim12, &H1, &H2); - pim02.preintMeasCov_ = H1 * pim01.preintMeasCov_ * H1.transpose() + - H2 * pim12.preintMeasCov_ * H2.transpose(); - return pim02; } @@ -229,6 +245,50 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, vel_j = pvb.velocity; } #endif +//------------------------------------------------------------------------------ +// ImuFactor2 methods +//------------------------------------------------------------------------------ +ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias), + _PIM_(pim) {} + +//------------------------------------------------------------------------------ +NonlinearFactor::shared_ptr ImuFactor2::clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) { + f._PIM_.print("preintegrated measurements:\n"); + os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); + return os; +} + +//------------------------------------------------------------------------------ +void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) + << "," << keyFormatter(this->key3()) << ")\n"; + cout << *this << endl; +} + +//------------------------------------------------------------------------------ +bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const { + const This *e = dynamic_cast(&other); + const bool base = Base::equals(*e, tol); + const bool pim = _PIM_.equals(e->_PIM_, tol); + return e != nullptr && base && pim; +} + +//------------------------------------------------------------------------------ +Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j, + const imuBias::ConstantBias& bias_i, // + boost::optional H1, + boost::optional H2, + boost::optional H3) const { + return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); +} + //------------------------------------------------------------------------------ } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 73a2f05d2..010550eb1 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -64,6 +64,7 @@ namespace gtsam { class PreintegratedImuMeasurements: public PreintegrationBase { friend class ImuFactor; + friend class ImuFactor2; protected: @@ -114,12 +115,18 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement */ - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double dt); + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt); + + /// Add multiple measurements, in matrix columns + void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas, + const Matrix& dts); /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } + /// Merge in a different set of measurements and update bias derivatives accordingly + void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2); + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated constructor /// NOTE(frank): assumes Z-Down convention, only second order integration supported @@ -162,8 +169,6 @@ private: */ class ImuFactor: public NoiseModelFactor5 { -public: - private: typedef ImuFactor This; @@ -220,7 +225,7 @@ public: /// 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 = + const imuBias::ConstantBias& bias_i, 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; @@ -265,10 +270,81 @@ private: }; // class ImuFactor +/** + * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. + * @addtogroup SLAM + */ +class ImuFactor2 : public NoiseModelFactor3 { +private: + + typedef ImuFactor2 This; + typedef NoiseModelFactor3 Base; + + PreintegratedImuMeasurements _PIM_; + +public: + + /** Default constructor - only use for serialization */ + ImuFactor2() {} + + /** + * Constructor + * @param state_i Previous state key + * @param state_j Current state key + * @param bias Previous bias key + */ + ImuFactor2(Key state_i, Key state_j, Key bias, + const PreintegratedImuMeasurements& preintegratedMeasurements); + + virtual ~ImuFactor2() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; + + /// @name Testable + /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&); + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + /// @} + + /** Access the preintegrated measurements. */ + + const PreintegratedImuMeasurements& preintegratedMeasurements() const { + return _PIM_; + } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const NavState& state_i, const NavState& state_j, + const imuBias::ConstantBias& bias_i, // + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const; + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(_PIM_); + } +}; +// class ImuFactor2 + template <> struct traits : public Testable {}; template <> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7e5d85cde..f01a55577 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -239,6 +239,7 @@ Matrix7 NavState::wedge(const Vector9& xi) { #define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { @@ -281,6 +282,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, } return newState; } +#endif //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 02da46317..eabd1f39b 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -203,11 +203,13 @@ public: /// @name Dynamics /// @{ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const; +#endif /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index df38df0b8..ec91d69b8 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -63,7 +63,8 @@ void PreintegratedRotation::print(const string& s) const { bool PreintegratedRotation::equals(const PreintegratedRotation& other, double tol) const { - return p_->equals(*other.p_,tol) && deltaRij_.equals(other.deltaRij_, tol) + return this->matchesParamsWith(other) + && deltaRij_.equals(other.deltaRij_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 7209391f1..fefd4b23c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -61,8 +61,8 @@ void PreintegrationBase::print(const string& s) const { //------------------------------------------------------------------------------ bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { - const bool params_match = p_->equals(*other.p_, tol); - return params_match && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) + && fabs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) @@ -72,9 +72,9 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, //------------------------------------------------------------------------------ pair PreintegrationBase::correctMeasurementsBySensorPose( const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, - OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc, - OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega, - OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega) const { + OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc, + OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega, + OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega) const { assert(p().body_P_sensor); // Compensate for sensor-body displacement if needed: we express the quantities @@ -89,9 +89,9 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( const Vector3 correctedOmega = bRs * unbiasedOmega; // Jacobians - if (D_correctedAcc_unbiasedAcc) *D_correctedAcc_unbiasedAcc = bRs; - if (D_correctedAcc_unbiasedOmega) *D_correctedAcc_unbiasedOmega = Z_3x3; - if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs; + if (correctedAcc_H_unbiasedAcc) *correctedAcc_H_unbiasedAcc = bRs; + if (correctedAcc_H_unbiasedOmega) *correctedAcc_H_unbiasedOmega = Z_3x3; + if (correctedOmega_H_unbiasedOmega) *correctedOmega_H_unbiasedOmega = bRs; // Centrifugal acceleration const Vector3 b_arm = p().body_P_sensor->translation(); @@ -103,9 +103,9 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( correctedAcc -= body_Omega_body * b_velocity_bs; // Update derivative: centrifugal causes the correlation between acc and omega!!! - if (D_correctedAcc_unbiasedOmega) { + if (correctedAcc_H_unbiasedOmega) { double wdp = correctedOmega.dot(b_arm); - *D_correctedAcc_unbiasedOmega = -(diag(Vector3::Constant(wdp)) + *correctedAcc_H_unbiasedOmega = -(diag(Vector3::Constant(wdp)) + correctedOmega * b_arm.transpose()) * bRs.matrix() + 2 * b_arm * unbiasedOmega.transpose(); } @@ -366,27 +366,26 @@ Vector9 PreintegrationBase::Compose(const Vector9& zeta01, //------------------------------------------------------------------------------ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, Matrix9* H2) { - if (!matchesParamsWith(pim12)) - throw std::domain_error( - "Cannot merge pre-integrated measurements with different params"); + if (!matchesParamsWith(pim12)) { + throw std::domain_error("Cannot merge pre-integrated measurements with different params"); + } - if (params()->body_P_sensor) - throw std::domain_error( - "Cannot merge pre-integrated measurements with sensor pose yet"); + if (params()->body_P_sensor) { + throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); + } - const double& t01 = deltaTij(); - const double& t12 = pim12.deltaTij(); + const double t01 = deltaTij(); + const double t12 = pim12.deltaTij(); deltaTij_ = t01 + t12; - Vector9 zeta01 = preintegrated(); - Vector9 zeta12 = pim12.preintegrated(); + const Vector9 zeta01 = preintegrated(); + Vector9 zeta12 = pim12.preintegrated(); // will be modified. - // TODO(frank): adjust zeta12 due to bias difference const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); - preintegrated_ << PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); + preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f8639cf79..9d751e92d 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -77,6 +77,7 @@ class PreintegrationBase { /** * Preintegrated navigation state, from frame i to frame j + * Order is: theta, position, velocity * Note: relative position does not take into account velocity at time i, see deltap+, in [2] * Note: velocity is now also in frame i, as opposed to deltaVij in [2] */ @@ -94,6 +95,9 @@ public: /// @name Constructors /// @{ + /// @name Constructors + /// @{ + /** * Constructor, initializes the variables in the base class * @param p Parameters, typically fixed in a single application @@ -111,7 +115,7 @@ public: /// check parameters equality: checks whether shared pointer points to same Params object. bool matchesParamsWith(const PreintegrationBase& other) const { - return p_ == other.p_; + return p_.get() == other.p_.get(); } /// shared pointer to params @@ -134,7 +138,7 @@ public: /// @name Instance variables access /// @{ const imuBias::ConstantBias& biasHat() const { return biasHat_; } - const double& deltaTij() const { return deltaTij_; } + double deltaTij() const { return deltaTij_; } const Vector9& preintegrated() const { return preintegrated_; } @@ -167,9 +171,9 @@ public: /// Ignore D_correctedOmega_measuredAcc as it is trivially zero std::pair correctMeasurementsBySensorPose( const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, - OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc = boost::none, - OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega = boost::none, - OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega = boost::none) const; + OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none, + OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, + OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; // Update integrated vector on tangent manifold preintegrated with acceleration // Static, functional version. @@ -215,7 +219,7 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; - // Compose the two preintegrated vectors + // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1 = boost::none, @@ -244,6 +248,8 @@ public: /// @} #endif + /// @} + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/tests/imuFactorTesting.h b/gtsam/navigation/tests/imuFactorTesting.h index 3247b56ee..5aa83e87e 100644 --- a/gtsam/navigation/tests/imuFactorTesting.h +++ b/gtsam/navigation/tests/imuFactorTesting.h @@ -28,8 +28,8 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +static const Vector3 kZero = Z_3x1; typedef imuBias::ConstantBias Bias; -static const Vector3 Z_3x1 = Vector3::Zero(); static const Bias kZeroBiasHat, kZeroBias; static const Vector3 kZeroOmegaCoriolis(0, 0, 0); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 743fc9baf..fa2e9d832 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -136,20 +136,19 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { auto p = testing::Params(); testing::SomeMeasurements measurements; - boost::function preintegrated = - [=](const Vector3& a, const Vector3& w) { - PreintegratedImuMeasurements pim(p, Bias(a, w)); - testing::integrateMeasurements(measurements, &pim); - return pim.preintegrated(); - }; + auto preintegrated = [=](const Vector3& a, const Vector3& w) { + PreintegratedImuMeasurements pim(p, Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.preintegrated(); + }; // Actual pre-integrated values PreintegratedCombinedMeasurements pim(p); testing::integrateMeasurements(measurements, &pim); - EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), pim.preintegrated_H_biasOmega(), 1e-3)); } diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 6149c1651..de318b3e4 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -29,19 +29,24 @@ using namespace gtsam; using namespace GeographicLib; // ************************************************************************* -TEST( GPSFactor, Constructors ) { +namespace example { +// ENU Origin is where the plane was in hold next to runway +const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; - // Convert from GPS to ENU - // ENU Origin is where the plane was in hold next to runway - const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; - LocalCartesian enu(lat0, lon0, h0, Geocentric::WGS84); +// Convert from GPS to ENU +LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84); - // Dekalb-Peachtree Airport runway 2L - const double lat = 33.87071, lon = -84.30482, h = 274; +// Dekalb-Peachtree Airport runway 2L +const double lat = 33.87071, lon = -84.30482, h = 274; +} + +// ************************************************************************* +TEST( GPSFactor, Constructor ) { + using namespace example; // From lat-lon to geocentric double E, N, U; - enu.Forward(lat, lon, h, E, N, U); + origin_ENU.Forward(lat, lon, h, E, N, U); EXPECT_DOUBLES_EQUAL(133.24, E, 1e-2); EXPECT_DOUBLES_EQUAL(80.98, N, 1e-2); EXPECT_DOUBLES_EQUAL(0, U, 1e-2); @@ -67,6 +72,35 @@ TEST( GPSFactor, Constructors ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +// ************************************************************************* +TEST( GPSFactor2, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactor2 factor(key, Point3(E, N, U), model); + + // Create a linearization point at zero error + NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero()); + EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + boost::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); + + // Use the factor to calculate the derivative + Matrix actualH; + factor.evaluateError(T, actualH); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + //*************************************************************************** TEST(GPSData, init) { diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 28ad037c0..54ca50797 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -94,9 +94,9 @@ TEST(ImuFactor, PreintegratedMeasurements) { // Actual pre-integrated values PreintegratedImuMeasurements actual(testing::Params()); - EXPECT(assert_equal(Z_3x1, actual.theta())); - EXPECT(assert_equal(Z_3x1, actual.deltaPij())); - EXPECT(assert_equal(Z_3x1, actual.deltaVij())); + EXPECT(assert_equal(kZero, actual.theta())); + EXPECT(assert_equal(kZero, actual.deltaPij())); + EXPECT(assert_equal(kZero, actual.deltaVij())); DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -185,8 +185,25 @@ TEST(ImuFactor, PreintegrationBaseMethods) { boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, boost::none), kZeroBias); EXPECT(assert_equal(eH2, aH2)); - return; +} +/* ************************************************************************* */ +TEST(ImuFactor, MultipleMeasurements) { + using namespace common; + + PreintegratedImuMeasurements expected(testing::Params(), kZeroBiasHat); + expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + Matrix32 acc,gyro; + Matrix12 dts; + acc << measuredAcc, measuredAcc; + gyro << measuredOmega, measuredOmega; + dts << deltaT, deltaT; + PreintegratedImuMeasurements actual(testing::Params(), kZeroBiasHat); + actual.integrateMeasurements(acc,gyro,dts); + + EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ @@ -436,9 +453,9 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { PreintegratedImuMeasurements pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); - EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero), pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero), pim.preintegrated_H_biasOmega(), 1e-3)); } @@ -780,7 +797,7 @@ struct ImuFactorMergeTest { ImuFactorMergeTest() : p_(PreintegratedImuMeasurements::Params::MakeSharedU(kGravity)), - forward_(Z_3x1, Vector3(kVelocity, 0, 0)), + forward_(kZero, Vector3(kVelocity, 0, 0)), loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) { // arbitrary noise values p_->gyroscopeCovariance = I_3x3 * 0.01; @@ -867,6 +884,29 @@ TEST(ImuFactor, MergeWithCoriolis) { mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); } +/* ************************************************************************* */ +// Same values as pre-integration test but now testing covariance +TEST(ImuFactor, CheckCovariance) { + // Measurements + Vector3 measuredAcc(0.1, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); + double deltaT = 0.5; + + PreintegratedImuMeasurements actual(testing::Params()); + actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Matrix9 expected; + expected << 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 1.0577e-08, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, 0, // + 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, // + 0, 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, // + 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, 0, // + 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, // + 0, 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06; + EXPECT(assert_equal(expected, actual.preintMeasCov())); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 2fbb9fee3..fb6045d33 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -193,6 +193,7 @@ TEST( NavState, Lie ) { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 TEST(NavState, Update) { Vector3 omega(M_PI / 100.0, 0.0, 0.0); Vector3 acc(0.1, 0.0, 0.0); @@ -220,6 +221,7 @@ TEST(NavState, Update) { EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7)); EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7)); } +#endif /* ************************************************************************* */ static const double dt = 2.0; diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index b733181ac..527a6da7b 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -120,25 +120,24 @@ TEST(PreintegrationBase, Compose) { } /* ************************************************************************* */ - TEST(PreintegrationBase, MergedBiasDerivatives) { +TEST(PreintegrationBase, MergedBiasDerivatives) { testing::SomeMeasurements measurements; - boost::function f = - [=](const Vector3& a, const Vector3& w) { - PreintegrationBase pim02(testing::Params(), Bias(a, w)); - testing::integrateMeasurements(measurements, &pim02); - testing::integrateMeasurements(measurements, &pim02); - return pim02.preintegrated(); - }; + auto f = [=](const Vector3& a, const Vector3& w) { + PreintegrationBase pim02(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim02); + testing::integrateMeasurements(measurements, &pim02); + return pim02.preintegrated(); + }; // Expected merge result PreintegrationBase expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); - EXPECT(assert_equal(numericalDerivative21(f, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative21(f, Z_3x1, Z_3x1), expected_pim02.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), expected_pim02.preintegrated_H_biasOmega(), 1e-7)); } diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index b5d312a86..5097e9e66 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -75,7 +75,7 @@ TEST(ScenarioRunner, Spin) { /* ************************************************************************* */ namespace forward { const double v = 2; // m/s -ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); +ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0)); } /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 70a872d15..e59201ca7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -27,11 +27,6 @@ namespace gtsam { -template -void Expression::print(const std::string& s) const { - root_->print(s); -} - template Expression::Expression(const T& value) : root_(new internal::ConstantExpression(value)) { @@ -48,7 +43,7 @@ Expression::Expression(const Symbol& symbol) : } template -Expression::Expression(unsigned char c, size_t j) : +Expression::Expression(unsigned char c, std::uint64_t j) : root_(new internal::LeafExpression(Symbol(c, j))) { } @@ -128,6 +123,11 @@ void Expression::dims(std::map& map) const { root_->dims(map); } +template +void Expression::print(const std::string& s) const { + root_->print(s); +} + template T Expression::value(const Values& values, boost::optional&> H) const { @@ -259,4 +259,30 @@ std::vector > createUnknowns(size_t n, char c, size_t start) { return unknowns; } +template +ScalarMultiplyExpression::ScalarMultiplyExpression(double s, const Expression& e) + : Expression(boost::make_shared>(s, e)) {} + +template +SumExpression::SumExpression(const std::vector>& expressions) + : Expression(boost::make_shared>(expressions)) {} + +template +SumExpression SumExpression::operator+(const Expression& e) const { + SumExpression copy = *this; + boost::static_pointer_cast>(copy.root_)->add(e); + return copy; +} + +template +SumExpression& SumExpression::operator+=(const Expression& e) { + boost::static_pointer_cast>(this->root_)->add(e); + return *this; +} + +template +size_t SumExpression::nrTerms() const { + return boost::static_pointer_cast>(this->root_)->nrTerms(); +} + } // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 0b348ece9..6ed6bb4a5 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -52,11 +53,14 @@ public: /// Define type so we can apply it as a meta-function typedef Expression type; -private: +protected: // Paul's trick shared pointer, polymorphic root of entire expression tree boost::shared_ptr > root_; + /// Construct with a custom root + Expression(const boost::shared_ptr >& root) : root_(root) {} + public: // Expressions wrap trees of functions that can evaluate their own derivatives. @@ -85,9 +89,6 @@ public: typename MakeOptionalJacobian::type)> type; }; - /// Print - void print(const std::string& s) const; - /// Construct a constant expression Expression(const T& value); @@ -98,7 +99,7 @@ public: Expression(const Symbol& symbol); /// Construct a leaf expression, creating Symbol - Expression(unsigned char c, size_t j); + Expression(unsigned char c, std::uint64_t j); /// Construct a unary function expression template @@ -147,6 +148,9 @@ public: /// Return dimensions for each argument, as a map void dims(std::map& map) const; + /// Print + void print(const std::string& s) const; + /** * @brief Return value and optional derivatives, reverse AD version * Notes: this is not terribly efficient, and H should have correct size. @@ -170,7 +174,7 @@ public: /// Return size needed for memory buffer in traceExecution size_t traceSize() const; -private: +protected: /// Default constructor, for serialization Expression() {} @@ -199,6 +203,85 @@ private: friend class ::ExpressionFactorShallowTest; }; +/** + * A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar + * It optimizes the Jacobian calculation for this specific case + */ +template +class ScalarMultiplyExpression : public Expression { + // Check that T is a vector space + BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); + + public: + explicit ScalarMultiplyExpression(double s, const Expression& e); +}; + +/** + * A SumExpression is a specialization of Expression that just sums the arguments + * It optimizes the Jacobian calculation for this specific case + */ +template +class SumExpression : public Expression { + // Check that T is a vector space + BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); + + public: + explicit SumExpression(const std::vector>& expressions); + + // Syntactic sugar to allow e1 + e2 + e3... + SumExpression operator+(const Expression& e) const; + SumExpression& operator+=(const Expression& e); + + size_t nrTerms() const; +}; + +/** + * Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA + * TODO(frank): create a more efficient version like ScalarMultiplyExpression. This version still + * does a malloc every linearize. + */ +template +Expression linearExpression( + const boost::function& f, const Expression& expression, + const Eigen::Matrix::dimension, traits::dimension>& dTdA) { + // Use lambda to endow f with a linear Jacobian + typename Expression::template UnaryFunction::type g = + [=](const A& value, typename MakeOptionalJacobian::type H) { + if (H) + *H << dTdA; + return f(value); + }; + return Expression(g, expression); +} + +/** + * Construct an expression that executes the scalar multiplication with an input expression + * The type T must be a vector space + * Example: + * Expression a(0), b = 12 * a; + */ +template +ScalarMultiplyExpression operator*(double s, const Expression& e) { + return ScalarMultiplyExpression(s, e); +} + +/** + * Construct an expression that sums two input expressions of the same type T + * The type T must be a vector space + * Example: + * Expression a(0), b(1), c = a + b; + */ +template +SumExpression operator+(const Expression& e1, const Expression& e2) { + return SumExpression({e1, e2}); +} + +/// Construct an expression that subtracts one expression from another +template +SumExpression operator-(const Expression& e1, const Expression& e2) { + return e1 + (-1.0) * e2; +} + /** * Construct a product expression, assumes T::compose(T) -> T * Example: diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 21e17a648..04836a1cb 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -43,8 +43,8 @@ protected: Expression expression_; ///< the expression that is AD enabled FastVector dims_; ///< dimensions of the Jacobian matrices -public: + public: typedef boost::shared_ptr > shared_ptr; /// Constructor @@ -61,9 +61,10 @@ public: const T& measured() const { return measured_; } /// print relies on Testable traits being defined for T - void print(const std::string& s, const KeyFormatter& keyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { NoiseModelFactor::print(s, keyFormatter); - traits::Print(measured_, s + ".measured_"); + traits::Print(measured_, "ExpressionFactor with measurement: "); } /// equals relies on Testable traits being defined for T @@ -80,7 +81,7 @@ public: * both the function evaluation and its derivative(s) in H. */ virtual Vector unwhitenedError(const Values& x, - boost::optional&> H = boost::none) const { + boost::optional&> H = boost::none) const { if (H) { const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 9251aac07..e0c4da94e 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -29,8 +29,8 @@ namespace gtsam { template typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( const GaussianFactorGraph& linearFactorGraph, - const Values& linearizationPoints, Key lastKey, - JacobianFactor::shared_ptr& newPrior) const + const Values& linearizationPoint, Key lastKey, + JacobianFactor::shared_ptr* newPrior) { // Compute the marginal on the last key // Solve the linear factor graph, converting it into a linear Bayes Network @@ -42,7 +42,7 @@ namespace gtsam { // Extract the current estimate of x1,P1 VectorValues result = marginal->solve(VectorValues()); - const T& current = linearizationPoints.at(lastKey); + const T& current = linearizationPoint.at(lastKey); T x = traits::Retract(current, result[lastKey]); // Create a Jacobian Factor from the root node of the produced Bayes Net. @@ -51,7 +51,7 @@ namespace gtsam { // and the key/index needs to be reset to 0, the first key in the next iteration. assert(marginal->nrFrontals() == 1); assert(marginal->nrParents() == 0); - newPrior = boost::make_shared( + *newPrior = boost::make_shared( marginal->keys().front(), marginal->getA(marginal->begin()), marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], @@ -80,20 +80,8 @@ namespace gtsam { /* ************************************************************************* */ template typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( - const MotionFactor& motionFactor) { - - // TODO: This implementation largely ignores the actual factor symbols. - // Calling predict() then update() with drastically - // different keys will still compute as if a common key-set was used - - // Create Keys - Key x0 = motionFactor.key1(); - Key x1 = motionFactor.key2(); - - // Create a set of linearization points - Values linearizationPoints; - linearizationPoints.insert(x0, x_); - linearizationPoints.insert(x1, x_); // TODO should this really be x_ ? + const NoiseModelFactor& motionFactor) { + const auto keys = motionFactor.keys(); // Create a Gaussian Factor Graph GaussianFactorGraph linearFactorGraph; @@ -102,12 +90,14 @@ namespace gtsam { linearFactorGraph.push_back(priorFactor_); // Linearize motion model and add it to the Kalman Filter graph - linearFactorGraph.push_back( - motionFactor.linearize(linearizationPoints)); + Values linearizationPoint; + linearizationPoint.insert(keys[0], x_); + linearizationPoint.insert(keys[1], x_); // TODO should this really be x_ ? + linearFactorGraph.push_back(motionFactor.linearize(linearizationPoint)); // Solve the factor graph and update the current state estimate // and the posterior for the next iteration. - x_ = solve_(linearFactorGraph, linearizationPoints, x1, priorFactor_); + x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_); return x_; } @@ -115,18 +105,8 @@ namespace gtsam { /* ************************************************************************* */ template typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( - const MeasurementFactor& measurementFactor) { - - // TODO: This implementation largely ignores the actual factor symbols. - // Calling predict() then update() with drastically - // different keys will still compute as if a common key-set was used - - // Create Keys - Key x0 = measurementFactor.key(); - - // Create a set of linearization points - Values linearizationPoints; - linearizationPoints.insert(x0, x_); + const NoiseModelFactor& measurementFactor) { + const auto keys = measurementFactor.keys(); // Create a Gaussian Factor Graph GaussianFactorGraph linearFactorGraph; @@ -135,12 +115,13 @@ namespace gtsam { linearFactorGraph.push_back(priorFactor_); // Linearize measurement factor and add it to the Kalman Filter graph - linearFactorGraph.push_back( - measurementFactor.linearize(linearizationPoints)); + Values linearizationPoint; + linearizationPoint.insert(keys[0], x_); + linearFactorGraph.push_back(measurementFactor.linearize(linearizationPoint)); // Solve the factor graph and update the current state estimate // and the prior factor for the next iteration - x_ = solve_(linearFactorGraph, linearizationPoints, x0, priorFactor_); + x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_); return x_; } diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 4adad676e..77bb1ca6c 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -24,76 +24,85 @@ namespace gtsam { +/** + * This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM + * basically does SRIF with Cholesky to solve the filter problem, making this an efficient, + *numerically + * stable Kalman Filter implementation. + * + * The Kalman Filter relies on two models: a motion model that predicts the next state using + * the current state, and a measurement model that predicts the measurement value at a given + * state. Because these two models are situation-dependent, base classes for each have been + * provided above, from which the user must derive a class and incorporate the actual modeling + * equations. + * + * The class provides a "predict" and "update" function to perform these steps independently. + * TODO: a "predictAndUpdate" that combines both steps for some computational savings. + * \nosubgrouping + */ + +template +class ExtendedKalmanFilter { + // Check that VALUE type is a testable Manifold + BOOST_CONCEPT_ASSERT((IsTestable)); + BOOST_CONCEPT_ASSERT((IsManifold)); + + public: + typedef boost::shared_ptr > shared_ptr; + typedef VALUE T; + + //@deprecated: any NoiseModelFactor will do, as long as they have the right keys + typedef NoiseModelFactor2 MotionFactor; + typedef NoiseModelFactor1 MeasurementFactor; + + protected: + T x_; // linearization point + JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_ + + static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints, + Key x, JacobianFactor::shared_ptr* newPrior); + + public: + /// @name Standard Constructors + /// @{ + + ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial); + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const { + std::cout << s << "\n"; + x_.print(s + "x"); + priorFactor_->print(s + "density"); + } + + /// @} + /// @name Interface + /// @{ + /** - * This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM - * basically does SRIF with Cholesky to solve the filter problem, making this an efficient, numerically - * stable Kalman Filter implementation. - * - * The Kalman Filter relies on two models: a motion model that predicts the next state using - * the current state, and a measurement model that predicts the measurement value at a given - * state. Because these two models are situation-dependent, base classes for each have been - * provided above, from which the user must derive a class and incorporate the actual modeling - * equations. - * - * The class provides a "predict" and "update" function to perform these steps independently. - * TODO: a "predictAndUpdate" that combines both steps for some computational savings. - * \nosubgrouping + * Calculate predictive density P(x_) ~ \int P(x_min) P(x_min, x_) + * The motion model should be given as a factor with key1 for x_min and key2_ for x */ + T predict(const NoiseModelFactor& motionFactor); - template - class ExtendedKalmanFilter { + /** + * Calculate posterior density P(x_) ~ L(z|x) P(x) + * The likelihood L(z|x) should be given as a unary factor on x + */ + T update(const NoiseModelFactor& measurementFactor); - // Check that VALUE type is a testable Manifold - BOOST_CONCEPT_ASSERT((IsTestable)); - BOOST_CONCEPT_ASSERT((IsManifold)); + /// Return current predictive (if called after predict)/posterior (if called after update) + const JacobianFactor::shared_ptr Density() const { + return priorFactor_; + } - public: + /// @} +}; - typedef boost::shared_ptr > shared_ptr; - typedef VALUE T; - typedef NoiseModelFactor2 MotionFactor; - typedef NoiseModelFactor1 MeasurementFactor; - - protected: - T x_; // linearization point - JacobianFactor::shared_ptr priorFactor_; // density - - T solve_(const GaussianFactorGraph& linearFactorGraph, - const Values& linearizationPoints, - Key x, JacobianFactor::shared_ptr& newPrior) const; - - public: - - /// @name Standard Constructors - /// @{ - - ExtendedKalmanFilter(Key key_initial, T x_initial, - noiseModel::Gaussian::shared_ptr P_initial); - - /// @} - /// @name Testable - /// @{ - - /// print - void print(const std::string& s="") const { - std::cout << s << "\n"; - x_.print(s+"x"); - priorFactor_->print(s+"density"); - } - - /// @} - /// @name Advanced Interface - /// @{ - - ///TODO: comment - T predict(const MotionFactor& motionFactor); - - ///TODO: comment - T update(const MeasurementFactor& measurementFactor); - - /// @} - }; - -} // namespace +} // namespace #include diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index e420567ec..d12c56b6f 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -11,7 +11,7 @@ /** * @file GaussNewtonOptimizer.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 26, 2012 */ @@ -26,14 +26,19 @@ namespace gtsam { /* ************************************************************************* */ void GaussNewtonOptimizer::iterate() { + gttic(GaussNewtonOptimizer_Iterate); const NonlinearOptimizerState& current = state_; // Linearize graph + gttic(GaussNewtonOptimizer_Linearize); GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values); + gttoc(GaussNewtonOptimizer_Linearize); // Solve Factor Graph + gttic(GaussNewtonOptimizer_Solve); const VectorValues delta = solve(*linear, current.values, params_); + gttoc(GaussNewtonOptimizer_Solve); // Maybe show output if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta"); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 52af4fee3..af37ca954 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -171,14 +171,14 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { } /* ************************************************************************* */ -FastSet ISAM2::getAffectedFactors(const FastList& keys) const { +KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } } if(debug) cout << endl; NonlinearFactorGraph allAffected; - FastSet indices; + KeySet indices; BOOST_FOREACH(const Key key, keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); @@ -197,7 +197,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const { gttic(getAffectedFactors); - FastSet candidates = getAffectedFactors(affectedKeys); + KeySet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); NonlinearFactorGraph nonlinearAffectedFactors; @@ -210,7 +210,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe gttic(check_candidates_and_linearize); GaussianFactorGraph::shared_ptr linearized = boost::make_shared(); - BOOST_FOREACH(size_t idx, candidates) { + BOOST_FOREACH(Key idx, candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { @@ -464,9 +464,8 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke constraintGroups = *constrainKeys; } else { constraintGroups = FastMap(); - const int group = observedKeys.size() < affectedFactorsVarIndex.size() - ? 1 : 0; - BOOST_FOREACH(Key var, observedKeys) + const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; + BOOST_FOREACH (Key var, observedKeys) constraintGroups.insert(make_pair(var, group)); } @@ -766,7 +765,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, map > marginalFactors; // Keep track of factors that get summarized by removing cliques - FastSet factorIndicesToRemove; + KeySet factorIndicesToRemove; // Keep track of variables removed in subtrees KeySet leafKeysRemoved; @@ -828,10 +827,10 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // get the childrens' marginals from any existing children, plus // the marginals from the marginalFactors multimap, which come from any // subtrees already marginalized out. - + // Add child marginals and remove marginalized subtrees GaussianFactorGraph graph; - FastSet factorsInSubtreeRoot; + KeySet factorsInSubtreeRoot; Cliques subtreesToRemove; BOOST_FOREACH(const sharedClique& child, clique->children) { // Remove subtree if child depends on any marginalized keys @@ -867,14 +866,14 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // These are the factors that involve *marginalized* frontal variables in this clique // but do not involve frontal variables of any of its children. // TODO: reuse cached linear factors - FastSet factorsFromMarginalizedInClique_step1; + KeySet factorsFromMarginalizedInClique_step1; BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { if(leafKeys.exists(frontal)) factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } // Remove any factors in subtrees that we're removing at this step BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) { - BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) { + BOOST_FOREACH(Key factorInvolving, variableIndex_[indexInClique]) { factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } // Create factor graph from factor indices BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) { @@ -989,15 +988,15 @@ void ISAM2::updateDelta(bool forceFullSolve) const gttic(Wildfire_update); lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold); gttoc(Wildfire_update); - + // Compute steepest descent step const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, RgProd_); // Update RgProd const VectorValues dx_u = Impl::ComputeGradientSearch(gradAtZero, RgProd_); // Compute gradient search point - + // Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_ deltaReplacedMask_.clear(); - + // Compute dogleg point DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_, @@ -1076,11 +1075,11 @@ VectorValues ISAM2::gradientAtZero() const { // Create result VectorValues g; - + // Sum up contributions for each clique BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots()) gradientAtZeroTreeAdder(root, g); - + return g; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index fdc0021e5..e8165aad9 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -640,7 +640,7 @@ public: protected: - FastSet getAffectedFactors(const FastList& keys) const; + FastSet getAffectedFactors(const FastList& keys) const; GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 3ae8d4c98..307c7f001 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -2,7 +2,7 @@ * @file LinearContainerFactor.h * * @brief Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph - * + * * @date Jul 6, 2012 * @author Alex Cunningham */ @@ -36,8 +36,14 @@ protected: /** direct copy constructor */ LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); + // Some handy typedefs + typedef NonlinearFactor Base; + typedef LinearContainerFactor This; + public: + typedef boost::shared_ptr shared_ptr; + /** Primary constructor: store a linear factor with optional linearization point */ LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 0dca18a1f..40d943656 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -11,7 +11,7 @@ /** * @file Marginals.cpp - * @brief + * @brief * @author Richard Roberts * @date May 14, 2012 */ @@ -26,7 +26,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, + EliminateableFactorGraph::OptionalOrdering ordering) { gttic(MarginalsConstructor); @@ -39,9 +40,9 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, // Compute BayesTree factorization_ = factorization; if(factorization_ == CHOLESKY) - bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminatePreferCholesky); + bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); else if(factorization_ == QR) - bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminateQR); + bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminateQR); } /* ************************************************************************* */ @@ -53,24 +54,29 @@ void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) } /* ************************************************************************* */ -Matrix Marginals::marginalCovariance(Key variable) const { - return marginalInformation(variable).inverse(); +GaussianFactor::shared_ptr Marginals::marginalFactor(Key variable) const { + gttic(marginalFactor); + + // Compute marginal factor + if(factorization_ == CHOLESKY) + return bayesTree_.marginalFactor(variable, EliminatePreferCholesky); + else if(factorization_ == QR) + return bayesTree_.marginalFactor(variable, EliminateQR); + else + throw std::runtime_error("Marginals::marginalFactor: Unknown factorization"); } /* ************************************************************************* */ Matrix Marginals::marginalInformation(Key variable) const { - gttic(marginalInformation); - - // Compute marginal - GaussianFactor::shared_ptr marginalFactor; - if(factorization_ == CHOLESKY) - marginalFactor = bayesTree_.marginalFactor(variable, EliminatePreferCholesky); - else if(factorization_ == QR) - marginalFactor = bayesTree_.marginalFactor(variable, EliminateQR); // Get information matrix (only store upper-right triangle) - gttic(AsMatrix); - return marginalFactor->information(); + gttic(marginalInformation); + return marginalFactor(variable)->information(); +} + +/* ************************************************************************* */ +Matrix Marginals::marginalCovariance(Key variable) const { + return marginalInformation(variable).inverse(); } /* ************************************************************************* */ @@ -129,6 +135,11 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab } } +/* ************************************************************************* */ +VectorValues Marginals::optimize() const { + return bayesTree_.optimize(); +} + /* ************************************************************************* */ void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "Joint marginal on keys "; diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index dd585e902..ad723015d 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -52,25 +52,32 @@ public: * @param graph The factor graph defining the full joint density on all variables. * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering An optional variable ordering for elimination. */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, + EliminateableFactorGraph::OptionalOrdering ordering = boost::none); /** print */ void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /** Compute the marginal factor of a single variable */ + GaussianFactor::shared_ptr marginalFactor(Key variable) const; + + /** Compute the marginal information matrix of a single variable. + * Use LLt(const Matrix&) or RtR(const Matrix&) to obtain the square-root information matrix. */ + Matrix marginalInformation(Key variable) const; + /** Compute the marginal covariance of a single variable */ Matrix marginalCovariance(Key variable) const; - /** Compute the marginal information matrix of a single variable. You can - * use LLt(const Matrix&) or RtR(const Matrix&) to obtain the square-root information - * matrix. */ - Matrix marginalInformation(Key variable) const; - /** Compute the joint marginal covariance of several variables */ JointMarginal jointMarginalCovariance(const std::vector& variables) const; /** Compute the joint marginal information of several variables */ JointMarginal jointMarginalInformation(const std::vector& variables) const; + + /** Optimize the bayes tree */ + VectorValues optimize() const; }; /** @@ -80,7 +87,7 @@ class GTSAM_EXPORT JointMarginal { protected: SymmetricBlockMatrix blockMatrix_; - std::vector keys_; + KeyVector keys_; FastMap indices_; public: diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 5b46742e7..ec77b2bd6 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -207,7 +207,12 @@ public: return noiseModel_->dim(); } - /** access to the noise model */ + /// access to the noise model + const SharedNoiseModel& noiseModel() const { + return noiseModel_; + } + + /// @deprecated access to the noise model SharedNoiseModel get_noiseModel() const { return noiseModel_; } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index f05504d6b..77809b51e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -43,8 +43,8 @@ namespace gtsam { template class FactorGraph; /* ************************************************************************* */ -double NonlinearFactorGraph::probPrime(const Values& c) const { - return exp(-0.5 * error(c)); +double NonlinearFactorGraph::probPrime(const Values& values) const { + return exp(-0.5 * error(values)); } /* ************************************************************************* */ @@ -58,6 +58,24 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key } } +/* ************************************************************************* */ +void NonlinearFactorGraph::printErrors(const Values& values, const std::string& str, + const KeyFormatter& keyFormatter) const { + cout << str << "size: " << size() << endl + << endl; + for (size_t i = 0; i < factors_.size(); i++) { + stringstream ss; + ss << "Factor " << i << ": "; + if (factors_[i] == NULL) { + cout << "NULL" << endl; + } else { + factors_[i]->print(ss.str(), keyFormatter); + cout << "error = " << factors_[i]->error(values) << endl; + } + cout << endl; + } +} + /* ************************************************************************* */ bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol) const { return Base::equals(other, tol); @@ -224,27 +242,17 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } /* ************************************************************************* */ -double NonlinearFactorGraph::error(const Values& c) const { +double NonlinearFactorGraph::error(const Values& values) const { gttic(NonlinearFactorGraph_error); double total_error = 0.; // iterate over all the factors_ to accumulate the log probabilities BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) - total_error += factor->error(c); + total_error += factor->error(values); } return total_error; } -/* ************************************************************************* */ -KeySet NonlinearFactorGraph::keys() const { - KeySet keys; - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { - if(factor) - keys.insert(factor->begin(), factor->end()); - } - return keys; -} - /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMD() const { diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index ecd3b9b56..3702d8a8f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -67,7 +67,7 @@ namespace gtsam { * A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors, * which derive from NonlinearFactor. The values structures are typically (in SAM) more general * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds. - * Linearizing the non-linear factor graph creates a linear factor graph on the + * Linearizing the non-linear factor graph creates a linear factor graph on the * tangent vector space at the linearization point. Because the tangent space is a true * vector space, the config type will be an VectorValues in that linearized factor graph. */ @@ -94,8 +94,13 @@ namespace gtsam { template NonlinearFactorGraph(const FactorGraph& graph) : Base(graph) {} - /** print just calls base class */ - void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /** print */ + void print(const std::string& str = "NonlinearFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** print errors along with factors*/ + void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Test equality */ bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const; @@ -105,14 +110,11 @@ namespace gtsam { const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /** return keys as an ordered set - ordering is by key value */ - KeySet keys() const; - /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ - double error(const Values& c) const; + double error(const Values& values) const; /** Unnormalized probability. O(n) */ - double probPrime(const Values& c) const; + double probPrime(const Values& values) const; /** * Create a symbolic factor graph diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 2c752815e..bb7b9717b 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -184,6 +184,10 @@ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold } /* ************************************************************************* */ - +GTSAM_EXPORT bool checkConvergence(const NonlinearOptimizerParams& params, double currentError, + double newError) { + return checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, + currentError, newError, params.verbosity); +} } diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 7b9fdba41..cb4cbebc1 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -191,7 +191,7 @@ public: /// @} -protected: +protected: /** A default implementation of the optimization loop, which calls iterate() * until checkConvergence returns true. */ @@ -214,4 +214,7 @@ GTSAM_EXPORT bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity = NonlinearOptimizerParams::SILENT); +GTSAM_EXPORT bool checkConvergence(const NonlinearOptimizerParams& params, double currentError, + double newError); + } // gtsam diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 05e58accb..0008252c4 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -284,7 +284,9 @@ namespace gtsam { try { return dynamic_cast&>(value).value(); } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); + // NOTE(abe): clang warns about potential side effects if done in typeid + const Value* value = item->second; + throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); } } @@ -300,7 +302,9 @@ namespace gtsam { try { return dynamic_cast&>(value).value(); } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); + // NOTE(abe): clang warns about potential side effects if done in typeid + const Value* value = item->second; + throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); } } else { return boost::none; diff --git a/gtsam/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h index b15592c00..a5326995c 100644 --- a/gtsam/nonlinear/expressionTesting.h +++ b/gtsam/nonlinear/expressionTesting.h @@ -28,14 +28,14 @@ namespace gtsam { namespace internal { // CPPUnitLite-style test for linearization of an ExpressionFactor template -bool testExpressionJacobians(TestResult& result_, const std::string& name_, +bool testExpressionJacobians(const std::string& name_, const gtsam::Expression& expression, const gtsam::Values& values, double nd_step, double tolerance) { // Create factor size_t size = traits::dimension; ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); - return testFactorJacobians(result_, name_, f, values, nd_step, tolerance); + return testFactorJacobians(name_, f, values, nd_step, tolerance); } } // namespace internal } // namespace gtsam @@ -46,4 +46,4 @@ bool testExpressionJacobians(TestResult& result_, const std::string& name_, /// \param numerical_derivative_step The step to use when computing the finite difference Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \ - { EXPECT(gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance)); } + { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, numerical_derivative_step, tolerance)); } diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 2a9d70cb4..a256f8fe2 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -22,9 +22,7 @@ #include #include -#include -#include -#include +#include namespace gtsam { @@ -33,7 +31,7 @@ namespace gtsam { * 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. - * NOTE(frank): factor that have non vector-space measurements use between or LocalCoordinates + * NOTE(frank): factors that have non vector-space measurements use between or LocalCoordinates * to evaluate the error, and their derivatives will only be correct for near-zero errors. * This is fixable but expensive, and does not matter in practice as most factors will sit near * zero errors anyway. However, it means that below will only be exact for the correct measurement. @@ -45,10 +43,11 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, std::vector > jacobians; // Get size - Eigen::VectorXd e = factor.whitenedError(values); + const Eigen::VectorXd e = factor.whitenedError(values); const size_t rows = e.size(); // Loop over all variables + const double one_over_2delta = 1.0 / (2.0 * delta); VectorValues dX = values.zeroVectors(); BOOST_FOREACH(Key key, factor) { // Compute central differences using the values struct. @@ -59,12 +58,12 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, dx[col] = delta; dX[key] = dx; Values eval_values = values.retract(dX); - Eigen::VectorXd left = factor.whitenedError(eval_values); + const 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)); + const Eigen::VectorXd right = factor.whitenedError(eval_values); + J.col(col) = (left - right) * one_over_2delta; } jacobians.push_back(std::make_pair(key,J)); } @@ -75,7 +74,7 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, namespace internal { // CPPUnitLite-style test for linearization of a factor -bool testFactorJacobians(TestResult& result_, const std::string& name_, +bool testFactorJacobians(const std::string& name_, const NoiseModelFactor& factor, const gtsam::Values& values, double delta, double tolerance) { @@ -111,6 +110,6 @@ bool testFactorJacobians(TestResult& result_, const std::string& name_, /// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ - { EXPECT(gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance)); } + { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); } } // namespace gtsam diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h index 90aa8a8be..6616f0a83 100644 --- a/gtsam/nonlinear/internal/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -31,34 +31,39 @@ namespace internal { * it just passes dense Eigen matrices through. */ template -struct ConvertToVirtualFunctionSupportedMatrixType { +struct ConvertToDynamicIf { template static Eigen::Matrix convert( - const Eigen::MatrixBase & x) { + const Eigen::MatrixBase& x) { return x; } }; template<> -struct ConvertToVirtualFunctionSupportedMatrixType { +struct ConvertToDynamicIf { template static const Eigen::Matrix convert( const Eigen::MatrixBase & x) { return x; } - // special treatment of matrices that don't need conversion - template - static const Eigen::Matrix & convert( - const Eigen::Matrix & x) { + // Most common case: just pass through matrices that are already of the right fixed-size type + template + static const Eigen::Matrix& convert( + const Eigen::Matrix& x) { return x; } }; /** - * The CallRecord is an abstract base class for the any class that stores + * The CallRecord is an abstract base class for any class that stores * the Jacobians of applying a function with respect to each of its arguments, * as well as an execution trace for each of its arguments. + * + * The complicated structure of this class is to get around the limitations of mixing inheritance + * (needed so that a trace can keep Record pointers) and templating (needed for efficient matrix + * multiplication). The "hack" is to implement N differently sized reverse AD methods, and select + * the appropriate version with the dispatch method reverseAD2 below. */ template struct CallRecord { @@ -72,19 +77,18 @@ struct CallRecord { // Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1 // This function then calls ExecutionTrace::reverseAD for every argument // which will in turn call the reverseAD method below. - // This non-virtual function _startReverseAD3, implemented in derived + // Calls virtual function _startReverseAD3, implemented in derived inline void startReverseAD2(JacobianMap& jacobians) const { _startReverseAD3(jacobians); } // Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1 - // Here we convert to dynamic if the - template - inline void reverseAD2(const Eigen::MatrixBase & dFdT, - JacobianMap& jacobians) const { - _reverseAD3( - ConvertToVirtualFunctionSupportedMatrixType< - (Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); + // Here we convert dFdT to a dynamic Matrix if the # rows>5, because _reverseAD3 is only + // specialized for fixed-size matrices up to 5 rows. + // The appropriate _reverseAD3 method is selected by method overloading. + template + inline void reverseAD2(const Eigen::MatrixBase& dFdT, JacobianMap& jacobians) const { + _reverseAD3(ConvertToDynamicIf<(Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); } // This overload supports matrices with both rows and columns dynamically sized. @@ -144,6 +148,8 @@ private: derived().print(indent); } + // Called from base class non-virtual inline method startReverseAD2 + // Calls non-virtual function startReverseAD4, implemented in Derived (ExpressionNode::Record) virtual void _startReverseAD3(JacobianMap& jacobians) const { derived().startReverseAD4(jacobians); } diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index 315261293..ed811764a 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -90,6 +90,7 @@ void handleLeafCase(const Eigen::MatrixBase& dTdA, template class ExecutionTrace { static const int Dim = traits::dimension; + typedef Eigen::Matrix JacobianTT; enum { Constant, Leaf, Function } kind; @@ -97,21 +98,26 @@ class ExecutionTrace { Key key; CallRecord* ptr; } content; -public: + + 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) @@ -122,6 +128,7 @@ public: content.ptr->print(indent + " "); } } + /// Return record pointer, quite unsafe, used only for testing template boost::optional record() { @@ -132,11 +139,11 @@ public: 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 @@ -147,7 +154,11 @@ public: // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. content.ptr->startReverseAD2(jacobians); } - // Either add to Jacobians (Leaf) or propagate (Function) + + /// Either add to Jacobians (Leaf) or propagate (Function) + // This is a crucial method in selecting the pipeline dimension: since it is templated method + // it will call the templated Record method reverseAD2. Hence, at this point in the pipeline + // there are as many reverseAD1 and reverseAD2 versions as there are different Jacobian sizes. template void reverseAD1(const Eigen::MatrixBase & dTdA, JacobianMap& jacobians) const { diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index d2a130830..966f43da8 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -234,7 +234,7 @@ class UnaryExpression: public ExpressionNode { /// 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(); + this->traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } friend class Expression; @@ -298,8 +298,8 @@ public: } /// Given df/dT, multiply in dT/dA and continue reverse AD process - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + template + void reverseAD4(const MatrixType & dFdT, JacobianMap& jacobians) const { trace1.reverseAD1(dFdT * dTdA1, jacobians); } }; @@ -339,7 +339,7 @@ class BinaryExpression: public ExpressionNode { BinaryExpression(Function f, const Expression& e1, const Expression& e2) : expression1_(e1.root()), expression2_(e2.root()), function_(f) { - ExpressionNode::traceSize_ = // + this->traceSize_ = // upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } @@ -406,15 +406,15 @@ public: std::cout << indent << "}" << std::endl; } - /// Start the reverse AD process, see comments in Base + /// Start the reverse AD process, see comments in UnaryExpression 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 { + template + void reverseAD4(const MatrixType & dFdT, JacobianMap& jacobians) const { trace1.reverseAD1(dFdT * dTdA1, jacobians); trace2.reverseAD1(dFdT * dTdA2, jacobians); } @@ -446,7 +446,7 @@ class TernaryExpression: public ExpressionNode { const Expression& e2, const Expression& e3) : expression1_(e1.root()), expression2_(e2.root()), expression3_(e3.root()), // function_(f) { - ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + // + this->traceSize_ = upAligned(sizeof(Record)) + // e1.traceSize() + e2.traceSize() + e3.traceSize(); } @@ -530,8 +530,8 @@ public: } /// Given df/dT, multiply in dT/dA and continue reverse AD process - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + template + void reverseAD4(const MatrixType & dFdT, JacobianMap& jacobians) const { trace1.reverseAD1(dFdT * dTdA1, jacobians); trace2.reverseAD1(dFdT * dTdA2, jacobians); trace3.reverseAD1(dFdT * dTdA3, jacobians); @@ -549,5 +549,194 @@ public: } }; -} // namespace internal -} // namespace gtsam +//----------------------------------------------------------------------------- +/// Expression for scalar multiplication +template +class ScalarMultiplyNode : public ExpressionNode { + // Check that T is a vector space + BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); + + double scalar_; + boost::shared_ptr > expression_; + + public: + /// Constructor with a unary function f, and input argument e1 + ScalarMultiplyNode(double s, const Expression& e) : scalar_(s), expression_(e.root()) { + this->traceSize_ = upAligned(sizeof(Record)) + e.traceSize(); + } + + /// Destructor + virtual ~ScalarMultiplyNode() {} + + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "ScalarMultiplyNode" << std::endl; + expression_->print(indent + " "); + } + + /// Return value + virtual T value(const Values& values) const { + return scalar_ * expression_->value(values); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + return expression_->keys(); + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression_->dims(map); + } + + // Inner Record Class + struct Record : public CallRecordImplementor::dimension> { + static const int Dim = traits::dimension; + typedef Eigen::Matrix JacobianTT; + + double scalar_dTdA; + ExecutionTrace trace; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "ScalarMultiplyNode::Record {" << std::endl; + std::cout << indent << "D(" << typeid(T).name() << ")/D(" << typeid(T).name() + << ") = " << scalar_dTdA << std::endl; + trace.print(); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process + void startReverseAD4(JacobianMap& jacobians) const { + trace.reverseAD1(scalar_dTdA * JacobianTT::Identity(), jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const MatrixType& dFdT, JacobianMap& jacobians) const { + trace.reverseAD1(dFdT * scalar_dTdA, 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); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + T value = expression_->traceExecution(values, record->trace, ptr); + ptr += expression_->traceSize(); + trace.setFunction(record); + record->scalar_dTdA = scalar_; + return scalar_ * value; + } +}; + +//----------------------------------------------------------------------------- +/// Sum Expression +template +class SumExpressionNode : public ExpressionNode { + typedef ExpressionNode NodeT; + std::vector> expressions_; + + public: + explicit SumExpressionNode(const std::vector>& expressions) { + this->traceSize_ = upAligned(sizeof(Record)); + for (const Expression& e : expressions) + add(e); + } + + void add(const Expression& e) { + expressions_.push_back(e.root()); + this->traceSize_ += e.traceSize(); + } + + /// Destructor + virtual ~SumExpressionNode() {} + + size_t nrTerms() const { + return expressions_.size(); + } + + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "SumExpressionNode" << std::endl; + for (const auto& node : expressions_) + node->print(indent + " "); + } + + /// Return value + virtual T value(const Values& values) const { + auto it = expressions_.begin(); + T sum = (*it)->value(values); + for (++it; it != expressions_.end(); ++it) + sum = sum + (*it)->value(values); + return sum; + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + for (const auto& node : expressions_) { + std::set myKeys = node->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + } + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + for (const auto& node : expressions_) + node->dims(map); + } + + // Inner Record Class + struct Record : public CallRecordImplementor::dimension> { + std::vector> traces_; + + explicit Record(size_t nrArguments) : traces_(nrArguments) {} + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "SumExpressionNode::Record {" << std::endl; + for (const auto& trace : traces_) + trace.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// If the SumExpression is the root, we just start as many pipelines as there are terms. + void startReverseAD4(JacobianMap& jacobians) const { + for (const auto& trace : traces_) + // NOTE(frank): equivalent to trace.reverseAD1(dTdA, jacobians) with dTdA=Identity + trace.startReverseAD1(jacobians); + } + + /// If we are not the root, we simply pass on the adjoint matrix dFdT to all terms + template + void reverseAD4(const MatrixType& dFdT, JacobianMap& jacobians) const { + for (const auto& trace : traces_) + // NOTE(frank): equivalent to trace.reverseAD1(dFdT * dTdA, jacobians) with dTdA=Identity + trace.reverseAD1(dFdT, 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); + size_t nrArguments = expressions_.size(); + Record* record = new (ptr) Record(nrArguments); + ptr += upAligned(sizeof(Record)); + size_t i = 0; + T sum = traits::Identity(); + for (const auto& node : expressions_) { + sum = sum + node->traceExecution(values, record->traces_[i++], ptr); + ptr += node->traceSize(); + } + trace.setFunction(record); + return sum; + } +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index af6954341..aa50ce73f 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -238,9 +238,6 @@ TEST(AdaptAutoDiff, SnavelyExpression) { EXPECT_LONGS_EQUAL( sizeof(internal::BinaryExpression::Record), expression.traceSize()); -#ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(336, expression.traceSize()); -#endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); } diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 6c9588d38..bbf19869d 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -18,8 +18,9 @@ */ #include -#include #include +#include +#include #include #include @@ -31,10 +32,16 @@ using boost::assign::map_list_of; using namespace std; using namespace gtsam; +typedef Expression double_; +typedef Expression Point3_; +typedef Expression Vector3_; +typedef Expression Pose3_; +typedef Expression Rot3_; + /* ************************************************************************* */ -template +template Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal, - OptionalJacobian<2, 2> Dp) { + OptionalJacobian<2, 2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -43,7 +50,7 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ // Constant TEST(Expression, Constant) { - Expression R(someR); + Rot3_ R(someR); Values values; Rot3 actual = R.value(values); EXPECT(assert_equal(someR, actual)); @@ -53,7 +60,7 @@ TEST(Expression, Constant) { /* ************************************************************************* */ // Leaf TEST(Expression, Leaf) { - Expression R(100); + Rot3_ R(100); Values values; values.insert(100, someR); @@ -67,7 +74,7 @@ TEST(Expression, Leaves) { Values values; Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); - std::vector > points = createUnknowns(10, 'p', 1); + std::vector points = createUnknowns(10, 'p', 1); EXPECT(assert_equal(somePoint, points.back().value(values))); } @@ -83,9 +90,10 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) { Vector f3(const Point3& p, OptionalJacobian H) { return p; } -Expression p(1); +Point3_ p(1); set expected = list_of(1); -} +} // namespace unary + TEST(Expression, Unary1) { using namespace unary; Expression e(f1, p); @@ -93,19 +101,20 @@ TEST(Expression, Unary1) { } TEST(Expression, Unary2) { using namespace unary; - Expression e(f2, p); + double_ e(f2, p); EXPECT(expected == e.keys()); } + /* ************************************************************************* */ // Unary(Leaf), dynamic TEST(Expression, Unary3) { using namespace unary; -// Expression e(f3, p); + // Expression e(f3, p); } -/* ************************************************************************* */ -//Nullary Method -TEST(Expression, NullaryMethod) { +/* ************************************************************************* */ +// Nullary Method +TEST(Expression, NullaryMethod) { // Create expression Expression p(67); Expression norm(>sam::norm, p); @@ -129,30 +138,34 @@ TEST(Expression, NullaryMethod) { expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); EXPECT(assert_equal(expected, H[0])); } + /* ************************************************************************* */ // Binary(Leaf,Leaf) namespace binary { // Create leaves -double doubleF(const Pose3& pose, // - const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) { +double doubleF(const Pose3& pose, // + const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) { return 0.0; } -Expression x(1); -Expression p(2); -Expression p_cam(x, &Pose3::transform_to, p); +Pose3_ x(1); +Point3_ p(2); +Point3_ p_cam(x, &Pose3::transform_to, p); } + /* ************************************************************************* */ // Check that creating an expression to double compiles TEST(Expression, BinaryToDouble) { using namespace binary; - Expression p_cam(doubleF, x, p); + double_ p_cam(doubleF, x, p); } + /* ************************************************************************* */ // keys TEST(Expression, BinaryKeys) { set expected = list_of(1)(2); EXPECT(expected == binary::p_cam.keys()); } + /* ************************************************************************* */ // dimensions TEST(Expression, BinaryDimensions) { @@ -160,6 +173,7 @@ TEST(Expression, BinaryDimensions) { binary::p_cam.dims(actual); EXPECT(actual == expected); } + /* ************************************************************************* */ // dimensions TEST(Expression, BinaryTraceSize) { @@ -167,6 +181,7 @@ TEST(Expression, BinaryTraceSize) { size_t expectedTraceSize = sizeof(Binary::Record); EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize()); } + /* ************************************************************************* */ // Binary(Leaf,Unary(Binary(Leaf,Leaf))) namespace tree { @@ -179,12 +194,14 @@ Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; Expression projection(f, p_cam); Expression uv_hat(uncalibrate, K, projection); } + /* ************************************************************************* */ // keys TEST(Expression, TreeKeys) { set expected = list_of(1)(2)(3); EXPECT(expected == tree::uv_hat.keys()); } + /* ************************************************************************* */ // dimensions TEST(Expression, TreeDimensions) { @@ -192,32 +209,30 @@ TEST(Expression, TreeDimensions) { tree::uv_hat.dims(actual); EXPECT(actual == expected); } + /* ************************************************************************* */ // TraceSize TEST(Expression, TreeTraceSize) { typedef internal::BinaryExpression Binary1; - EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), - tree::p_cam.traceSize()); + 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(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()); + EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() + + tree::projection.traceSize(), + tree::uv_hat.traceSize()); } + /* ************************************************************************* */ - TEST(Expression, compose1) { - // Create expression - Expression R1(1), R2(2); - Expression R3 = R1 * R2; + Rot3_ R1(1), R2(2); + Rot3_ R3 = R1 * R2; // Check keys set expected = list_of(1)(2); @@ -227,10 +242,9 @@ TEST(Expression, compose1) { /* ************************************************************************* */ // Test compose with arguments referring to the same rotation TEST(Expression, compose2) { - // Create expression - Expression R1(1), R2(1); - Expression R3 = R1 * R2; + Rot3_ R1(1), R2(1); + Rot3_ R3 = R1 * R2; // Check keys set expected = list_of(1); @@ -240,10 +254,9 @@ TEST(Expression, compose2) { /* ************************************************************************* */ // Test compose with one arguments referring to constant rotation TEST(Expression, compose3) { - // Create expression - Expression R1(Rot3::identity()), R2(3); - Expression R3 = R1 * R2; + Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R3 = R1 * R2; // Check keys set expected = list_of(3); @@ -252,9 +265,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) { +Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, 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); @@ -266,20 +278,203 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, } TEST(Expression, ternary) { - // Create expression - Expression A(1), B(2), C(3); - Expression ABC(composeThree, A, B, C); + Rot3_ A(1), B(2), C(3); + Rot3_ ABC(composeThree, A, B, C); // Check keys set expected = list_of(1)(2)(3); EXPECT(expected == ABC.keys()); } +/* ************************************************************************* */ +TEST(Expression, ScalarMultiply) { + const Key key(67); + const Point3_ sum_ = 23 * Point3_(key); + + set expected_keys = list_of(key); + EXPECT(expected_keys == sum_.keys()); + + map actual_dims, expected_dims = map_list_of(key, 3); + sum_.dims(actual_dims); + EXPECT(actual_dims == expected_dims); + + // Check dims as map + std::map map; + sum_.dims(map); + LONGS_EQUAL(1, map.size()); + + Values values; + values.insert(key, Point3(1, 0, 2)); + + // Check value + const Point3 expected(23, 0, 46); + EXPECT(assert_equal(expected, sum_.value(values))); + + // Check value + Jacobians + std::vector H(1); + EXPECT(assert_equal(expected, sum_.value(values, H))); + EXPECT(assert_equal(23 * I_3x3, H[0])); +} + +/* ************************************************************************* */ +TEST(Expression, Sum) { + const Key key(67); + const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1)); + + set expected_keys = list_of(key); + EXPECT(expected_keys == sum_.keys()); + + map actual_dims, expected_dims = map_list_of(key, 3); + sum_.dims(actual_dims); + EXPECT(actual_dims == expected_dims); + + // Check dims as map + std::map map; + sum_.dims(map); + LONGS_EQUAL(1, map.size()); + + Values values; + values.insert(key, Point3(2, 2, 2)); + + // Check value + const Point3 expected(3, 3, 3); + EXPECT(assert_equal(expected, sum_.value(values))); + + // Check value + Jacobians + std::vector H(1); + EXPECT(assert_equal(expected, sum_.value(values, H))); + EXPECT(assert_equal(I_3x3, H[0])); +} + +/* ************************************************************************* */ +TEST(Expression, TripleSum) { + const Key key(67); + const Point3_ p1_(Point3(1, 1, 1)), p2_(key); + const SumExpression sum_ = p1_ + p2_ + p1_; + + LONGS_EQUAL(3, sum_.nrTerms()); + LONGS_EQUAL(1, sum_.keys().size()); + + Values values; + values.insert(key, Point3(2, 2, 2)); + + // Check value + const Point3 expected(4, 4, 4); + EXPECT(assert_equal(expected, sum_.value(values))); + + // Check value + Jacobians + std::vector H(1); + EXPECT(assert_equal(expected, sum_.value(values, H))); + EXPECT(assert_equal(I_3x3, H[0])); +} + +/* ************************************************************************* */ +TEST(Expression, SumOfUnaries) { + const Key key(67); + const double_ norm_(Point3_(key), &Point3::norm); + const double_ sum_ = norm_ + norm_; + + Values values; + values.insert(key, Point3(6, 0, 0)); + + // Check value + EXPECT_DOUBLES_EQUAL(12, sum_.value(values), 1e-9); + + // Check value + Jacobians + std::vector H(1); + EXPECT_DOUBLES_EQUAL(12, sum_.value(values, H), 1e-9); + EXPECT(assert_equal(Vector3(2, 0, 0).transpose(), H[0])); +} + +/* ************************************************************************* */ +TEST(Expression, UnaryOfSum) { + const Key key1(42), key2(67); + const Point3_ sum_ = Point3_(key1) + Point3_(key2); + const double_ norm_(sum_, &Point3::norm); + + map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); + norm_.dims(actual_dims); + EXPECT(actual_dims == expected_dims); + + Values values; + values.insert(key1, Point3(1, 0, 0)); + values.insert(key2, Point3(0, 1, 0)); + + // Check value + EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values), 1e-9); + + // Check value + Jacobians + std::vector H(2); + EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values, H), 1e-9); + EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[0])); + EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[1])); +} + +/* ************************************************************************* */ +TEST(Expression, WeightedSum) { + const Key key1(42), key2(67); + const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2); + + map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); + weighted_sum_.dims(actual_dims); + EXPECT(actual_dims == expected_dims); + + Values values; + values.insert(key1, Point3(1, 0, 0)); + values.insert(key2, Point3(0, 1, 0)); + + // Check value + const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0); + EXPECT(assert_equal(expected, weighted_sum_.value(values))); + + // Check value + Jacobians + std::vector H(2); + EXPECT(assert_equal(expected, weighted_sum_.value(values, H))); + EXPECT(assert_equal(17 * I_3x3, H[0])); + EXPECT(assert_equal(23 * I_3x3, H[1])); +} + +/* ************************************************************************* */ +TEST(Expression, Subtract) { + const Vector3 p = Vector3::Random(), q = Vector3::Random(); + Values values; + values.insert(0, p); + values.insert(1, q); + const Vector3_ expression = Vector3_(0) - Vector3_(1); + set expected_keys = {0, 1}; + EXPECT(expression.keys() == expected_keys); + + // Check value + Jacobians + std::vector H(2); + EXPECT(assert_equal(p - q, expression.value(values, H))); + EXPECT(assert_equal(I_3x3, H[0])); + EXPECT(assert_equal(-I_3x3, H[1])); +} + +/* ************************************************************************* */ +TEST(Expression, LinearExpression) { + const Key key(67); + const boost::function f = boost::bind(&Point3::vector, _1); + const Matrix3 kIdentity = I_3x3; + const Expression linear_ = linearExpression(f, Point3_(key), kIdentity); + + Values values; + values.insert(key, Point3(1, 0, 2)); + + // Check value + const Vector3 expected(1, 0, 2); + EXPECT(assert_equal(expected, linear_.value(values))); + + // Check value + Jacobians + std::vector H(1); + EXPECT(assert_equal(expected, linear_.value(values, H))); + EXPECT(assert_equal(I_3x3, H[0])); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index ddc7b18d6..b93fe1fc1 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -125,7 +125,7 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { if (pose3Prior) pose3Graph.add( BetweenFactor(keyAnchor, pose3Prior->keys()[0], - pose3Prior->prior(), pose3Prior->get_noiseModel())); + pose3Prior->prior(), pose3Prior->noiseModel())); } return pose3Graph; } @@ -327,7 +327,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ Key key = key_value.key; const Rot3& rot = initialRot.at(key); - Pose3 initializedPose = Pose3(rot, Point3(0,0,0)); + Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); initialPose.insert(key, initializedPose); } // add prior diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 86636c38f..dabb7600d 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -9,7 +9,20 @@ namespace gtsam { /** - * JacobianFactor for Schur complement + * JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis + * + * This trick is equivalent to the Schur complement, but can be faster. + * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses, + * is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., + * The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) + * where Enull is an m x (m-3) matrix + * Then Enull'*E*dp = 0, and + * |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| + * Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix. + * + * The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks. + * Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) + * Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult */ template class JacobianFactorSVD: public RegularJacobianFactor { @@ -51,7 +64,7 @@ public: const SharedDiagonal& model = SharedDiagonal()) : Base() { size_t numKeys = Enull.rows() / ZDim; - size_t m2 = ZDim * numKeys - 3; + size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index ab77ec612..1f56adc45 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -30,6 +30,7 @@ public: /// Constructor OrientedPlane3Factor() { } + virtual ~OrientedPlane3Factor() {} /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel, diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 2ac217ac1..9d0975df3 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -61,12 +61,12 @@ public: }; /** - * Factor on unknown rotation R that relates two directions p_i = iRc * z_c + * Factor on unknown rotation iRc that relates two directions c * Directions provide less constraints than a full rotation */ class RotateDirectionsFactor: public NoiseModelFactor1 { - Unit3 p_, z_; ///< Predicted and measured directions, p = iRc * z + Unit3 i_p_, c_z_; ///< Predicted and measured directions, i_p = iRc * c_z typedef NoiseModelFactor1 Base; typedef RotateDirectionsFactor This; @@ -74,9 +74,17 @@ class RotateDirectionsFactor: public NoiseModelFactor1 { public: /// Constructor - RotateDirectionsFactor(Key key, const Unit3& p, const Unit3& z, + RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z, const SharedNoiseModel& model) : - Base(model, key), p_(p), z_(z) { + Base(model, key), i_p_(i_p), c_z_(c_z) { + } + + /// Initialize rotation iRc such that i_p = iRc * c_z + static Rot3 Initialize(const Unit3& i_p, const Unit3& c_z) { + gtsam::Quaternion iRc; + // setFromTwoVectors sets iRc to (a) quaternion which transform c_z into i_p + iRc.setFromTwoVectors(c_z.unitVector(), i_p.unitVector()); + return Rot3(iRc); } /// @return a deep copy of this factor @@ -89,23 +97,21 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << "RotateDirectionsFactor:" << std::endl; - p_.print("p"); - z_.print("z"); + i_p_.print("p"); + c_z_.print("z"); } /// vector of errors returns 2D vector - Vector evaluateError(const Rot3& R, - boost::optional H = boost::none) const { - Unit3 q = R * z_; - Vector e = p_.error(q, H); + Vector evaluateError(const Rot3& iRc, boost::optional H = boost::none) const { + Unit3 i_q = iRc * c_z_; + Vector error = i_p_.error(i_q, H); if (H) { Matrix DR; - R.rotate(z_, DR); + iRc.rotate(c_z_, DR); *H = (*H) * DR; } - return e; + return error; } - }; -} // gtsam +} // namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 09fe84caa..00c19198b 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -1,12 +1,12 @@ /* ---------------------------------------------------------------------------- - + * 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 - + * -------------------------------------------------------------------------- */ /** diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index d037f7535..8636138f2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -432,7 +432,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (factor){ - SharedNoiseModel model = factor->get_noiseModel(); + SharedNoiseModel model = factor->noiseModel(); boost::shared_ptr gaussianModel = boost::dynamic_pointer_cast(model); if (!gaussianModel){ @@ -455,7 +455,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, boost::dynamic_pointer_cast< BetweenFactor >(factor_); if (factor3D){ - SharedNoiseModel model = factor3D->get_noiseModel(); + SharedNoiseModel model = factor3D->noiseModel(); boost::shared_ptr gaussianModel = boost::dynamic_pointer_cast(model); diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index b2528b9d8..e46b8ee71 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -146,7 +146,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, deltaTheta = (Vector(1) << pose2Between->measured().theta()).finished(); // Retrieve the noise model for the relative rotation - SharedNoiseModel model = pose2Between->get_noiseModel(); + SharedNoiseModel model = pose2Between->noiseModel(); boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(model); if (!diagonalModel) @@ -213,7 +213,7 @@ static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { if (pose2Prior) pose2Graph.add( BetweenFactor(keyAnchor, pose2Prior->keys()[0], - pose2Prior->prior(), pose2Prior->get_noiseModel())); + pose2Prior->prior(), pose2Prior->noiseModel())); } return pose2Graph; } @@ -329,7 +329,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, // Retrieve the noise model for the relative rotation boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast( - pose2Between->get_noiseModel()); + pose2Between->noiseModel()); linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); } else { diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 4e8e3dbf3..1dd4e8abc 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -413,8 +413,8 @@ TEST( dataSet, writeBAL_Dubrovnik) CHECK(readBAL(filenameToWrite, writtenData)); // Check that what we read is the same as what we wrote - EXPECT(assert_equal(readData.number_cameras(),writtenData.number_cameras())); - EXPECT(assert_equal(readData.number_tracks(),writtenData.number_tracks())); + EXPECT_LONGS_EQUAL(readData.number_cameras(),writtenData.number_cameras()); + EXPECT_LONGS_EQUAL(readData.number_tracks(),writtenData.number_tracks()); for (size_t i = 0; i < readData.number_cameras(); i++){ PinholeCamera expectedCamera = writtenData.cameras[i]; @@ -437,7 +437,7 @@ TEST( dataSet, writeBAL_Dubrovnik) // check measurements for (size_t k = 0; k < actualTrack.number_measurements(); k++){ - EXPECT(assert_equal(expectedTrack.measurements[k].first,actualTrack.measurements[k].first)); + EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); } } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 3bcc3eccd..8ac6c48b6 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -8,6 +8,8 @@ #include #include +#include +#include #include #include #include @@ -37,7 +39,9 @@ bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); PinholeCamera camera2(data.cameras[1].pose(), Cal3_S2()); -EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); +Rot3 trueRotation(c1Rc2); +Unit3 trueDirection(c1Tc2); +EssentialMatrix trueE(trueRotation, trueDirection); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { @@ -84,8 +88,9 @@ TEST (EssentialMatrixFactor, testData) { //************************************************************************* TEST (EssentialMatrixFactor, factor) { + Key key(1); for (size_t i = 0; i < 5; i++) { - EssentialMatrixFactor factor(1, pA(i), pB(i), model1); + EssentialMatrixFactor factor(key, pA(i), pB(i), model1); // Check evaluation Vector expected(1); @@ -106,6 +111,62 @@ TEST (EssentialMatrixFactor, factor) { } } +//************************************************************************* +TEST(EssentialMatrixFactor, ExpressionFactor) { + Key key(1); + for (size_t i = 0; i < 5; i++) { + boost::function)> f = + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + Expression E_(key); // leaf expression + Expression expr(f, E_); // unary expression + + // Test the derivatives using Paul's magic + Values values; + values.insert(key, trueE); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); + + // Create the factor + ExpressionFactor factor(model1, 0, expr); + + // Check evaluation + Vector expected(1); + expected << 0; + vector Hactual(1); + Vector actual = factor.unwhitenedError(values, Hactual); + EXPECT(assert_equal(expected, actual, 1e-7)); + } +} + +//************************************************************************* +TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { + Key key(1); + for (size_t i = 0; i < 5; i++) { + boost::function)> f = + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + boost::function, + OptionalJacobian<5, 2>)> g; + Expression R_(key); + Expression d_(trueDirection); + Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); + Expression expr(f, E_); + + // Test the derivatives using Paul's magic + Values values; + values.insert(key, trueRotation); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); + + // Create the factor + ExpressionFactor factor(model1, 0, expr); + + // Check evaluation + Vector expected(1); + expected << 0; + vector Hactual(1); + Vector actual = factor.unwhitenedError(values, Hactual); + EXPECT(assert_equal(expected, actual, 1e-7)); + } +} + //************************************************************************* TEST (EssentialMatrixFactor, minimization) { // Here we want to optimize directly on essential matrix constraints diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 1283987d4..a8f48b050 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -20,26 +20,28 @@ using namespace std; using namespace boost::assign; using namespace gtsam; +static const double kDegree = M_PI / 180; + //************************************************************************* // Create some test data // Let's assume IMU is aligned with aero (X-forward,Z down) // And camera is looking forward. -Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0); -Rot3 iRc(cameraX, cameraY, cameraZ); +static const Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0); +static const Rot3 iRc(cameraX, cameraY, cameraZ); // Now, let's create some rotations around IMU frame -Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); -Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), // +static const Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); +static const Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), // i2Ri3 = Rot3::AxisAngle(p2, 1), // i3Ri4 = Rot3::AxisAngle(p3, 1); // The corresponding rotations in the camera frame -Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, // +static const Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, // c2Zc3 = iRc.inverse() * i2Ri3 * iRc, // c3Zc4 = iRc.inverse() * i3Ri4 * iRc; // The corresponding rotated directions in the camera frame -Unit3 z1 = iRc.inverse() * p1, // +static const Unit3 z1 = iRc.inverse() * p1, // z2 = iRc.inverse() * p2, // z3 = iRc.inverse() * p3; @@ -98,8 +100,7 @@ TEST (RotateFactor, minimization) { // Check error at initial estimate Values initial; - double degree = M_PI / 180; - Rot3 initialE = iRc.retract(degree * Vector3(20, -20, 20)); + Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20)); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -172,8 +173,7 @@ TEST (RotateDirectionsFactor, minimization) { // Check error at initial estimate Values initial; - double degree = M_PI / 180; - Rot3 initialE = iRc.retract(degree * Vector3(20, -20, 20)); + Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20)); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -184,7 +184,6 @@ TEST (RotateDirectionsFactor, minimization) { // Optimize LevenbergMarquardtParams parameters; - //parameters.setVerbosity("ERROR"); LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); Values result = optimizer.optimize(); @@ -196,6 +195,39 @@ TEST (RotateDirectionsFactor, minimization) { EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } +//************************************************************************* +TEST(RotateDirectionsFactor, Initialization) { + // Create a gravity vector in a nav frame that has Z up + const Point3 n_gravity(0, 0, -10); + const Unit3 n_p(-n_gravity); + + // NOTE(frank): avoid singularities by using 85/275 instead of 90/270 + std::vector angles = {0, 45, 85, 135, 180, 225, 275, 315}; + for (double yaw : angles) { + const Rot3 nRy = Rot3::Yaw(yaw * kDegree); + for (double pitch : angles) { + const Rot3 yRp = Rot3::Pitch(pitch * kDegree); + for (double roll : angles) { + const Rot3 pRb = Rot3::Roll(roll * kDegree); + + // Rotation from body to nav frame: + const Rot3 nRb = nRy * yRp * pRb; + const Vector3 rpy = nRb.rpy() / kDegree; + + // Simulate measurement of IMU in body frame: + const Point3 b_acc = nRb.unrotate(-n_gravity); + const Unit3 b_z(b_acc); + + // Check initialization + const Rot3 actual_nRb = RotateDirectionsFactor::Initialize(n_p, b_z); + const Vector3 actual_rpy = actual_nRb.rpy() / kDegree; + EXPECT_DOUBLES_EQUAL(rpy.x(), actual_rpy.x(), 1e-5); + EXPECT_DOUBLES_EQUAL(rpy.y(), actual_rpy.y(), 1e-5); + } + } + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 93c38b324..82d8fb3ec 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -231,8 +231,8 @@ TEST( BayesTree, shortcutCheck ) (SymbolicFactor(_E_, _B_)) (SymbolicFactor(_F_, _E_)) (SymbolicFactor(_G_, _F_)); - SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal( - Ordering(list_of(_G_)(_F_)(_E_)(_D_)(_C_)(_B_)(_A_))); + Ordering ordering(list_of(_G_)(_F_)(_E_)(_D_)(_C_)(_B_)(_A_)); + SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal(ordering); //bayesTree.saveGraph("BT1.dot"); @@ -337,8 +337,8 @@ TEST( BayesTree, removeTop3 ) (SymbolicFactor(X(4), L(5))) (SymbolicFactor(X(2), X(4))) (SymbolicFactor(X(3), X(2))); - SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( - Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // remove all SymbolicBayesNet bn; @@ -361,8 +361,8 @@ TEST( BayesTree, removeTop4 ) (SymbolicFactor(X(4), L(5))) (SymbolicFactor(X(2), X(4))) (SymbolicFactor(X(3), X(2))); - SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( - Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // remove all SymbolicBayesNet bn; @@ -386,8 +386,8 @@ TEST( BayesTree, removeTop5 ) (SymbolicFactor(X(4), L(5))) (SymbolicFactor(X(2), X(4))) (SymbolicFactor(X(3), X(2))); - SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( - Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // Remove nonexistant SymbolicBayesNet bn; diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index a5ad7519c..a26d2f581 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -23,10 +23,28 @@ #include #include +#include + using namespace std; using namespace gtsam; using namespace boost::assign; +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, keys1) { + KeySet expected; + expected += 0, 1, 2, 3, 4; + KeySet actual = simpleTestGraph1.keys(); + EXPECT(expected == actual); +} + +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, keys2) { + KeySet expected; + expected += 0, 1, 2, 3, 4, 5; + KeySet actual = simpleTestGraph2.keys(); + EXPECT(expected == actual); +} + /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminateFullSequential) { diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp index 52dfbacb8..d192a6064 100644 --- a/gtsam/symbolic/tests/testSymbolicISAM.cpp +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -87,7 +87,8 @@ TEST( SymbolicISAM, iSAM ) fullGraph += SymbolicFactor(_B_, _S_); // This ordering is chosen to match the one chosen by COLAMD during the ISAM update - SymbolicBayesTree expected = *fullGraph.eliminateMultifrontal(Ordering(list_of(_X_)(_B_)(_S_)(_E_)(_L_)(_T_))); + Ordering ordering(list_of(_X_)(_B_)(_S_)(_E_)(_L_)(_T_)); + SymbolicBayesTree expected = *fullGraph.eliminateMultifrontal(ordering); // Add factor on B and S SymbolicISAM actual = *asiaGraph.eliminateMultifrontal(); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index d74a0c302..470701d97 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -536,6 +536,12 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors( PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); + if (!std::includes(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), + marginalizeKeys.end())) { + throw std::runtime_error("Some keys found in marginalizeKeys do not" + " occur in the graph."); + } + // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys KeySet remainingKeys; std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 042a15108..52786702d 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -31,7 +32,6 @@ using namespace gtsam; // typedefs typedef Eigen::Matrix Vector1; -typedef Expression Double_; typedef Expression Point3_; typedef Expression Event_; @@ -52,7 +52,7 @@ TEST( TOAFactor, NewWay ) { Event_ eventExpression(key); Point3_ microphoneConstant(microphoneAt0); // constant expression double measurement = 7; - Double_ expression(&Event::toa, eventExpression, microphoneConstant); + double_ expression(&Event::toa, eventExpression, microphoneConstant); ExpressionFactor factor(model, measurement, expression); } @@ -92,7 +92,7 @@ TEST( TOAFactor, WholeEnchilada ) { Event_ eventExpression(key); for (size_t i = 0; i < K; i++) { Point3_ microphone_i(microphones[i]); // constant expression - Double_ predictTOA(&Event::toa, eventExpression, microphone_i); + double_ predictTOA(&Event::toa, eventExpression, microphone_i); graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); } diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index f9e40517d..2e95ea33a 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1 +1 @@ -from ._libgtsam_python import * +from gtsampy import * diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py index ca5e524ee..0c80c2fb5 100644 --- a/python/gtsam_examples/ImuFactorExample.py +++ b/python/gtsam_examples/ImuFactorExample.py @@ -18,6 +18,8 @@ BIAS_KEY = int(gtsam.Symbol('b', 0)) V = lambda j: int(gtsam.Symbol('v', j)) X = lambda i: int(gtsam.Symbol('x', i)) +np.set_printoptions(precision=3, suppress=True) + class ImuFactorExample(PreintegrationExample): def __init__(self): @@ -25,14 +27,18 @@ class ImuFactorExample(PreintegrationExample): self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + # Choose one of these twists to change scenario: + zero_twist = (np.zeros(3), np.zeros(3)) forward_twist = (np.zeros(3), self.velocity) loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) + sick_twist = (np.array([math.radians(30), -math.radians(30), 0]), self.velocity) accBias = np.array([-0.3, 0.1, 0.2]) gyroBias = np.array([0.1, 0.3, -0.1]) bias = gtsam.ConstantBias(accBias, gyroBias) - super(ImuFactorExample, self).__init__(loop_twist, bias) + dt = 1e-2 + super(ImuFactorExample, self).__init__(sick_twist, bias, dt) def addPrior(self, i, graph): state = self.scenario.navState(i) @@ -42,41 +48,13 @@ class ImuFactorExample(PreintegrationExample): def run(self): graph = gtsam.NonlinearFactorGraph() - i = 0 # state index - # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) - # simulate the loop - T = 12 - actual_state_i = self.scenario.navState(0) - for k, t in enumerate(np.arange(0, T, self.dt)): - # get measurements and add them to PIM - measuredOmega = self.runner.measuredAngularVelocity(t) - measuredAcc = self.runner.measuredSpecificForce(t) - pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) - - # Plot IMU many times - if k % 10 == 0: - self.plotImu(t, measuredOmega, measuredAcc) - - # Plot every second - if k % 100 == 0: - self.plotGroundTruthPose(t) - - # create IMU factor every second - if (k + 1) % 100 == 0: - factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) - graph.push_back(factor) - pim.resetIntegration() - actual_state_i = self.scenario.navState(t + self.dt) - i += 1 - - # add priors on beginning and end - num_poses = i + 1 - self.addPrior(0, graph) - self.addPrior(num_poses - 1, graph) + H9 = gtsam.OptionalJacobian9(); + T = 12 + num_poses = T + 1 # assumes 1 factor per second initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) for i in range(num_poses): @@ -84,11 +62,51 @@ class ImuFactorExample(PreintegrationExample): initial.insert(X(i), state_i.pose()) initial.insert(V(i), state_i.velocity()) + # simulate the loop + i = 0 # state index + actual_state_i = self.scenario.navState(0) + for k, t in enumerate(np.arange(0, T, self.dt)): + # get measurements and add them to PIM + measuredOmega = self.runner.measuredAngularVelocity(t) + measuredAcc = self.runner.measuredSpecificForce(t) + pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt, H9, H9) + + # Plot IMU many times + if k % 10 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + + # Plot every second + if k % int(1 / self.dt) == 0: + self.plotGroundTruthPose(t) + + # create IMU factor every second + if (k + 1) % int(1 / self.dt) == 0: + factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) + graph.push_back(factor) + if True: + print(factor) + H2 = gtsam.OptionalJacobian96(); + print(pim.predict(actual_state_i, self.actualBias, H9, H2)) + pim.resetIntegration() + actual_state_i = self.scenario.navState(t + self.dt) + i += 1 + + # add priors on beginning and end + self.addPrior(0, graph) + self.addPrior(num_poses - 1, graph) + # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() + + # Calculate and print marginal covariances + marginals = gtsam.Marginals(graph, result) + print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY)) + for i in range(num_poses): + print("Covariance on pose {}:\n{}\n".format(i, marginals.marginalCovariance(X(i)))) + print("Covariance on vel {}:\n{}\n".format(i, marginals.marginalCovariance(V(i)))) # Plot resulting poses i = 0 diff --git a/python/gtsam_examples/Pose2SLAMExample.py b/python/gtsam_examples/Pose2SLAMExample.py new file mode 100644 index 000000000..7bb1296d7 --- /dev/null +++ b/python/gtsam_examples/Pose2SLAMExample.py @@ -0,0 +1,68 @@ +from __future__ import print_function +import gtsam +import math +import numpy as np + +def Vector3(x, y, z): return np.array([x, y, z]) + +# 1. Create a factor graph container and add factors to it +graph = gtsam.NonlinearFactorGraph() + +# 2a. Add a prior on the first pose, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorNoise = gtsam.noiseModel.Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1)) +graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise)) + +# For simplicity, we will use the same noise model for odometry and loop closures +model = gtsam.noiseModel.Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1)) + +# 2b. Add odometry factors +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), model)) +graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), model)) +graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), model)) +graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), model)) + +# 2c. Add the loop closure constraint +# This factor encodes the fact that we have returned to the same pose. In real +# systems, these constraints may be identified in many ways, such as appearance-based +# techniques with camera images. We will use another Between Factor to enforce this constraint: +graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), model)) +graph.print("\nFactor Graph:\n") # print + +# 3. Create the data structure to hold the initialEstimate estimate to the +# solution. For illustrative purposes, these have been deliberately set to incorrect values +initialEstimate = gtsam.Values() +initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) +initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) +initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) +initialEstimate.print("\nInitial Estimate:\n") # print + +# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer +# The optimizer accepts an optional set of configuration parameters, +# controlling things like convergence criteria, the type of linear +# system solver to use, and the amount of information displayed during +# optimization. We will set a few parameters as a demonstration. +parameters = gtsam.GaussNewtonParams() + +# Stop iterating once the change in error between steps is less than this value +parameters.relativeErrorTol = 1e-5 +# Do not perform more than N iteration steps +parameters.maxIterations = 100 +# Create the optimizer ... +optimizer = gtsam.GaussNewtonOptimizer(graph, initialEstimate, parameters) +# ... and optimize +result = optimizer.optimize() +result.print("Final Result:\n") + +# 5. Calculate and print marginal covariances for all variables +marginals = gtsam.Marginals(graph, result) +print("x1 covariance:\n", marginals.marginalCovariance(1)) +print("x2 covariance:\n", marginals.marginalCovariance(2)) +print("x3 covariance:\n", marginals.marginalCovariance(3)) +print("x4 covariance:\n", marginals.marginalCovariance(4)) +print("x5 covariance:\n", marginals.marginalCovariance(5)) + + diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py index 7095dc59e..6b0d83b10 100644 --- a/python/gtsam_examples/PreintegrationExample.py +++ b/python/gtsam_examples/PreintegrationExample.py @@ -27,7 +27,7 @@ class PreintegrationExample(object): params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) return params - def __init__(self, twist=None, bias=None): + def __init__(self, twist=None, bias=None, dt=1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting @@ -43,7 +43,7 @@ class PreintegrationExample(object): V = np.array([2, 0, 0]) self.scenario = gtsam.ConstantTwistScenario(W, V) - self.dt = 1e-2 + self.dt = dt self.maxDim = 5 self.labels = list('xyz') diff --git a/python/gtsam_utils/plot.py b/python/gtsam_utils/plot.py index 84a388bbb..01ec85009 100644 --- a/python/gtsam_utils/plot.py +++ b/python/gtsam_utils/plot.py @@ -1,12 +1,13 @@ import numpy as np import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D as _Axes3D + +def plotPoint3OnAxes(ax, point, linespec): + ax.plot([point.x()], [point.y()], [point.z()], linespec) def plotPoint3(fignum, point, linespec): fig = plt.figure(fignum) ax = fig.gca(projection='3d') - ax.plot([point.x()], [point.y()], [point.z()], linespec) - + plotPoint3OnAxes(ax, point, linespec) def plot3DPoints(fignum, values, linespec, marginals=None): # PLOT3DPOINTS Plots the Point3's in a values, with optional covariances @@ -29,11 +30,7 @@ def plot3DPoints(fignum, values, linespec, marginals=None): continue # I guess it's not a Point3 -def plotPose3(fignum, pose, axisLength=0.1): - # get figure object - fig = plt.figure(fignum) - ax = fig.gca(projection='3d') - +def plotPose3OnAxes(ax, pose, axisLength=0.1): # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global C = pose.translation().vector() @@ -42,18 +39,24 @@ def plotPose3(fignum, pose, axisLength=0.1): xAxis = C + gRp[:, 0] * axisLength L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0) ax.plot(L[:, 0], L[:, 1], L[:, 2], 'r-') - + yAxis = C + gRp[:, 1] * axisLength L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0) ax.plot(L[:, 0], L[:, 1], L[:, 2], 'g-') - + zAxis = C + gRp[:, 2] * axisLength L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0) ax.plot(L[:, 0], L[:, 1], L[:, 2], 'b-') # # plot the covariance # if (nargin>2) && (~isempty(P)) - # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame + # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - # gtsam.covarianceEllipse3D(C,gPp); + # gtsam.covarianceEllipse3D(C,gPp); # end + +def plotPose3(fignum, pose, axisLength=0.1): + # get figure object + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + plotPose3OnAxes(ax, pose, axisLength) diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt index 9d4f9151a..00900206c 100644 --- a/python/handwritten/CMakeLists.txt +++ b/python/handwritten/CMakeLists.txt @@ -10,10 +10,13 @@ endforeach() # Create the library add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs}) +string(TOUPPER "${CMAKE_BUILD_TYPE}" build_type_toupper) set_target_properties(gtsam_python PROPERTIES - OUTPUT_NAME gtsam_python - SKIP_BUILD_RPATH TRUE - CLEAN_DIRECT_OUTPUT 1 + OUTPUT_NAME gtsampy + PREFIX "" + ${build_type_toupper}_POSTFIX "" + SKIP_BUILD_RPATH TRUE + CLEAN_DIRECT_OUTPUT 1 ) target_link_libraries(gtsam_python ${Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_LIBRARY} @@ -21,10 +24,11 @@ target_link_libraries(gtsam_python # Cause the library to be output in the correct directory. # TODO: Change below to work on different systems (currently works only with Linux) +set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_libgtsam_python.so) add_custom_command( - OUTPUT ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + OUTPUT ${output_path} DEPENDS gtsam_python - COMMAND ${CMAKE_COMMAND} -E copy $ ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + COMMAND ${CMAKE_COMMAND} -E copy $ ${output_path} COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so" ) -add_custom_target(copy_gtsam_python_module ALL DEPENDS ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so) \ No newline at end of file +add_custom_target(copy_gtsam_python_module ALL DEPENDS ${output_path}) \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp index 7e8c22a82..94dc10e56 100644 --- a/python/handwritten/exportgtsam.cpp +++ b/python/handwritten/exportgtsam.cpp @@ -16,8 +16,6 @@ **/ #include -#include - #include // base @@ -33,6 +31,7 @@ void exportPose3(); void exportPinholeBaseK(); void exportPinholeCamera(); void exportCal3_S2(); +void export_geometry(); // inference void exportSymbol(); @@ -51,6 +50,7 @@ void exportISAM2(); void exportPriorFactors(); void exportBetweenFactors(); void exportGenericProjectionFactor(); +void export_slam(); // navigation void exportImuFactor(); @@ -62,7 +62,7 @@ void registerNumpyEigenConversions(); //-----------------------------------// -BOOST_PYTHON_MODULE(_libgtsam_python){ +BOOST_PYTHON_MODULE(gtsampy){ // NOTE: We need to call import_array1() instead of import_array() to support both python 2 // and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function @@ -85,6 +85,7 @@ BOOST_PYTHON_MODULE(_libgtsam_python){ exportPinholeBaseK(); exportPinholeCamera(); exportCal3_S2(); + export_geometry(); exportSymbol(); @@ -99,6 +100,7 @@ BOOST_PYTHON_MODULE(_libgtsam_python){ exportPriorFactors(); exportBetweenFactors(); exportGenericProjectionFactor(); + export_slam(); exportImuFactor(); exportScenario(); diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index 57f0fb75e..de5c8e556 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -27,6 +27,7 @@ using namespace gtsam; BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(norm_overloads, Point3::norm, 0, 1) void exportPoint3(){ @@ -40,7 +41,7 @@ class_("Point3") .def("distance", &Point3::distance) .def("dot", &Point3::dot) .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) - .def("norm", &Point3::norm) + .def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>"))) .def("normalized", &Point3::normalized) .def("print", &Point3::print, print_overloads(args("s"))) #ifndef GTSAM_USE_VECTOR3_POINTS diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp index dfdfe8ad1..f778ea4e0 100644 --- a/python/handwritten/geometry/Pose3.cpp +++ b/python/handwritten/geometry/Pose3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -32,6 +32,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(rotation_overloads, Pose3::rotation, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 2, 3) @@ -54,39 +55,38 @@ void exportPose3(){ double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; class_("Pose3") - .def(init<>()) - .def(init()) - .def(init()) - .def(init()) - .def(init()) - .def(init()) - .def("print", &Pose3::print, print_overloads(args("s"))) - .def("equals", &Pose3::equals, equals_overloads(args("pose","tol"))) - .def("identity", &Pose3::identity) - .staticmethod("identity") - .def("bearing", &Pose3::bearing) - .def("matrix", &Pose3::matrix) - .def("transform_from", &Pose3::transform_from, - transform_from_overloads(args("point", "H1", "H2"))) - .def("transform_to", transform_to1, - transform_to_overloads(args("point", "H1", "H2"))) - .def("transform_to", transform_to2) - .def("x", &Pose3::x) - .def("y", &Pose3::y) - .def("z", &Pose3::z) - .def("translation", &Pose3::translation, - translation_overloads()[return_value_policy()]) - .def("rotation", &Pose3::rotation, return_value_policy()) - .def(self * self) // __mult__ - .def(self * other()) // __mult__ - .def(self_ns::str(self)) // __str__ - .def(repr(self)) // __repr__ - .def("compose", compose1) - .def("compose", compose2, compose_overloads()) - .def("between", between1) - .def("between", between2, between_overloads()) - .def("range", range1, range_overloads()) - .def("range", range2, range_overloads()) - .def("bearing", &Pose3::bearing, bearing_overloads()) - ; -} \ No newline at end of file + .def(init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose3::print, print_overloads(args("s"))) + .def("equals", &Pose3::equals, equals_overloads(args("pose", "tol"))) + .def("identity", &Pose3::identity) + .staticmethod("identity") + .def("bearing", &Pose3::bearing) + .def("matrix", &Pose3::matrix) + .def("transform_from", &Pose3::transform_from, + transform_from_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to1, transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to2) + .def("x", &Pose3::x) + .def("y", &Pose3::y) + .def("z", &Pose3::z) + .def("rotation", &Pose3::rotation, + rotation_overloads()[return_value_policy()]) + .def("translation", &Pose3::translation, + translation_overloads()[return_value_policy()]) + .def(self * self) // __mult__ + .def(self * other()) // __mult__ + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + .def("compose", compose1) + .def("compose", compose2, compose_overloads()) + .def("between", between1) + .def("between", between2, between_overloads()) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("bearing", &Pose3::bearing, bearing_overloads()); +} diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp index 685a37ca9..b840718a3 100644 --- a/python/handwritten/geometry/Rot3.cpp +++ b/python/handwritten/geometry/Rot3.cpp @@ -58,7 +58,6 @@ void exportRot3(){ .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion") .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) ) .staticmethod("Quaternion") - .staticmethod("AxisAngle") .def("Expmap", &Rot3::Expmap) .staticmethod("Expmap") .def("ExpmapDerivative", &Rot3::ExpmapDerivative) @@ -69,6 +68,7 @@ void exportRot3(){ .staticmethod("LogmapDerivative") .def("AxisAngle", AxisAngle_0) .def("AxisAngle", AxisAngle_1) + .staticmethod("AxisAngle") .def("Rodrigues", Rodrigues_0) .def("Rodrigues", Rodrigues_1) .staticmethod("Rodrigues") diff --git a/python/handwritten/geometry/export_geometry.cpp b/python/handwritten/geometry/export_geometry.cpp new file mode 100644 index 000000000..8c04f23dc --- /dev/null +++ b/python/handwritten/geometry/export_geometry.cpp @@ -0,0 +1,35 @@ +/* ---------------------------------------------------------------------------- + + * 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 export_geometry + * @brief wraps geometry classes + * @author Ellon Paiva Mendes (LAAS-CNRS) + * @author Frank Dellaert + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; +using namespace std; + +void export_geometry() { + class_("Unit3") + .def(init<>()) + .def(init()) + .def(init()); +} diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp index 3453184bd..3612f7e14 100644 --- a/python/handwritten/linear/NoiseModel.cpp +++ b/python/handwritten/linear/NoiseModel.cpp @@ -110,6 +110,7 @@ void exportNoiseModels(){ .def("Covariance",&Gaussian::Covariance, Gaussian_Covariance_overloads()) .staticmethod("Covariance") ; + register_ptr_to_python< boost::shared_ptr >(); class_, bases >("Diagonal", no_init) .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) @@ -119,6 +120,7 @@ void exportNoiseModels(){ .def("Precisions",&Diagonal::Precisions, Diagonal_Precisions_overloads()) .staticmethod("Precisions") ; + register_ptr_to_python< boost::shared_ptr >(); class_, bases >("Isotropic", no_init) .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) @@ -128,10 +130,12 @@ void exportNoiseModels(){ .def("Precision",&Isotropic::Precision, Isotropic_Precision_overloads()) .staticmethod("Precision") ; + register_ptr_to_python< boost::shared_ptr >(); class_, bases >("Unit", no_init) .def("Create",&Unit::Create) .staticmethod("Create") ; -} \ No newline at end of file + register_ptr_to_python< boost::shared_ptr >(); +} diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index c947ae9ee..2d7e36f47 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -20,6 +20,7 @@ #include #include "gtsam/navigation/ImuFactor.h" +#include "gtsam/navigation/GPSFactor.h" using namespace boost::python; using namespace gtsam; @@ -32,6 +33,8 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PreintegratedImuMeasurements::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(predict_overloads, PreintegrationBase::predict, 2, 4) void exportImuFactor() { class_("OptionalJacobian39", init<>()); @@ -39,6 +42,7 @@ void exportImuFactor() { class_("OptionalJacobian9", init<>()); class_("NavState", init<>()) + .def(init()) // TODO(frank): overload with jacobians .def("print", &NavState::print, print_overloads()) .def("attitude", &NavState::attitude, @@ -73,22 +77,45 @@ void exportImuFactor() { .def("MakeSharedU", &PreintegrationParams::MakeSharedU) .staticmethod("MakeSharedU"); - class_( + // NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html + register_ptr_to_python< boost::shared_ptr >(); + + class_( + "PreintegrationBase", + init&, const imuBias::ConstantBias&>()) + .def("predict", &PreintegrationBase::predict, predict_overloads()) + .def("computeError", &PreintegrationBase::computeError) + .def("resetIntegration", &PreintegrationBase::resetIntegration) + .def("deltaTij", &PreintegrationBase::deltaTij); + + class_>( "PreintegratedImuMeasurements", - init&, - const imuBias::ConstantBias&>()) + init&, const imuBias::ConstantBias&>()) .def(repr(self)) - .def("predict", &PreintegratedImuMeasurements::predict) - .def("computeError", &PreintegratedImuMeasurements::computeError) - .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration) - .def("integrateMeasurement", - &PreintegratedImuMeasurements::integrateMeasurement) + .def("equals", &PreintegratedImuMeasurements::equals, equals_overloads(args("other", "tol"))) + .def("integrateMeasurement", &PreintegratedImuMeasurements::integrateMeasurement) + .def("integrateMeasurements", &PreintegratedImuMeasurements::integrateMeasurements) .def("preintMeasCov", &PreintegratedImuMeasurements::preintMeasCov); - // NOTE(frank): Abstract classes need boost::noncopyable - class_, boost::shared_ptr>( - "ImuFactor") + class_, boost::shared_ptr>("ImuFactor") .def("error", &ImuFactor::error) .def(init()) .def(repr(self)); + register_ptr_to_python>(); + + class_, boost::shared_ptr>("ImuFactor2") + .def("error", &ImuFactor2::error) + .def(init()) + .def(repr(self)); + register_ptr_to_python>(); + + class_, boost::shared_ptr>("GPSFactor") + .def("error", &GPSFactor::error) + .def(init()); + register_ptr_to_python>(); + + class_, boost::shared_ptr>("GPSFactor2") + .def("error", &GPSFactor2::error) + .def(init()); + register_ptr_to_python>(); } diff --git a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp index 7c7cdf3cc..f1c6a0b73 100644 --- a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -3,25 +3,40 @@ #define NO_IMPORT_ARRAY #include +#include #include +#include using namespace boost::python; using namespace gtsam; -void exportLevenbergMarquardtOptimizer(){ - class_("LevenbergMarquardtParams", init<>()) - .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping) - .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor) - .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial) - .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound) - .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound) - .def("setLogFile", &LevenbergMarquardtParams::setLogFile) - .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) - .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM) - ; +void exportLevenbergMarquardtOptimizer() { + class_("GaussNewtonParams", init<>()) + .def_readwrite("maxIterations", &GaussNewtonParams::maxIterations) + .def_readwrite("relativeErrorTol", &GaussNewtonParams::relativeErrorTol); - class_("LevenbergMarquardtOptimizer", - init()) - .def("optimize", &LevenbergMarquardtOptimizer::optimize, return_value_policy()) - ; + class_( + "GaussNewtonOptimizer", + init()) + .def("optimize", &GaussNewtonOptimizer::optimize, + return_value_policy()); + + class_("LevenbergMarquardtParams", init<>()) + .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping) + .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor) + .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial) + .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound) + .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound) + .def("setLogFile", &LevenbergMarquardtParams::setLogFile) + .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) + .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM); + + class_( + "LevenbergMarquardtOptimizer", + init()) + .def("optimize", &LevenbergMarquardtOptimizer::optimize, + return_value_policy()); + + class_("Marginals", init()) + .def("marginalCovariance", &Marginals::marginalCovariance); } diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index 84e82f376..b635589c3 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -27,6 +27,7 @@ #include "gtsam/geometry/Rot3.h" #include "gtsam/geometry/Pose3.h" #include "gtsam/navigation/ImuBias.h" +#include "gtsam/navigation/NavState.h" using namespace boost::python; using namespace gtsam; @@ -45,6 +46,7 @@ void exportValues(){ void (Values::*insert_rot3) (Key, const gtsam::Rot3&) = &Values::insert; void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert; void (Values::*insert_bias) (Key, const Bias&) = &Values::insert; + void (Values::*insert_navstate) (Key, const NavState&) = &Values::insert; void (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &Values::insert; class_("Values", init<>()) @@ -65,6 +67,7 @@ void exportValues(){ .def("insert", insert_rot3) .def("insert", insert_pose3) .def("insert", insert_bias) + .def("insert", insert_navstate) .def("insert", insert_vector3) .def("atPoint2", &Values::at, return_value_policy()) .def("atRot2", &Values::at, return_value_policy()) @@ -73,6 +76,7 @@ void exportValues(){ .def("atRot3", &Values::at, return_value_policy()) .def("atPose3", &Values::at, return_value_policy()) .def("atConstantBias", &Values::at, return_value_policy()) + .def("atNavState", &Values::at, return_value_policy()) .def("atVector3", &Values::at, return_value_policy()) .def("exists", exists1) .def("keys", &Values::keys) diff --git a/python/handwritten/slam/PriorFactor.cpp b/python/handwritten/slam/PriorFactor.cpp index 3ada91f43..ea917d2fa 100644 --- a/python/handwritten/slam/PriorFactor.cpp +++ b/python/handwritten/slam/PriorFactor.cpp @@ -27,6 +27,7 @@ #include "gtsam/geometry/Point3.h" #include "gtsam/geometry/Rot3.h" #include "gtsam/geometry/Pose3.h" +#include "gtsam/navigation/NavState.h" using namespace boost::python; using namespace gtsam; @@ -55,4 +56,5 @@ void exportPriorFactors() PRIORFACTOR(Rot3) PRIORFACTOR(Pose3) PRIORFACTOR(Vector3) + PRIORFACTOR(NavState) } diff --git a/python/handwritten/slam/export_slam.cpp b/python/handwritten/slam/export_slam.cpp new file mode 100644 index 000000000..9431af7aa --- /dev/null +++ b/python/handwritten/slam/export_slam.cpp @@ -0,0 +1,36 @@ +/* ---------------------------------------------------------------------------- + + * 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 export_slam + * @brief wraps slam classes + * @author Ellon Paiva Mendes (LAAS-CNRS) + * @author Frank Dellaert + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; +using namespace std; + +void export_slam() { + class_>( + "RotateDirectionsFactor", + init()) + .def("Initialize", &RotateDirectionsFactor::Initialize) + .staticmethod("Initialize"); +} diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index bda23b3f6..282a6b816 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index f7f5467af..cf54ce96e 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -309,7 +309,8 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator) // c(5|6) // c(1,2|5) // c(3,4|5) - GaussianBayesTree bt = *fg.eliminateMultifrontal(Ordering(fg.keys())); // eliminate in increasing key order, fg.keys() is sorted. + Ordering ordering(fg.keys()); + GaussianBayesTree bt = *fg.eliminateMultifrontal(ordering); // eliminate in increasing key order, fg.keys() is sorted. GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 130dc1bc8..a9ce8ff0a 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -18,10 +18,11 @@ #include #include -#include #include #include #include +#include +#include #include #include #include @@ -513,70 +514,63 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { CHECK(assert_equal(expected, actual, tol)); } -// make a realistic calibration matrix -static double fov = 60; // degrees -static int w = 640, h = 480; -static Cal3_S2 K(fov, w, h); -static boost::shared_ptr shK(new Cal3_S2(K)); - -// typedefs for visual SLAM example -typedef NonlinearFactorGraph VGraph; - -// factors for visual slam -typedef NonlinearEquality2 Point3Equality; - //****************************************************************************** TEST (testNonlinearEqualityConstraint, stereo_constrained ) { + // make a realistic calibration matrix + static double fov = 60; // degrees + static int w = 640, h = 480; + static Cal3_S2 K(fov, w, h); + static boost::shared_ptr shK(new Cal3_S2(K)); + // create initial estimates - Rot3 faceDownY( - (Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished()); - Pose3 pose1(faceDownY, Point3(0,0,0)); // origin, left camera - SimpleCamera camera1(pose1, K); - Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left - SimpleCamera camera2(pose2, K); - Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away + Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0)); + Pose3 poseLeft(faceTowardsY, Point3()); // origin, left camera + SimpleCamera leftCamera(poseLeft, K); + Pose3 poseRight(faceTowardsY, Point3(2, 0, 0)); // 2 units to the right + SimpleCamera rightCamera(poseRight, K); + Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away // keys - Symbol x1('x', 1), x2('x', 2); - Symbol l1('l', 1), l2('l', 2); + Symbol key_x1('x', 1), key_x2('x', 2); + Symbol key_l1('l', 1), key_l2('l', 2); // create graph - VGraph graph; + NonlinearFactorGraph graph; // create equality constraints for poses - graph += NonlinearEquality(x1, camera1.pose()); - graph += NonlinearEquality(x2, camera2.pose()); + graph += NonlinearEquality(key_x1, leftCamera.pose()); + graph += NonlinearEquality(key_x2, rightCamera.pose()); // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(2); graph += GenericProjectionFactor( - camera1.project(landmark), vmodel, x1, l1, shK); + leftCamera.project(landmark), vmodel, key_x1, key_l1, shK); graph += GenericProjectionFactor( - camera2.project(landmark), vmodel, x2, l2, shK); + rightCamera.project(landmark), vmodel, key_x2, key_l2, shK); - // add equality constraint - graph += Point3Equality(l1, l2); + // add equality constraint saying there is only one point + graph += NonlinearEquality2(key_l1, key_l2); // create initial data - Point3 landmark1(0.5, 5.0, 0.0); - Point3 landmark2(1.5, 5.0, 0.0); + Point3 landmark1(0.5, 5, 0); + Point3 landmark2(1.5, 5, 0); Values initValues; - initValues.insert(x1, pose1); - initValues.insert(x2, pose2); - initValues.insert(l1, landmark1); - initValues.insert(l2, landmark2); + initValues.insert(key_x1, poseLeft); + initValues.insert(key_x2, poseRight); + initValues.insert(key_l1, landmark1); + initValues.insert(key_l2, landmark2); // optimize Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); // create config Values truthValues; - truthValues.insert(x1, camera1.pose()); - truthValues.insert(x2, camera2.pose()); - truthValues.insert(l1, landmark); - truthValues.insert(l2, landmark); + truthValues.insert(key_x1, leftCamera.pose()); + truthValues.insert(key_x2, rightCamera.pose()); + truthValues.insert(key_l1, landmark); + truthValues.insert(key_l2, landmark); // check if correct CHECK(assert_equal(truthValues, actual, 1e-5));