diff --git a/.cproject b/.cproject index 7086ce250..5d8469baa 100644 --- a/.cproject +++ b/.cproject @@ -747,6 +747,14 @@ true true + + make + -j4 + testSimilarity3.run + true + true + true + make -j5 @@ -1011,6 +1019,30 @@ true true + + make + -j4 + testOrientedPlane3.run + true + true + true + + + make + -j4 + testPinholePose.run + true + true + true + + + make + -j4 + testCyclic.run + true + true + true + make -j2 @@ -1277,6 +1309,7 @@ make + testSimulated2DOriented.run true false @@ -1316,6 +1349,7 @@ make + testSimulated2D.run true false @@ -1323,6 +1357,7 @@ make + testSimulated3D.run true false @@ -1426,7 +1461,6 @@ make - testErrors.run true false @@ -1520,6 +1554,22 @@ true true + + make + -j4 + testOrientedPlane3Factor.run + true + true + true + + + make + -j4 + testSmartProjectionPoseFactor.run + true + true + true + make -j3 @@ -1721,7 +1771,6 @@ cpack - -G DEB true false @@ -1729,7 +1778,6 @@ cpack - -G RPM true false @@ -1737,7 +1785,6 @@ cpack - -G TGZ true false @@ -1745,7 +1792,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1937,6 +1983,7 @@ make + tests/testGaussianISAM2 true false @@ -1998,22 +2045,6 @@ true true - - make - -j5 - testExpressionMeta.run - true - true - true - - - make - -j4 - testCustomChartExpression.run - true - true - true - make -j5 @@ -2088,7 +2119,6 @@ make - tests/testBayesTree.run true false @@ -2096,7 +2126,6 @@ make - testBinaryBayesNet.run true false @@ -2144,7 +2173,6 @@ make - testSymbolicBayesNet.run true false @@ -2152,7 +2180,6 @@ make - tests/testSymbolicFactor.run true false @@ -2160,7 +2187,6 @@ make - testSymbolicFactorGraph.run true false @@ -2176,7 +2202,6 @@ make - tests/testBayesTree true false @@ -2422,6 +2447,14 @@ true true + + make + -j4 + SFMExampleExpressions_bal.run + true + true + true + make -j2 @@ -2606,14 +2639,6 @@ true true - - make - -j4 - testOrdering.run - true - true - true - make -j2 @@ -2678,14 +2703,6 @@ true true - - make - -j4 - testSymbolicBayesTree.run - true - true - true - make -j5 @@ -2758,6 +2775,14 @@ true true + + make + -j4 + testExecutionTrace.run + true + true + true + make -j5 @@ -2870,6 +2895,14 @@ true true + + make + -j4 + timeSchurFactors.run + true + true + true + make -j5 @@ -3166,6 +3199,14 @@ true true + + make + -j4 + testGroup.run + true + true + true + make -j5 @@ -3304,7 +3345,6 @@ make - testGraph.run true false @@ -3312,7 +3352,6 @@ make - testJunctionTree.run true false @@ -3320,7 +3359,6 @@ make - testSymbolicBayesNetB.run true false @@ -3390,6 +3428,14 @@ true true + + make + -j4 + testLie.run + true + true + true + make -j2 diff --git a/.settings/.gitignore b/.settings/.gitignore new file mode 100644 index 000000000..faa6d2991 --- /dev/null +++ b/.settings/.gitignore @@ -0,0 +1 @@ +/org.eclipse.cdt.codan.core.prefs diff --git a/CMakeLists.txt b/CMakeLists.txt index 38ee89760..fd11a6733 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,6 +158,12 @@ else() set(GTSAM_USE_TBB 0) # This will go into config.h endif() +############################################################################### +# Prohibit Timing build mode in combination with TBB +if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") +endif() + ############################################################################### # Find Google perftools @@ -173,6 +179,11 @@ 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") + endif() else() set(GTSAM_USE_EIGEN_MKL 0) set(EIGEN_USE_MKL_ALL 0) @@ -192,36 +203,40 @@ endif() ############################################################################### # Option for using system Eigen or GTSAM-bundled Eigen -### Disabled until our patches are included in Eigen ### +### These patches only affect usage of MKL. If you want to enable MKL, you *must* +### use our patched version of Eigen ### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) ### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) -### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization) -# option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) -set(GTSAM_USE_SYSTEM_EIGEN OFF) +option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) - # Use generic Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "") - find_package(Eigen3 REQUIRED) include_directories(AFTER "${EIGEN3_INCLUDE_DIR}") + + # Use generic Eigen include paths e.g. + set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") + + # check if MKL is also enabled - can have one or the other, but not both! + if(EIGEN_USE_MKL_ALL) + message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL") + endif() else() - # Use bundled Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/") - + # Use bundled Eigen include path. # Clear any variables set by FindEigen3 if(EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) endif() + # Add the bundled version of eigen to the include path so that it can still be included + # with #include + include_directories(BEFORE "gtsam/3rdparty/Eigen/") + + # set full path to be used by external projects + # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in + set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/") + endif() -# Write Eigen include file with the paths for either the system Eigen or the GTSAM-bundled Eigen -configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eigen_includes.h) - -# Install the configuration file for Eigen -install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty) - ############################################################################### # Global compile options @@ -389,6 +404,11 @@ if(NOT MSVC AND NOT XCODE_VERSION) message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") endif() +if(GTSAM_USE_SYSTEM_EIGEN) + message(STATUS " Use System Eigen : Yes") +else() + message(STATUS " Use System Eigen : No") +endif() if(GTSAM_USE_TBB) message(STATUS " Use Intel TBB : Yes") elseif(TBB_FOUND) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 1bead58d8..c2cd7b449 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -53,11 +53,12 @@ if(NOT FIRST_PASS_DONE) endif() endif() -# Clang on Mac uses a template depth that is less than standard and is too small +# Clang uses a template depth that is less than standard and is too small if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") - if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") - endif() + # Apple Clang before 5.0 does not support -ftemplate-depth. + if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") + endif() endif() # Set up build type library postfixes @@ -97,7 +98,8 @@ if( NOT cmake_build_type_tolower STREQUAL "" AND NOT cmake_build_type_tolower STREQUAL "release" AND NOT cmake_build_type_tolower STREQUAL "timing" AND NOT cmake_build_type_tolower STREQUAL "profiling" - AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo") + AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo" + AND NOT cmake_build_type_tolower STREQUAL "minsizerel") message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).") endif() diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index ba616b025..d1d3d93dd 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -270,7 +270,7 @@ function(install_wrapped_library_internal interfaceHeader) if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - if("${build_type_upper}" STREQUAL "RELEASE") + if(${build_type_upper} STREQUAL "RELEASE") set(build_type_tag "") # Don't create release mode tag on installed directory else() set(build_type_tag "${build_type}") @@ -367,13 +367,18 @@ endfunction() # should be installed to all build type toolboxes function(install_matlab_scripts source_directory patterns) set(patterns_args "") + set(exclude_patterns "") + if(NOT GTSAM_WRAP_SERIALIZATION) + set(exclude_patterns "testSerialization.m") + endif() + foreach(pattern ${patterns}) list(APPEND patterns_args PATTERN "${pattern}") endforeach() if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - if("${build_type_upper}" STREQUAL "RELEASE") + if(${build_type_upper} STREQUAL "RELEASE") set(build_type_tag "") # Don't create release mode tag on installed directory else() set(build_type_tag "${build_type}") @@ -381,10 +386,10 @@ function(install_matlab_scripts source_directory patterns) # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) - install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE) + install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE) + install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 4b3af9810..3e5cadd32 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -195,7 +195,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_test(NAME ${target_name} COMMAND ${target_name}) add_dependencies(check.${groupName} ${target_name}) add_dependencies(check ${target_name}) - add_dependencies(all.tests ${script_name}) + if(NOT XCODE_VERSION) + add_dependencies(all.tests ${script_name}) + endif() # Add TOPSRCDIR set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") diff --git a/doc/.gitignore b/doc/.gitignore new file mode 100644 index 000000000..ac7af2e80 --- /dev/null +++ b/doc/.gitignore @@ -0,0 +1 @@ +/html/ diff --git a/examples/Data/.gitignore b/examples/Data/.gitignore new file mode 100644 index 000000000..2211df63d --- /dev/null +++ b/examples/Data/.gitignore @@ -0,0 +1 @@ +*.txt diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 92f94c3f3..1f6de6cb1 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -14,20 +14,18 @@ * @brief Expressions version of Pose2SLAMExample.cpp * @date Oct 2, 2014 * @author Frank Dellaert - * @author Yong Dian Jian */ // The two new headers that allow using our Automatic Differentiation Expression framework #include #include -// Header order is close to far -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp +#include +#include +#include #include #include -#include -#include -#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 564930d74..35f9884e1 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 46ca02ee4..c3d901507 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -16,11 +16,15 @@ * @author Frank Dellaert */ -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include -#include -#include +#include #include +#include +#include + +// This new header allows us to read examples easily from .graph files +#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 818a9e577..314ccbdb4 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -16,11 +16,11 @@ * @author Frank Dellaert */ +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include -#include -#include #include +#include #include using namespace std; diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index 9507797eb..b83dfa1db 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include using namespace std; diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 4422586b0..2c25d2f8e 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -16,47 +16,15 @@ * @date June 2, 2012 */ -/** - * A simple 2D pose slam example solved using a Conjugate-Gradient method - * - The robot moves in a 2 meter square - * - The robot moves 2 meters each step, turning 90 degrees after each step - * - The robot initially faces along the X axis (horizontal, to the right in 2D) - * - We have full odometry between pose - * - We have a loop closure constraint when the robot returns to the first position - */ - -// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent -// the robot positions -#include -#include - -// Each variable in the system (poses) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use simple integer keys -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Between factors for the relative motion described by odometry measurements. -// We will also use a Between Factor to encode the loop closure constraint -// Also, we will initialize the robot at the origin using a Prior factor. +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include +#include #include +// In contrast to that example, however, we will use a PCG solver here +#include + using namespace std; using namespace gtsam; @@ -66,32 +34,24 @@ int main(int argc, char** argv) { NonlinearFactorGraph graph; // 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) Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); // 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, with the distance set to zero, noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), 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 Values initialEstimate; initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); @@ -104,15 +64,18 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = NonlinearOptimizerParams::Iterative; - { - parameters.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); - cout << "subgraph solver final error = " << graph.error(result) << endl; - } + // LM is still the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.iterativeParams = boost::make_shared(); + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + cout << "subgraph solver final error = " << graph.error(result) << endl; return 0; } diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 5be266d11..38dd1ca81 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -15,13 +15,7 @@ * @author Duy-Nguyen Ta */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data +// For loading the data, see the comments therein for scenario (camera rotates around cube) #include "SFMdata.h" // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index b999e6600..97d646552 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -17,51 +17,25 @@ * @author Frank Dellaert */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data -#include "SFMdata.h" - -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -#include - // In GTSAM, measurement functions are represented as 'factors'. -// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, -// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, -// The calibration should be known. +// The factor we used here is SmartProjectionPoseFactor. +// Every smart factor represent a single landmark, seen from multiple cameras. +// The SmartProjectionPoseFactor only optimizes for the poses of a camera, +// not the calibration, which is assumed known. #include -// Also, we will initialize the robot at some location using a Prior factor. -#include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// Finally, once all of the factors have been added to our factor graph, we will want to -// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -// trust-region method known as Powell's Degleg -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include +// For an explanation of these headers, see SFMExample.cpp +#include "SFMdata.h" +#include using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactor; + +// create a typedef to the camera type +typedef PinholePose Camera; /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -84,12 +58,12 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. - SmartFactor::shared_ptr smartfactor(new SmartFactor()); + SmartFactor::shared_ptr smartfactor(new SmartFactor(K)); for (size_t i = 0; i < poses.size(); ++i) { // generate the 2D measurement - SimpleCamera camera(poses[i], *K); + Camera camera(poses[i], K); Point2 measurement = camera.project(points[j]); // call add() function to add measurement into a single factor, here we need to add: @@ -97,7 +71,7 @@ int main(int argc, char* argv[]) { // 2. the corresponding camera's key // 3. camera noise model // 4. camera calibration - smartfactor->add(measurement, i, measurementNoise, K); + smartfactor->add(measurement, i, measurementNoise); } // insert the smart factor in the graph @@ -106,15 +80,15 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], poseNoise)); + graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); // Because the structure-from-motion problem has a scale ambiguity, the problem is // still under-constrained. Here we add a prior on the second pose x1, so this will // fix the scale by indicating the distance between x0 and x1. // Because these two are fixed, the rest of the poses will be also be fixed. - graph.push_back(PriorFactor(1, poses[1], poseNoise)); // add directly to graph + graph.push_back(PriorFactor(1, Camera(poses[1],K), noise)); // add directly to graph graph.print("Factor Graph:\n"); @@ -123,11 +97,12 @@ int main(int argc, char* argv[]) { Values initialEstimate; Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(i, poses[i].compose(delta)); + initialEstimate.insert(i, Camera(poses[i].compose(delta), K)); initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); result.print("Final results:\n"); // A smart factor represent the 3D point inside the factor, not as a variable. @@ -136,20 +111,20 @@ int main(int argc, char* argv[]) { Values landmark_result; for (size_t j = 0; j < points.size(); ++j) { - // The output of point() is in boost::optional, as sometimes - // the triangulation operation inside smart factor will encounter degeneracy. - boost::optional point; - // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { - point = smart->point(result); + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. + boost::optional point = smart->point(result); if (point) // ignore if boost::optional return NULL landmark_result.insert(j, *point); } } landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; return 0; } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp new file mode 100644 index 000000000..c1b18a946 --- /dev/null +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SFMExample_SmartFactorPCG.cpp + * @brief Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient + * @author Frank Dellaert + */ + +// For an explanation of these headers, see SFMExample_SmartFactor.cpp +#include "SFMdata.h" +#include + +// These extra headers allow us a LM outer loop with PCG linear solver (inner loop) +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Make the typename short so it looks much cleaner +typedef SmartProjectionPoseFactor SmartFactor; + +// create a typedef to the camera type +typedef PinholePose Camera; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // Simulated measurements from each camera pose, adding them to the factor graph + for (size_t j = 0; j < points.size(); ++j) { + + // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. + SmartFactor::shared_ptr smartfactor(new SmartFactor(K)); + + for (size_t i = 0; i < poses.size(); ++i) { + + // generate the 2D measurement + Camera camera(poses[i], K); + Point2 measurement = camera.project(points[j]); + + // call add() function to add measurement into a single factor, here we need to add: + smartfactor->add(measurement, i, measurementNoise); + } + + // insert the smart factor in the graph + graph.push_back(smartfactor); + } + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); + + // Fix the scale ambiguity by adding a prior + graph.push_back(PriorFactor(1, Camera(poses[0],K), noise)); + + // Create the initial estimate to the solution + Values initialEstimate; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(i, Camera(poses[i].compose(delta),K)); + + // We will use LM in the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + LevenbergMarquardtParams parameters; + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.absoluteErrorTol = 1e-10; + parameters.relativeErrorTol = 1e-10; + parameters.maxIterations = 500; + PCGSolverParameters::shared_ptr pcg = + boost::make_shared(); + pcg->preconditioner_ = + boost::make_shared(); + // Following is crucial: + pcg->setEpsilon_abs(1e-10); + pcg->setEpsilon_rel(1e-10); + parameters.iterativeParams = pcg; + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + + // Display result as in SFMExample_SmartFactor.run + result.print("Final results:\n"); + Values landmark_result; + for (size_t j = 0; j < points.size(); ++j) { + SmartFactor::shared_ptr smart = // + boost::dynamic_pointer_cast(graph[j]); + if (smart) { + boost::optional point = smart->point(result); + if (point) // ignore if boost::optional return NULL + landmark_result.insert(j, *point); + } + } + + landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; + + return 0; +} +/* ************************************************************************* */ + diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 0f61a4220..068846884 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -31,28 +31,30 @@ * */ -#include -#include -#include -#include -#include #include #include -#include -#include -#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include +#include // for GTSAM_USE_TBB -#include -#include -#include #include -#include +#include #include #include #include +#include + +#include +#include #ifdef GTSAM_USE_TBB #include @@ -72,23 +74,6 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -//GTSAM_VALUE_EXPORT(Value); -//GTSAM_VALUE_EXPORT(Pose); -//GTSAM_VALUE_EXPORT(Rot2); -//GTSAM_VALUE_EXPORT(Point2); -//GTSAM_VALUE_EXPORT(NonlinearFactor); -//GTSAM_VALUE_EXPORT(NoiseModelFactor); -//GTSAM_VALUE_EXPORT(NM1); -//GTSAM_VALUE_EXPORT(NM2); -//GTSAM_VALUE_EXPORT(BetweenFactor); -//GTSAM_VALUE_EXPORT(PriorFactor); -//GTSAM_VALUE_EXPORT(BR); -//GTSAM_VALUE_EXPORT(noiseModel::Base); -//GTSAM_VALUE_EXPORT(noiseModel::Isotropic); -//GTSAM_VALUE_EXPORT(noiseModel::Gaussian); -//GTSAM_VALUE_EXPORT(noiseModel::Diagonal); -//GTSAM_VALUE_EXPORT(noiseModel::Unit); - double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { // Compute degrees of freedom (observations - variables) // In ocaml, +1 was added to the observations to account for the prior, but @@ -269,12 +254,12 @@ void runIncremental() boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { Key key1 = measurement->key1(), key2 = measurement->key2(); - if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) { + if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { // We found an odometry starting at firstStep firstPose = std::min(key1, key2); break; } - if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) { + if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) { // We found an odometry joining firstStep with a previous pose havePreviousPose = true; firstPose = std::max(key1, key2); @@ -303,7 +288,9 @@ void runIncremental() cout << "Playing forward time steps..." << endl; - for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step) + for (size_t step = firstPose; + nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep); + ++step) { Values newVariables; NonlinearFactorGraph newFactors; diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index a35980836..602a00593 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -17,6 +17,7 @@ #include #include + #include #include #include diff --git a/gtsam.h b/gtsam.h index c5528309e..46e0ffa28 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,7 +156,7 @@ virtual class Value { size_t dim() const; }; -#include +#include class LieScalar { // Standard constructors LieScalar(); @@ -185,7 +185,7 @@ class LieScalar { static Vector Logmap(const gtsam::LieScalar& p); }; -#include +#include class LieVector { // Standard constructors LieVector(); @@ -217,7 +217,7 @@ class LieVector { void serialize() const; }; -#include +#include class LieMatrix { // Standard constructors LieMatrix(); @@ -288,6 +288,32 @@ class Point2 { void serialize() const; }; +// std::vector +class Point2Vector +{ + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); + + //Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + //Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; + + //Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; + class StereoPoint2 { // Standard Constructors StereoPoint2(); @@ -304,8 +330,6 @@ class StereoPoint2 { gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::StereoPoint2 retract(Vector v) const; Vector localCoordinates(const gtsam::StereoPoint2& p) const; @@ -550,6 +574,16 @@ class Pose3 { void serialize() const; }; +// std::vector +class Pose3Vector +{ + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& x); +}; + #include class Unit3 { // Standard Constructors @@ -778,7 +812,7 @@ class CalibratedCamera { // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); // Standard Interface gtsam::Pose3 pose() const; @@ -788,56 +822,16 @@ class CalibratedCamera { void serialize() const; }; -class SimpleCamera { - // Standard Constructors and Named Constructors - SimpleCamera(); - SimpleCamera(const gtsam::Pose3& pose); - SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, - const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, - const gtsam::Point3& target, const gtsam::Point3& upVector, - const gtsam::Cal3_S2& K); - - // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - gtsam::Cal3_S2 calibration(); - - // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::SimpleCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; -}; - -template +template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); - static This Level(const gtsam::Cal3DS2& K, - const gtsam::Pose2& pose, double height); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); static This Level(const gtsam::Pose2& pose, double height); - static This Lookat(const gtsam::Point3& eye, - const gtsam::Point3& target, const gtsam::Point3& upVector, - const gtsam::Cal3DS2& K); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); // Testable void print(string s) const; @@ -854,7 +848,7 @@ class PinholeCamera { static size_t Dim(); // Transformations and measurement functions - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; gtsam::Point2 project(const gtsam::Point3& point); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; @@ -865,6 +859,50 @@ class PinholeCamera { void serialize() const; }; +virtual class SimpleCamera { + // Standard Constructors and Named Constructors + SimpleCamera(); + SimpleCamera(const gtsam::Pose3& pose); + SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); + static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); + + // Testable + void print(string s) const; + bool equals(const gtsam::SimpleCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + gtsam::Cal3_S2 calibration() const; + + // Manifold + gtsam::SimpleCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::SimpleCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; + +}; + +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; + class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); @@ -893,6 +931,16 @@ class StereoCamera { void serialize() const; }; +#include + +// Templates appear not yet supported for free functions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); + //************************************************************************* // Symbolic //************************************************************************* @@ -1606,12 +1654,12 @@ char symbolChr(size_t key); size_t symbolIndex(size_t key); // Default keyformatter -void printKeyList (const gtsam::KeyList& keys); -void printKeyList (const gtsam::KeyList& keys, string s); -void printKeyVector(const gtsam::KeyVector& keys); -void printKeyVector(const gtsam::KeyVector& keys, string s); -void printKeySet (const gtsam::KeySet& keys); -void printKeySet (const gtsam::KeySet& keys, string s); +void PrintKeyList (const gtsam::KeyList& keys); +void PrintKeyList (const gtsam::KeyList& keys, string s); +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); +void PrintKeySet (const gtsam::KeySet& keys); +void PrintKeySet (const gtsam::KeySet& keys, string s); #include class LabeledSymbol { @@ -1728,7 +1776,7 @@ class Values { void swap(gtsam::Values& values); bool exists(size_t j) const; - gtsam::KeyList keys() const; + gtsam::KeyVector keys() const; gtsam::VectorValues zeroVectors() const; @@ -1845,8 +1893,6 @@ class KeySet { class KeyVector { KeyVector(); KeyVector(const gtsam::KeyVector& other); - KeyVector(const gtsam::KeySet& other); - KeyVector(const gtsam::KeyList& other); // Note: no print function @@ -2176,9 +2222,6 @@ class NonlinearISAM { //************************************************************************* // Nonlinear factor types //************************************************************************* -#include -#include -#include #include #include #include @@ -2309,24 +2352,37 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { void serialize() const; }; +#include +class SmartProjectionParams { + SmartProjectionParams(); + // TODO(frank): make these work: + // void setLinearizationMode(LinearizationMode linMode); + // void setDegeneracyMode(DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; + #include -template -virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { +template +virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + SmartProjectionPoseFactor(const CALIBRATION* K); + SmartProjectionPoseFactor(const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); - SmartProjectionPoseFactor(double rankTol); - SmartProjectionPoseFactor(); - - void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, - const CALIBRATION* K_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i, + const gtsam::noiseModel::Base* noise_i); // enabling serialization functionality //void serialize() const; }; -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 301548dcf..4adbfb250 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -58,10 +58,10 @@ add_subdirectory(ceres) include(GeographicLib/cmake/FindGeographicLib.cmake) # Set up the option to install GeographicLib -if(GEOGRAPHICLIB_FOUND) - set(install_geographiclib_default OFF) -else() +if(GEOGRAPHICLIB-NOTFOUND) set(install_geographiclib_default ON) +else() + set(install_geographiclib_default OFF) endif() option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default}) diff --git a/gtsam/3rdparty/Eigen/.hg_archival.txt b/gtsam/3rdparty/Eigen/.hg_archival.txt index 9fceca658..aea5a5515 100644 --- a/gtsam/3rdparty/Eigen/.hg_archival.txt +++ b/gtsam/3rdparty/Eigen/.hg_archival.txt @@ -1,4 +1,4 @@ repo: 8a21fd850624c931e448cbcfb38168cb2717c790 -node: ffa86ffb557094721ca71dcea6aed2651b9fd610 +node: 10219c95fe653d4962aa9db4946f6fbea96dd740 branch: 3.2 -tag: 3.2.0 +tag: 3.2.4 diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags index 1b9b1142e..7a6e19411 100644 --- a/gtsam/3rdparty/Eigen/.hgtags +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -23,3 +23,7 @@ bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2 da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1 4b687cad1d23066f66863f4f87298447298443df 3.2-rc1 1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2 +ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0 +6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1 +1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2 +36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3 diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index c52b7d1a6..02ab93880 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -442,6 +442,7 @@ LDLT& LDLT::compute(const MatrixType& a) m_transpositions.resize(size); m_isInitialized = false; m_temporary.resize(size); + m_sign = internal::ZeroSign; internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, m_sign); @@ -502,7 +503,6 @@ struct solve_retval, Rhs> using std::abs; using std::max; typedef typename LDLTType::MatrixType MatrixType; - typedef typename LDLTType::Scalar Scalar; typedef typename LDLTType::RealScalar RealScalar; const typename Diagonal::RealReturnType vectorD(dec().vectorD()); // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h index a791bc358..b4641e2a0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h @@ -29,6 +29,11 @@ struct traits > : public traits::type > { typedef ArrayXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits::type >::Flags, + Flags = Flags0 & ~NestByRefBit + }; }; } @@ -149,6 +154,11 @@ struct traits > : public traits::type > { typedef MatrixXpr XprKind; + // Let's remove NestByRefBit + enum { + Flags0 = traits::type >::Flags, + Flags = Flags0 & ~NestByRefBit + }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index c5800f6c8..04862f374 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -462,8 +462,10 @@ template class DenseBase template RealScalar lpNorm() const; template - const Replicate replicate() const; - const Replicate replicate(Index rowFacor,Index colFactor) const; + inline const Replicate replicate() const; + + typedef Replicate ReplicateReturnType; + inline const ReplicateReturnType replicate(Index rowFacor,Index colFactor) const; typedef Reverse ReverseReturnType; typedef const Reverse ConstReverseReturnType; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h index aab8007b3..68cf6d4b0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h @@ -190,18 +190,18 @@ MatrixBase::diagonal() const * * \sa MatrixBase::diagonal(), class Diagonal */ template -inline typename MatrixBase::template DiagonalIndexReturnType::Type +inline typename MatrixBase::DiagonalDynamicIndexReturnType MatrixBase::diagonal(Index index) { - return typename DiagonalIndexReturnType::Type(derived(), index); + return DiagonalDynamicIndexReturnType(derived(), index); } /** This is the const version of diagonal(Index). */ template -inline typename MatrixBase::template ConstDiagonalIndexReturnType::Type +inline typename MatrixBase::ConstDiagonalDynamicIndexReturnType MatrixBase::diagonal(Index index) const { - return typename ConstDiagonalIndexReturnType::Type(derived(), index); + return ConstDiagonalDynamicIndexReturnType(derived(), index); } /** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h index 2a59d9464..9e805a80f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h @@ -232,7 +232,7 @@ EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest& // FIXME not very good if rhs is real and lhs complex while alpha is real too const Index cols = dest.cols(); for (Index j=0; j diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index ab50c9b81..cebed2bb6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -168,6 +168,7 @@ template class MapBase template class MapBase : public MapBase { + typedef MapBase ReadOnlyMapBase; public: typedef MapBase Base; @@ -230,11 +231,13 @@ template class MapBase Derived& operator=(const MapBase& other) { - Base::Base::operator=(other); + ReadOnlyMapBase::Base::operator=(other); return derived(); } - using Base::Base::operator=; + // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base, + // see bugs 821 and 920. + using ReadOnlyMapBase::Base::operator=; }; #undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index 344b38f2f..cc3279746 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -215,7 +215,7 @@ template class MatrixBase typedef Diagonal DiagonalReturnType; DiagonalReturnType diagonal(); - typedef typename internal::add_const >::type ConstDiagonalReturnType; + typedef typename internal::add_const >::type ConstDiagonalReturnType; ConstDiagonalReturnType diagonal() const; template struct DiagonalIndexReturnType { typedef Diagonal Type; }; @@ -223,16 +223,12 @@ template class MatrixBase template typename DiagonalIndexReturnType::Type diagonal(); template typename ConstDiagonalIndexReturnType::Type diagonal() const; + + typedef Diagonal DiagonalDynamicIndexReturnType; + typedef typename internal::add_const >::type ConstDiagonalDynamicIndexReturnType; - // Note: The "MatrixBase::" prefixes are added to help MSVC9 to match these declarations with the later implementations. - // On the other hand they confuse MSVC8... - #if (defined _MSC_VER) && (_MSC_VER >= 1500) // 2008 or later - typename MatrixBase::template DiagonalIndexReturnType::Type diagonal(Index index); - typename MatrixBase::template ConstDiagonalIndexReturnType::Type diagonal(Index index) const; - #else - typename DiagonalIndexReturnType::Type diagonal(Index index); - typename ConstDiagonalIndexReturnType::Type diagonal(Index index) const; - #endif + DiagonalDynamicIndexReturnType diagonal(Index index); + ConstDiagonalDynamicIndexReturnType diagonal(Index index) const; #ifdef EIGEN2_SUPPORT template typename internal::eigen2_part_return_type::type part(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 1297b8413..f26f3e5cc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -555,7 +555,10 @@ struct permut_matrix_product_retval const Index n = Side==OnTheLeft ? rows() : cols(); // FIXME we need an is_same for expression that is not sensitive to constness. For instance // is_same_xpr, Block >::value should be true. - if(is_same::value && extract_data(dst) == extract_data(m_matrix)) + if( is_same::value + && blas_traits::HasUsableDirectAccess + && blas_traits::HasUsableDirectAccess + && extract_data(dst) == extract_data(m_matrix)) { // apply the permutation inplace Matrix mask(m_permutation.size()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h index a494b5f87..cf74470a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h @@ -85,7 +85,14 @@ class ProductBase : public MatrixBase public: +#ifndef EIGEN_NO_MALLOC + typedef typename Base::PlainObject BasePlainObject; + typedef Matrix DynPlainObject; + typedef typename internal::conditional<(BasePlainObject::SizeAtCompileTime==Dynamic) || (BasePlainObject::SizeAtCompileTime*int(sizeof(Scalar)) < int(EIGEN_STACK_ALLOCATION_LIMIT)), + BasePlainObject, DynPlainObject>::type PlainObject; +#else typedef typename Base::PlainObject PlainObject; +#endif ProductBase(const Lhs& a_lhs, const Rhs& a_rhs) : m_lhs(a_lhs), m_rhs(a_rhs) @@ -180,7 +187,12 @@ namespace internal { template struct nested, N, PlainObject> { - typedef PlainObject const& type; + typedef typename GeneralProduct::PlainObject const& type; +}; +template +struct nested, N, PlainObject> +{ + typedef typename GeneralProduct::PlainObject const& type; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index cd6d949c4..df87b1af4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -188,6 +188,8 @@ template class Ref : public RefBase > { typedef internal::traits Traits; + template + inline Ref(const PlainObjectBase& expr); public: typedef RefBase Base; @@ -196,20 +198,21 @@ template class Ref #ifndef EIGEN_PARSED_BY_DOXYGEN template - inline Ref(PlainObjectBase& expr, - typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) + inline Ref(PlainObjectBase& expr) { - Base::construct(expr); + EIGEN_STATIC_ASSERT(static_cast(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + Base::construct(expr.derived()); } template - inline Ref(const DenseBase& expr, - typename internal::enable_if::value&&bool(Traits::template match::MatchAtCompileTime)),Derived>::type* = 0, - int = Derived::ThisConstantIsPrivateInPlainObjectBase) + inline Ref(const DenseBase& expr) #else template inline Ref(DenseBase& expr) #endif { + EIGEN_STATIC_ASSERT(static_cast(internal::is_lvalue::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY); + EIGEN_STATIC_ASSERT(static_cast(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); + enum { THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY = Derived::ThisConstantIsPrivateInPlainObjectBase}; Base::construct(expr.const_cast_derived()); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h index dde86a834..ac4537c14 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h @@ -135,7 +135,7 @@ template class Replicate */ template template -inline const Replicate +const Replicate DenseBase::replicate() const { return Replicate(derived()); @@ -150,7 +150,7 @@ DenseBase::replicate() const * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate */ template -inline const Replicate +const typename DenseBase::ReplicateReturnType DenseBase::replicate(Index rowFactor,Index colFactor) const { return Replicate(derived(),rowFactor,colFactor); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h index 845ae1aec..4d65392c6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h @@ -380,19 +380,19 @@ template class TriangularView EIGEN_STRONG_INLINE TriangularView& operator=(const ProductBase& other) { setZero(); - return assignProduct(other,1); + return assignProduct(other.derived(),1); } template EIGEN_STRONG_INLINE TriangularView& operator+=(const ProductBase& other) { - return assignProduct(other,1); + return assignProduct(other.derived(),1); } template EIGEN_STRONG_INLINE TriangularView& operator-=(const ProductBase& other) { - return assignProduct(other,-1); + return assignProduct(other.derived(),-1); } @@ -400,25 +400,34 @@ template class TriangularView EIGEN_STRONG_INLINE TriangularView& operator=(const ScaledProduct& other) { setZero(); - return assignProduct(other,other.alpha()); + return assignProduct(other.derived(),other.alpha()); } template EIGEN_STRONG_INLINE TriangularView& operator+=(const ScaledProduct& other) { - return assignProduct(other,other.alpha()); + return assignProduct(other.derived(),other.alpha()); } template EIGEN_STRONG_INLINE TriangularView& operator-=(const ScaledProduct& other) { - return assignProduct(other,-other.alpha()); + return assignProduct(other.derived(),-other.alpha()); } protected: template EIGEN_STRONG_INLINE TriangularView& assignProduct(const ProductBase& prod, const Scalar& alpha); + + template + EIGEN_STRONG_INLINE TriangularView& assignProduct(const TriangularProduct& prod, const Scalar& alpha) + { + lazyAssign(alpha*prod.eval()); + return *this; + } MatrixTypeNested m_matrix; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h index f183d31de..8d9255eef 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h @@ -110,7 +110,7 @@ template<> EIGEN_STRONG_INLINE Packet2cf ploaddup(const std::complex< template<> EIGEN_STRONG_INLINE void pstore >(std::complex * to, const Packet2cf& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((float*)to, from.v); } template<> EIGEN_STRONG_INLINE void pstoreu >(std::complex * to, const Packet2cf& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((float*)to, from.v); } -template<> EIGEN_STRONG_INLINE void prefetch >(const std::complex * addr) { __pld((float *)addr); } +template<> EIGEN_STRONG_INLINE void prefetch >(const std::complex * addr) { EIGEN_ARM_PREFETCH((float *)addr); } template<> EIGEN_STRONG_INLINE std::complex pfirst(const Packet2cf& a) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h index 163bac215..94dfab330 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h @@ -48,9 +48,18 @@ typedef uint32x4_t Packet4ui; #define EIGEN_INIT_NEON_PACKET2(X, Y) {X, Y} #define EIGEN_INIT_NEON_PACKET4(X, Y, Z, W) {X, Y, Z, W} #endif - -#ifndef __pld -#define __pld(x) asm volatile ( " pld [%[addr]]\n" :: [addr] "r" (x) : "cc" ); + +// arm64 does have the pld instruction. If available, let's trust the __builtin_prefetch built-in function +// which available on LLVM and GCC (at least) +#if EIGEN_HAS_BUILTIN(__builtin_prefetch) || defined(__GNUC__) + #define EIGEN_ARM_PREFETCH(ADDR) __builtin_prefetch(ADDR); +#elif defined __pld + #define EIGEN_ARM_PREFETCH(ADDR) __pld(ADDR) +#elif !defined(__aarch64__) + #define EIGEN_ARM_PREFETCH(ADDR) __asm__ __volatile__ ( " pld [%[addr]]\n" :: [addr] "r" (ADDR) : "cc" ); +#else + // by default no explicit prefetching + #define EIGEN_ARM_PREFETCH(ADDR) #endif template<> struct packet_traits : default_packet_traits @@ -209,8 +218,8 @@ template<> EIGEN_STRONG_INLINE void pstore(int* to, const Packet4i& f template<> EIGEN_STRONG_INLINE void pstoreu(float* to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to, from); } template<> EIGEN_STRONG_INLINE void pstoreu(int* to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to, from); } -template<> EIGEN_STRONG_INLINE void prefetch(const float* addr) { __pld(addr); } -template<> EIGEN_STRONG_INLINE void prefetch(const int* addr) { __pld(addr); } +template<> EIGEN_STRONG_INLINE void prefetch(const float* addr) { EIGEN_ARM_PREFETCH(addr); } +template<> EIGEN_STRONG_INLINE void prefetch(const int* addr) { EIGEN_ARM_PREFETCH(addr); } // FIXME only store the 2 first elements ? template<> EIGEN_STRONG_INLINE float pfirst(const Packet4f& a) { float EIGEN_ALIGN16 x[4]; vst1q_f32(x, a); return x[0]; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h index 99cbd0d95..d16f30bb0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h @@ -52,7 +52,7 @@ Packet4f plog(const Packet4f& _x) Packet4i emm0; - Packet4f invalid_mask = _mm_cmplt_ps(x, _mm_setzero_ps()); + Packet4f invalid_mask = _mm_cmpnge_ps(x, _mm_setzero_ps()); // not greater equal is true if x is NaN Packet4f iszero_mask = _mm_cmpeq_ps(x, _mm_setzero_ps()); x = pmax(x, p4f_min_norm_pos); /* cut off denormalized stuff */ @@ -166,7 +166,7 @@ Packet4f pexp(const Packet4f& _x) emm0 = _mm_cvttps_epi32(fx); emm0 = _mm_add_epi32(emm0, p4i_0x7f); emm0 = _mm_slli_epi32(emm0, 23); - return pmul(y, _mm_castsi128_ps(emm0)); + return pmax(pmul(y, Packet4f(_mm_castsi128_ps(emm0))), _x); } template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2d pexp(const Packet2d& _x) @@ -239,7 +239,7 @@ Packet2d pexp(const Packet2d& _x) emm0 = _mm_add_epi32(emm0, p4i_1023_0); emm0 = _mm_slli_epi32(emm0, 20); emm0 = _mm_shuffle_epi32(emm0, _MM_SHUFFLE(1,2,0,3)); - return pmul(x, _mm_castsi128_pd(emm0)); + return pmax(pmul(x, Packet2d(_mm_castsi128_pd(emm0))), _x); } /* evaluation of 4 sines at onces, using SSE2 intrinsics. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h index c06a0df1c..421f925e1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h @@ -90,6 +90,7 @@ struct traits > | (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0), CoeffReadCost = InnerSize == Dynamic ? Dynamic + : InnerSize == 0 ? 0 : InnerSize * (NumTraits::MulCost + LhsCoeffReadCost + RhsCoeffReadCost) + (InnerSize - 1) * NumTraits::AddCost, @@ -133,7 +134,7 @@ class CoeffBasedProduct }; typedef internal::product_coeff_impl ScalarCoeffImpl; typedef CoeffBasedProduct LazyCoeffBasedProductType; @@ -184,7 +185,7 @@ class CoeffBasedProduct { PacketScalar res; internal::product_packet_impl ::run(row, col, m_lhs, m_rhs, res); return res; @@ -262,10 +263,7 @@ struct product_coeff_impl typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar& res) { - eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix"); - res = lhs.coeff(row, 0) * rhs.coeff(0, col); - for(Index i = 1; i < lhs.cols(); ++i) - res += lhs.coeff(row, i) * rhs.coeff(i, col); + res = (lhs.row(row).transpose().cwiseProduct( rhs.col(col) )).sum(); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index 3a010ec6a..6d1e6c133 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h @@ -13,7 +13,7 @@ #define EIGEN_WORLD_VERSION 3 #define EIGEN_MAJOR_VERSION 2 -#define EIGEN_MINOR_VERSION 2 +#define EIGEN_MINOR_VERSION 4 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -96,6 +96,13 @@ #define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t #endif +// Cross compiler wrapper around LLVM's __has_builtin +#ifdef __has_builtin +# define EIGEN_HAS_BUILTIN(x) __has_builtin(x) +#else +# define EIGEN_HAS_BUILTIN(x) 0 +#endif + /** Allows to disable some optimizations which might affect the accuracy of the result. * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them. * They currently include: @@ -247,7 +254,7 @@ namespace Eigen { #if !defined(EIGEN_ASM_COMMENT) #if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) ) - #define EIGEN_ASM_COMMENT(X) asm("#" X) + #define EIGEN_ASM_COMMENT(X) __asm__("#" X) #else #define EIGEN_ASM_COMMENT(X) #endif @@ -306,7 +313,7 @@ namespace Eigen { // just an empty macro ! #define EIGEN_EMPTY -#if defined(_MSC_VER) && (!defined(__INTEL_COMPILER)) +#if defined(_MSC_VER) && (_MSC_VER < 1900) && (!defined(__INTEL_COMPILER)) #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \ using Base::operator =; #elif defined(__clang__) // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index 6c2461725..41dd7db06 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -63,7 +63,7 @@ // Currently, let's include it only on unix systems: #if defined(__unix__) || defined(__unix) #include - #if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0) + #if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || (defined __PGI) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0) #define EIGEN_HAS_POSIX_MEMALIGN 1 #endif #endif @@ -417,6 +417,8 @@ template inline T* conditional_aligned_realloc_new(T* pt template inline T* conditional_aligned_new_auto(size_t size) { + if(size==0) + return 0; // short-cut. Also fixes Bug 884 check_size_for_overflow(size); T *result = reinterpret_cast(conditional_aligned_malloc(sizeof(T)*size)); if(NumTraits::RequireInitialization) @@ -464,9 +466,8 @@ template inline void conditional_aligned_delete_auto(T * template static inline Index first_aligned(const Scalar* array, Index size) { - enum { PacketSize = packet_traits::size, - PacketAlignedMask = PacketSize-1 - }; + static const Index PacketSize = packet_traits::size; + static const Index PacketAlignedMask = PacketSize-1; if(PacketSize==1) { @@ -612,7 +613,6 @@ template class aligned_stack_memory_handler void* operator new(size_t size, const std::nothrow_t&) throw() { \ try { return Eigen::internal::conditional_aligned_malloc(size); } \ catch (...) { return 0; } \ - return 0; \ } #else #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h index 8872c5b64..bac5d9fe9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h @@ -90,7 +90,9 @@ YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED, THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE, THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH, - OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG + OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG, + IMPLICIT_CONVERSION_TO_SCALAR_IS_FOR_INNER_PRODUCT_ONLY, + STORAGE_LAYOUT_DOES_NOT_MATCH }; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h index 3c4773054..781965d2c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h @@ -341,7 +341,7 @@ template::type> str }; template -T* const_cast_ptr(const T* ptr) +inline T* const_cast_ptr(const T* ptr) { return const_cast(ptr); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h index 0e6fdb488..7992d4944 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h @@ -147,7 +147,6 @@ void fitHyperplane(int numPoints, // compute the covariance matrix CovMatrixType covMat = CovMatrixType::Zero(size, size); - VectorType remean = VectorType::Zero(size); for(int i = 0; i < numPoints; ++i) { VectorType diff = (*(points[i]) - mean).conjugate(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h index 5706eeebe..4f36091db 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h @@ -313,7 +313,7 @@ namespace Eigen { using std::abs; using std::sqrt; const Index dim=m_S.cols(); - if (abs(m_S.coeff(i+1,i)==Scalar(0))) + if (abs(m_S.coeff(i+1,i))==Scalar(0)) return; Index z = findSmallDiagEntry(i,i+1); if (z==i-1) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h index 3993046a8..be89de4a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -563,7 +563,6 @@ template struct direct_selfadjoint_eigenvalues::epsilon(); - safeNorm2 *= safeNorm2; if((eivals(2)-eivals(0))<=Eigen::NumTraits::epsilon()) { eivecs.setIdentity(); @@ -577,7 +576,7 @@ template struct direct_selfadjoint_eigenvalues d1 ? 2 : 0; - d0 = d0 > d1 ? d1 : d0; + d0 = d0 > d1 ? d0 : d1; tmp.diagonal().array () -= eivals(k); VectorType cross; @@ -585,19 +584,25 @@ template struct direct_selfadjoint_eigenvaluessafeNorm2) + { eivecs.col(k) = cross / sqrt(n); + } else { n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm(); if(n>safeNorm2) + { eivecs.col(k) = cross / sqrt(n); + } else { n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm(); if(n>safeNorm2) + { eivecs.col(k) = cross / sqrt(n); + } else { // the input matrix and/or the eigenvaues probably contains some inf/NaN, @@ -617,12 +622,16 @@ template struct direct_selfadjoint_eigenvalues::epsilon()) + { eivecs.col(1) = eivecs.col(k).unitOrthogonal(); + } else { - n = (cross = eivecs.col(k).cross(tmp.row(0).normalized())).squaredNorm(); + n = (cross = eivecs.col(k).cross(tmp.row(0))).squaredNorm(); if(n>safeNorm2) + { eivecs.col(1) = cross / sqrt(n); + } else { n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm(); @@ -636,13 +645,14 @@ template struct direct_selfadjoint_eigenvalues::epsilon()) + { + Matrix m; m << v0.transpose(), v1.transpose(); + JacobiSVD > svd(m, ComputeFullV); + result.normal() = svd.matrixV().col(2); + } + else + result.normal() /= norm; result.offset() = -p0.dot(result.normal()); return result; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h index 1cac343a5..a2d59fce1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h @@ -60,6 +60,9 @@ public: /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ inline Rotation2D(const Scalar& a) : m_angle(a) {} + + /** Default constructor wihtout initialization. The represented rotation is undefined. */ + Rotation2D() {} /** \returns the rotation angle */ inline Scalar angle() const { return m_angle; } @@ -81,10 +84,10 @@ public: /** Applies the rotation to a 2D vector */ Vector2 operator* (const Vector2& vec) const { return toRotationMatrix() * vec; } - + template Rotation2D& fromRotationMatrix(const MatrixBase& m); - Matrix2 toRotationMatrix(void) const; + Matrix2 toRotationMatrix() const; /** \returns the spherical interpolation between \c *this and \a other using * parameter \a t. It is in fact equivalent to a linear interpolation. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 56f361566..e786e5356 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -62,6 +62,8 @@ struct transform_construct_from_matrix; template struct transform_take_affine_part; +template struct transform_make_affine; + } // end namespace internal /** \geometry_module \ingroup Geometry_Module @@ -230,8 +232,7 @@ public: inline Transform() { check_template_params(); - if (int(Mode)==Affine) - makeAffine(); + internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix); } inline Transform(const Transform& other) @@ -591,11 +592,7 @@ public: */ void makeAffine() { - if(int(Mode)!=int(AffineCompact)) - { - matrix().template block<1,Dim>(Dim,0).setZero(); - matrix().coeffRef(Dim,Dim) = Scalar(1); - } + internal::transform_make_affine::run(m_matrix); } /** \internal @@ -1079,6 +1076,24 @@ Transform::fromPositionOrientationScale(const MatrixBas namespace internal { +template +struct transform_make_affine +{ + template + static void run(MatrixType &mat) + { + static const int Dim = MatrixType::ColsAtCompileTime-1; + mat.template block<1,Dim>(Dim,0).setZero(); + mat.coeffRef(Dim,Dim) = typename MatrixType::Scalar(1); + } +}; + +template<> +struct transform_make_affine +{ + template static void run(MatrixType &) { } +}; + // selector needed to avoid taking the inverse of a 3x4 matrix template struct projective_transform_inverse diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h index 2b9fb7f88..dd135c21f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h @@ -39,7 +39,6 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, int maxIters = iters; int n = mat.cols(); - x = precond.solve(x); VectorType r = rhs - mat * x; VectorType r0 = r; @@ -143,7 +142,7 @@ struct traits > * SparseMatrix A(n,n); * // fill A and b * BiCGSTAB > solver; - * solver(A); + * solver.compute(A); * x = solver.solve(b); * std::cout << "#iterations: " << solver.iterations() << std::endl; * std::cout << "estimated error: " << solver.error() << std::endl; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h index 1c48f0df7..18cd7d88a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h @@ -219,7 +219,7 @@ class PardisoImpl void pardisoInit(int type) { m_type = type; - bool symmetric = abs(m_type) < 10; + bool symmetric = std::abs(m_type) < 10; m_iparm[0] = 1; // No solver default m_iparm[1] = 3; // use Metis for the ordering m_iparm[2] = 1; // Numbers of processors, value of OMP_NUM_THREADS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index dff9e44eb..c57f2974c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -762,6 +762,7 @@ template class JacobiSVD internal::qr_preconditioner_impl m_qr_precond_morecols; internal::qr_preconditioner_impl m_qr_precond_morerows; + MatrixType m_scaledMatrix; }; template @@ -808,8 +809,9 @@ void JacobiSVD::allocate(Index rows, Index cols, u : 0); m_workMatrix.resize(m_diagSize, m_diagSize); - if(m_cols>m_rows) m_qr_precond_morecols.allocate(*this); - if(m_rows>m_cols) m_qr_precond_morerows.allocate(*this); + if(m_cols>m_rows) m_qr_precond_morecols.allocate(*this); + if(m_rows>m_cols) m_qr_precond_morerows.allocate(*this); + if(m_cols!=m_cols) m_scaledMatrix.resize(rows,cols); } template @@ -826,21 +828,26 @@ JacobiSVD::compute(const MatrixType& matrix, unsig // limit for very small denormal numbers to be considered zero in order to avoid infinite loops (see bug 286) const RealScalar considerAsZero = RealScalar(2) * std::numeric_limits::denorm_min(); + // Scaling factor to reduce over/under-flows + RealScalar scale = matrix.cwiseAbs().maxCoeff(); + if(scale==RealScalar(0)) scale = RealScalar(1); + /*** step 1. The R-SVD step: we use a QR decomposition to reduce to the case of a square matrix */ - if(!m_qr_precond_morecols.run(*this, matrix) && !m_qr_precond_morerows.run(*this, matrix)) + if(m_rows!=m_cols) { - m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize); + m_scaledMatrix = matrix / scale; + m_qr_precond_morecols.run(*this, m_scaledMatrix); + m_qr_precond_morerows.run(*this, m_scaledMatrix); + } + else + { + m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize) / scale; if(m_computeFullU) m_matrixU.setIdentity(m_rows,m_rows); if(m_computeThinU) m_matrixU.setIdentity(m_rows,m_diagSize); if(m_computeFullV) m_matrixV.setIdentity(m_cols,m_cols); if(m_computeThinV) m_matrixV.setIdentity(m_cols, m_diagSize); } - - // Scaling factor to reduce over/under-flows - RealScalar scale = m_workMatrix.cwiseAbs().maxCoeff(); - if(scale==RealScalar(0)) scale = RealScalar(1); - m_workMatrix /= scale; /*** step 2. The main Jacobi SVD iteration. ***/ @@ -861,7 +868,8 @@ JacobiSVD::compute(const MatrixType& matrix, unsig using std::max; RealScalar threshold = (max)(considerAsZero, precision * (max)(abs(m_workMatrix.coeff(p,p)), abs(m_workMatrix.coeff(q,q)))); - if((max)(abs(m_workMatrix.coeff(p,q)),abs(m_workMatrix.coeff(q,p))) > threshold) + // We compare both values to threshold instead of calling max to be robust to NaN (See bug 791) + if(abs(m_workMatrix.coeff(p,q))>threshold || abs(m_workMatrix.coeff(q,p)) > threshold) { finished = false; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h index 17fff96a7..220c6451c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h @@ -69,7 +69,7 @@ class AmbiVector delete[] m_buffer; if (size<1000) { - Index allocSize = (size * sizeof(ListEl))/sizeof(Scalar); + Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1)/sizeof(Scalar); m_allocatedElements = (allocSize*sizeof(Scalar))/sizeof(ListEl); m_buffer = new Scalar[allocSize]; } @@ -88,7 +88,7 @@ class AmbiVector Index copyElements = m_allocatedElements; m_allocatedElements = (std::min)(Index(m_allocatedElements*1.5),m_size); Index allocSize = m_allocatedElements * sizeof(ListEl); - allocSize = allocSize/sizeof(Scalar) + (allocSize%sizeof(Scalar)>0?1:0); + allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar); Scalar* newBuffer = new Scalar[allocSize]; memcpy(newBuffer, m_buffer, copyElements * sizeof(ListEl)); delete[] m_buffer; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 16a20a574..0ede034ba 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -68,6 +68,8 @@ public: const internal::variable_if_dynamic m_outerSize; EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + private: + Index nonZeros() const; }; @@ -82,6 +84,7 @@ class BlockImpl,BlockRows,BlockCols,true typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; typedef typename internal::remove_all::type _MatrixTypeNested; typedef Block BlockType; + typedef Block ConstBlockType; public: enum { IsRowMajor = internal::traits::IsRowMajor }; EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) @@ -245,6 +248,93 @@ public: }; + +template +class BlockImpl,BlockRows,BlockCols,true,Sparse> + : public SparseMatrixBase,BlockRows,BlockCols,true> > +{ + typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef Block BlockType; +public: + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) +protected: + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + + class InnerIterator: public SparseMatrixType::InnerIterator + { + public: + inline InnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator + { + public: + inline ReverseInnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline BlockImpl(const SparseMatrixType& xpr, int i) + : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) + {} + + inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) + {} + + inline const Scalar* valuePtr() const + { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* innerIndexPtr() const + { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* outerIndexPtr() const + { return m_matrix.outerIndexPtr() + m_outerStart; } + + Index nonZeros() const + { + if(m_matrix.isCompressed()) + return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) + - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); + else if(m_outerSize.value()==0) + return 0; + else + return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); + } + + const Scalar& lastCoeff() const + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); + eigen_assert(nonZeros()>0); + if(m_matrix.isCompressed()) + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; + else + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + typename SparseMatrixType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; + +}; + //---------- /** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h index 78411db98..a9084192e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h @@ -306,15 +306,6 @@ class DenseTimeSparseProduct DenseTimeSparseProduct& operator=(const DenseTimeSparseProduct&); }; -// sparse * dense -template -template -inline const typename SparseDenseProductReturnType::Type -SparseMatrixBase::operator*(const MatrixBase &other) const -{ - return typename SparseDenseProductReturnType::Type(derived(), other.derived()); -} - } // end namespace Eigen #endif // EIGEN_SPARSEDENSEPRODUCT_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h index bbcf7fb1c..485e85bec 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h @@ -358,7 +358,8 @@ template class SparseMatrixBase : public EigenBase /** sparse * dense (returns a dense object unless it is an outer product) */ template const typename SparseDenseProductReturnType::Type - operator*(const MatrixBase &other) const; + operator*(const MatrixBase &other) const + { return typename SparseDenseProductReturnType::Type(derived(), other.derived()); } /** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */ SparseSymmetricPermutationProduct twistedBy(const PermutationMatrix& perm) const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h index b85be93f6..75e210009 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h @@ -61,7 +61,7 @@ struct permut_sparsematrix_product_retval for(Index j=0; jcols(); ++j) { for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) { - if(it.row() < j) continue; - if(it.row() == j) + if(it.index() == j) { det *= (std::abs)(it.value()); break; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h index ad6f2183f..b16afd6a4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h @@ -189,8 +189,8 @@ class MappedSuperNodalMatrix::InnerIterator m_idval(mat.colIndexPtr()[outer]), m_startidval(m_idval), m_endidval(mat.colIndexPtr()[outer+1]), - m_idrow(mat.rowIndexPtr()[outer]), - m_endidrow(mat.rowIndexPtr()[outer+1]) + m_idrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]]), + m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]+1]) {} inline InnerIterator& operator++() { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h index 4c6553bf2..a00bd5db1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h @@ -75,7 +75,7 @@ class SparseQR typedef Matrix ScalarVector; typedef PermutationMatrix PermutationType; public: - SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false) + SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false) { } /** Construct a QR factorization of the matrix \a mat. @@ -84,7 +84,7 @@ class SparseQR * * \sa compute() */ - SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false) + SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false) { compute(mat); } @@ -262,6 +262,7 @@ class SparseQR IndexVector m_etree; // Column elimination tree IndexVector m_firstRowElt; // First element in each row bool m_isQSorted; // whether Q is sorted or not + bool m_isEtreeOk; // whether the elimination tree match the initial input matrix template friend struct SparseQR_QProduct; template friend struct SparseQRMatrixQReturnType; @@ -281,9 +282,11 @@ template void SparseQR::analyzePattern(const MatrixType& mat) { eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR"); + // Copy to a column major matrix if the input is rowmajor + typename internal::conditional::type matCpy(mat); // Compute the column fill reducing ordering OrderingType ord; - ord(mat, m_perm_c); + ord(matCpy, m_perm_c); Index n = mat.cols(); Index m = mat.rows(); Index diagSize = (std::min)(m,n); @@ -296,7 +299,8 @@ void SparseQR::analyzePattern(const MatrixType& mat) // Compute the column elimination tree of the permuted matrix m_outputPerm_c = m_perm_c.inverse(); - internal::coletree(mat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + m_isEtreeOk = true; m_R.resize(m, n); m_Q.resize(m, diagSize); @@ -330,15 +334,38 @@ void SparseQR::factorize(const MatrixType& mat) Index nzcolR, nzcolQ; // Number of nonzero for the current column of R and Q ScalarVector tval(m); // The dense vector used to compute the current column RealScalar pivotThreshold = m_threshold; - + + m_R.setZero(); + m_Q.setZero(); m_pmat = mat; - m_pmat.uncompress(); // To have the innerNonZeroPtr allocated - // Apply the fill-in reducing permutation lazily: - for (int i = 0; i < n; i++) + if(!m_isEtreeOk) { - Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i; - m_pmat.outerIndexPtr()[p] = mat.outerIndexPtr()[i]; - m_pmat.innerNonZeroPtr()[p] = mat.outerIndexPtr()[i+1] - mat.outerIndexPtr()[i]; + m_outputPerm_c = m_perm_c.inverse(); + internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); + m_isEtreeOk = true; + } + + m_pmat.uncompress(); // To have the innerNonZeroPtr allocated + + // Apply the fill-in reducing permutation lazily: + { + // If the input is row major, copy the original column indices, + // otherwise directly use the input matrix + // + IndexVector originalOuterIndicesCpy; + const Index *originalOuterIndices = mat.outerIndexPtr(); + if(MatrixType::IsRowMajor) + { + originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1); + originalOuterIndices = originalOuterIndicesCpy.data(); + } + + for (int i = 0; i < n; i++) + { + Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i; + m_pmat.outerIndexPtr()[p] = originalOuterIndices[i]; + m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i]; + } } /* Compute the default threshold as in MatLab, see: @@ -349,6 +376,8 @@ void SparseQR::factorize(const MatrixType& mat) { RealScalar max2Norm = 0.0; for (int j = 0; j < n; j++) max2Norm = (max)(max2Norm, m_pmat.col(j).norm()); + if(max2Norm==RealScalar(0)) + max2Norm = RealScalar(1); pivotThreshold = 20 * (m + n) * max2Norm * NumTraits::epsilon(); } @@ -357,7 +386,7 @@ void SparseQR::factorize(const MatrixType& mat) Index nonzeroCol = 0; // Record the number of valid pivots m_Q.startVec(0); - + // Left looking rank-revealing QR factorization: compute a column of R and Q at a time for (Index col = 0; col < n; ++col) { @@ -373,7 +402,7 @@ void SparseQR::factorize(const MatrixType& mat) // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k. // Note: if the diagonal entry does not exist, then its contribution must be explicitly added, // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found. - for (typename MatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp) + for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp) { Index curIdx = nonzeroCol; if(itp) curIdx = itp.row(); @@ -447,7 +476,7 @@ void SparseQR::factorize(const MatrixType& mat) } } // End update current column - Scalar tau; + Scalar tau = 0; RealScalar beta = 0; if(nonzeroCol < diagSize) @@ -461,7 +490,6 @@ void SparseQR::factorize(const MatrixType& mat) for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq))); if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0)) { - tau = RealScalar(0); beta = numext::real(c0); tval(Qidx(0)) = 1; } @@ -514,6 +542,7 @@ void SparseQR::factorize(const MatrixType& mat) // Recompute the column elimination tree internal::coletree(m_pmat, m_etree, m_firstRowElt, m_pivotperm.indices().data()); + m_isEtreeOk = false; } } @@ -525,13 +554,13 @@ void SparseQR::factorize(const MatrixType& mat) m_R.finalize(); m_R.makeCompressed(); m_isQSorted = false; - + m_nonzeropivots = nonzeroCol; if(nonzeroCol *Mx, double *Ex, void *N return umfpack_zi_get_determinant(&mx_real,0,Ex,NumericHandle,User_Info); } +namespace internal { + template struct umfpack_helper_is_sparse_plain : false_type {}; + template + struct umfpack_helper_is_sparse_plain > + : true_type {}; + template + struct umfpack_helper_is_sparse_plain > + : true_type {}; +} + /** \ingroup UmfPackSupport_Module * \brief A sparse LU factorization and solver based on UmfPack * @@ -192,10 +202,14 @@ class UmfPackLU : internal::noncopyable * Note that the matrix should be column-major, and in compressed format for best performance. * \sa SparseMatrix::makeCompressed(). */ - void compute(const MatrixType& matrix) + template + void compute(const InputMatrixType& matrix) { - analyzePattern(matrix); - factorize(matrix); + if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar()); + if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar()); + grapInput(matrix.derived()); + analyzePattern_impl(); + factorize_impl(); } /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A. @@ -230,23 +244,15 @@ class UmfPackLU : internal::noncopyable * * \sa factorize(), compute() */ - void analyzePattern(const MatrixType& matrix) + template + void analyzePattern(const InputMatrixType& matrix) { - if(m_symbolic) - umfpack_free_symbolic(&m_symbolic,Scalar()); - if(m_numeric) - umfpack_free_numeric(&m_numeric,Scalar()); + if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar()); + if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar()); - grapInput(matrix); + grapInput(matrix.derived()); - int errorCode = 0; - errorCode = umfpack_symbolic(matrix.rows(), matrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, - &m_symbolic, 0, 0); - - m_isInitialized = true; - m_info = errorCode ? InvalidInput : Success; - m_analysisIsOk = true; - m_factorizationIsOk = false; + analyzePattern_impl(); } /** Performs a numeric decomposition of \a matrix @@ -255,20 +261,16 @@ class UmfPackLU : internal::noncopyable * * \sa analyzePattern(), compute() */ - void factorize(const MatrixType& matrix) + template + void factorize(const InputMatrixType& matrix) { eigen_assert(m_analysisIsOk && "UmfPackLU: you must first call analyzePattern()"); if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar()); - grapInput(matrix); - - int errorCode; - errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, - m_symbolic, &m_numeric, 0, 0); - - m_info = errorCode ? NumericalIssue : Success; - m_factorizationIsOk = true; + grapInput(matrix.derived()); + + factorize_impl(); } #ifndef EIGEN_PARSED_BY_DOXYGEN @@ -283,19 +285,20 @@ class UmfPackLU : internal::noncopyable protected: - void init() { - m_info = InvalidInput; - m_isInitialized = false; - m_numeric = 0; - m_symbolic = 0; - m_outerIndexPtr = 0; - m_innerIndexPtr = 0; - m_valuePtr = 0; + m_info = InvalidInput; + m_isInitialized = false; + m_numeric = 0; + m_symbolic = 0; + m_outerIndexPtr = 0; + m_innerIndexPtr = 0; + m_valuePtr = 0; + m_extractedDataAreDirty = true; } - void grapInput(const MatrixType& mat) + template + void grapInput_impl(const InputMatrixType& mat, internal::true_type) { m_copyMatrix.resize(mat.rows(), mat.cols()); if( ((MatrixType::Flags&RowMajorBit)==RowMajorBit) || sizeof(typename MatrixType::Index)!=sizeof(int) || !mat.isCompressed() ) @@ -313,6 +316,45 @@ class UmfPackLU : internal::noncopyable m_valuePtr = mat.valuePtr(); } } + + template + void grapInput_impl(const InputMatrixType& mat, internal::false_type) + { + m_copyMatrix = mat; + m_outerIndexPtr = m_copyMatrix.outerIndexPtr(); + m_innerIndexPtr = m_copyMatrix.innerIndexPtr(); + m_valuePtr = m_copyMatrix.valuePtr(); + } + + template + void grapInput(const InputMatrixType& mat) + { + grapInput_impl(mat, internal::umfpack_helper_is_sparse_plain()); + } + + void analyzePattern_impl() + { + int errorCode = 0; + errorCode = umfpack_symbolic(m_copyMatrix.rows(), m_copyMatrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, + &m_symbolic, 0, 0); + + m_isInitialized = true; + m_info = errorCode ? InvalidInput : Success; + m_analysisIsOk = true; + m_factorizationIsOk = false; + m_extractedDataAreDirty = true; + } + + void factorize_impl() + { + int errorCode; + errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr, + m_symbolic, &m_numeric, 0, 0); + + m_info = errorCode ? NumericalIssue : Success; + m_factorizationIsOk = true; + m_extractedDataAreDirty = true; + } // cached data to reduce reallocation, etc. mutable LUMatrixType m_l; diff --git a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake index d9e22ab1a..80b2841df 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake @@ -452,20 +452,12 @@ macro(ei_set_build_string) endmacro(ei_set_build_string) macro(ei_is_64bit_env VAR) - - file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp" - "int main() { return (sizeof(int*) == 8 ? 1 : 0); } - ") - try_run(run_res compile_res - ${CMAKE_CURRENT_BINARY_DIR} "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp" - RUN_OUTPUT_VARIABLE run_output) - - if(compile_res AND run_res) - set(${VAR} ${run_res}) - elseif(CMAKE_CL_64) - set(${VAR} 1) - elseif("$ENV{Platform}" STREQUAL "X64") # nmake 64 bit + if(CMAKE_SIZEOF_VOID_P EQUAL 8) set(${VAR} 1) + elseif(CMAKE_SIZEOF_VOID_P EQUAL 4) + set(${VAR} 0) + else() + message(WARNING "Unsupported pointer size. Please contact the authors.") endif() endmacro(ei_is_64bit_env) diff --git a/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake b/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake index 7b3046d45..23239c300 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake @@ -86,4 +86,4 @@ include(FindPackageHandleStandardArgs) find_package_handle_standard_args(CHOLMOD DEFAULT_MSG CHOLMOD_INCLUDES CHOLMOD_LIBRARIES) -mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY) +mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY CAMD_LIBRARY CCOLAMD_LIBRARY CHOLMOD_METIS_LIBRARY) diff --git a/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake b/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake index a9e9925e7..6c4dc9ab4 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake @@ -115,5 +115,5 @@ include(FindPackageHandleStandardArgs) find_package_handle_standard_args(FFTW DEFAULT_MSG FFTW_INCLUDES FFTW_LIBRARIES) -mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES) +mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES FFTW_LIB FFTWF_LIB FFTWL_LIB) diff --git a/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake b/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake index 627c3e9ae..e0040d320 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake @@ -10,16 +10,50 @@ find_path(METIS_INCLUDES PATHS $ENV{METISDIR} ${INCLUDE_INSTALL_DIR} - PATH_SUFFIXES + PATH_SUFFIXES + . metis include ) +macro(_metis_check_version) + file(READ "${METIS_INCLUDES}/metis.h" _metis_version_header) + + string(REGEX MATCH "define[ \t]+METIS_VER_MAJOR[ \t]+([0-9]+)" _metis_major_version_match "${_metis_version_header}") + set(METIS_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+METIS_VER_MINOR[ \t]+([0-9]+)" _metis_minor_version_match "${_metis_version_header}") + set(METIS_MINOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+METIS_VER_SUBMINOR[ \t]+([0-9]+)" _metis_subminor_version_match "${_metis_version_header}") + set(METIS_SUBMINOR_VERSION "${CMAKE_MATCH_1}") + if(NOT METIS_MAJOR_VERSION) + message(WARNING "Could not determine Metis version. Assuming version 4.0.0") + set(METIS_VERSION 4.0.0) + else() + set(METIS_VERSION ${METIS_MAJOR_VERSION}.${METIS_MINOR_VERSION}.${METIS_SUBMINOR_VERSION}) + endif() + if(${METIS_VERSION} VERSION_LESS ${Metis_FIND_VERSION}) + set(METIS_VERSION_OK FALSE) + else() + set(METIS_VERSION_OK TRUE) + endif() + + if(NOT METIS_VERSION_OK) + message(STATUS "Metis version ${METIS_VERSION} found in ${METIS_INCLUDES}, " + "but at least version ${Metis_FIND_VERSION} is required") + endif(NOT METIS_VERSION_OK) +endmacro(_metis_check_version) + + if(METIS_INCLUDES AND Metis_FIND_VERSION) + _metis_check_version() + else() + set(METIS_VERSION_OK TRUE) + endif() + find_library(METIS_LIBRARIES metis PATHS $ENV{METISDIR} ${LIB_INSTALL_DIR} PATH_SUFFIXES lib) include(FindPackageHandleStandardArgs) find_package_handle_standard_args(METIS DEFAULT_MSG - METIS_INCLUDES METIS_LIBRARIES) + METIS_INCLUDES METIS_LIBRARIES METIS_VERSION_OK) mark_as_advanced(METIS_INCLUDES METIS_LIBRARIES) diff --git a/gtsam/3rdparty/Eigen/cmake/language_support.cmake b/gtsam/3rdparty/Eigen/cmake/language_support.cmake index d687b71f6..231f7ff70 100644 --- a/gtsam/3rdparty/Eigen/cmake/language_support.cmake +++ b/gtsam/3rdparty/Eigen/cmake/language_support.cmake @@ -33,7 +33,7 @@ function(workaround_9220 language language_works) file(WRITE ${CMAKE_BINARY_DIR}/language_tests/${language}/CMakeLists.txt ${text}) execute_process( - COMMAND ${CMAKE_COMMAND} . + COMMAND ${CMAKE_COMMAND} . -G "${CMAKE_GENERATOR}" WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language} RESULT_VARIABLE return_code OUTPUT_QUIET diff --git a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt index 1e74e0528..b9f497f87 100644 --- a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt +++ b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt @@ -67,10 +67,10 @@ P.rightCols() // P(:, end-cols+1:end) P.rightCols(cols) // P(:, end-cols+1:end) P.topRows() // P(1:rows, :) P.topRows(rows) // P(1:rows, :) -P.middleRows(i) // P(:, i+1:i+rows) -P.middleRows(i, rows) // P(:, i+1:i+rows) -P.bottomRows() // P(:, end-rows+1:end) -P.bottomRows(rows) // P(:, end-rows+1:end) +P.middleRows(i) // P(i+1:i+rows, :) +P.middleRows(i, rows) // P(i+1:i+rows, :) +P.bottomRows() // P(end-rows+1:end, :) +P.bottomRows(rows) // P(end-rows+1:end, :) P.topLeftCorner(rows, cols) // P(1:rows, 1:cols) P.topRightCorner(rows, cols) // P(1:rows, end-cols+1:end) P.bottomLeftCorner(rows, cols) // P(end-rows+1:end, 1:cols) diff --git a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox index 4a33d0cc9..d04ac35c5 100644 --- a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox +++ b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox @@ -71,11 +71,10 @@ i.e either row major or column major. The default is column major. Most arithmet Constant or Random Insertion \code -sm1.setZero(); // Set the matrix with zero elements -sm1.setConstant(val); //Replace all the nonzero values with val +sm1.setZero(); \endcode - The matrix sm1 should have been created before ??? +Remove all non-zero coefficients diff --git a/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt index 71b272a15..08cf8efd7 100644 --- a/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt @@ -6,12 +6,10 @@ foreach(example_src ${examples_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(${example} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - get_target_property(example_executable - ${example} LOCATION) add_custom_command( TARGET ${example} POST_BUILD - COMMAND ${example_executable} + COMMAND ${example} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out ) add_dependencies(all_examples ${example}) diff --git a/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt index 92a22ea61..1135900cf 100644 --- a/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt @@ -14,12 +14,10 @@ foreach(snippet_src ${snippets_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(${compile_snippet_target} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - get_target_property(compile_snippet_executable - ${compile_snippet_target} LOCATION) add_custom_command( TARGET ${compile_snippet_target} POST_BUILD - COMMAND ${compile_snippet_executable} + COMMAND ${compile_snippet_target} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${snippet}.out ) add_dependencies(all_snippets ${compile_snippet_target}) @@ -27,4 +25,4 @@ foreach(snippet_src ${snippets_SRCS}) PROPERTIES OBJECT_DEPENDS ${snippet_src}) endforeach(snippet_src) -ei_add_target_property(compile_tut_arithmetic_transpose_aliasing COMPILE_FLAGS -DEIGEN_NO_DEBUG) \ No newline at end of file +ei_add_target_property(compile_tut_arithmetic_transpose_aliasing COMPILE_FLAGS -DEIGEN_NO_DEBUG) diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt index 0c9b3c3ba..f8a0148c8 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt @@ -1,4 +1,3 @@ - if(NOT EIGEN_TEST_NOQT) find_package(Qt4) if(QT4_FOUND) @@ -6,16 +5,16 @@ if(NOT EIGEN_TEST_NOQT) endif() endif(NOT EIGEN_TEST_NOQT) - if(QT4_FOUND) add_executable(Tutorial_sparse_example Tutorial_sparse_example.cpp Tutorial_sparse_example_details.cpp) target_link_libraries(Tutorial_sparse_example ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) - + add_custom_command( TARGET Tutorial_sparse_example POST_BUILD COMMAND Tutorial_sparse_example ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg ) - + add_dependencies(all_examples Tutorial_sparse_example) endif(QT4_FOUND) + diff --git a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt index 5afa2ac82..5c4860237 100644 --- a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt @@ -26,6 +26,12 @@ ei_add_failtest("block_on_const_type_actually_const_1") ei_add_failtest("transpose_on_const_type_actually_const") ei_add_failtest("diagonal_on_const_type_actually_const") +ei_add_failtest("ref_1") +ei_add_failtest("ref_2") +ei_add_failtest("ref_3") +ei_add_failtest("ref_4") +ei_add_failtest("ref_5") + if (EIGEN_FAILTEST_FAILURE_COUNT) message(FATAL_ERROR "${EIGEN_FAILTEST_FAILURE_COUNT} out of ${EIGEN_FAILTEST_COUNT} failtests FAILED. " diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index ccb0fc798..d32451df6 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -66,7 +66,7 @@ endif() find_package(Pastix) find_package(Scotch) -find_package(Metis) +find_package(Metis 5.0 REQUIRED) if(PASTIX_FOUND AND BLAS_FOUND) add_definitions("-DEIGEN_PASTIX_SUPPORT") include_directories(${PASTIX_INCLUDES}) @@ -279,6 +279,7 @@ ei_add_property(EIGEN_TESTING_SUMMARY "CXX_FLAGS: ${CMAKE_CXX_FLAGS}\n") ei_add_property(EIGEN_TESTING_SUMMARY "Sparse lib flags: ${SPARSE_LIBS}\n") option(EIGEN_TEST_EIGEN2 "Run whole Eigen2 test suite against EIGEN2_SUPPORT" OFF) +mark_as_advanced(EIGEN_TEST_EIGEN2) if(EIGEN_TEST_EIGEN2) add_subdirectory(eigen2) endif() diff --git a/gtsam/3rdparty/Eigen/test/cholesky.cpp b/gtsam/3rdparty/Eigen/test/cholesky.cpp index 64bcbccc4..56885deb7 100644 --- a/gtsam/3rdparty/Eigen/test/cholesky.cpp +++ b/gtsam/3rdparty/Eigen/test/cholesky.cpp @@ -320,33 +320,35 @@ template void cholesky_definiteness(const MatrixType& m) { eigen_assert(m.rows() == 2 && m.cols() == 2); MatrixType mat; + LDLT ldlt(2); + { mat << 1, 0, 0, -1; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } { mat << 1, 2, 2, 1; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } { mat << 0, 0, 0, 0; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(ldlt.isNegative()); VERIFY(ldlt.isPositive()); } { mat << 0, 0, 0, 1; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(!ldlt.isNegative()); VERIFY(ldlt.isPositive()); } { mat << -1, 0, 0, 0; - LDLT ldlt(mat); + ldlt.compute(mat); VERIFY(ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } diff --git a/gtsam/3rdparty/Eigen/test/cwiseop.cpp b/gtsam/3rdparty/Eigen/test/cwiseop.cpp index 247fa2a09..e3361da17 100644 --- a/gtsam/3rdparty/Eigen/test/cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/cwiseop.cpp @@ -9,6 +9,8 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #define EIGEN2_SUPPORT +#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING + #define EIGEN_NO_STATIC_ASSERT #include "main.h" #include diff --git a/gtsam/3rdparty/Eigen/test/dynalloc.cpp b/gtsam/3rdparty/Eigen/test/dynalloc.cpp index 9a6a9d140..7e41bfa97 100644 --- a/gtsam/3rdparty/Eigen/test/dynalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/dynalloc.cpp @@ -87,32 +87,6 @@ template void check_dynaligned() delete obj; } -template void check_custom_new_delete() -{ - { - T* t = new T; - delete t; - } - - { - std::size_t N = internal::random(1,10); - T* t = new T[N]; - delete[] t; - } - -#ifdef EIGEN_ALIGN - { - T* t = static_cast((T::operator new)(sizeof(T))); - (T::operator delete)(t, sizeof(T)); - } - - { - T* t = static_cast((T::operator new)(sizeof(T))); - (T::operator delete)(t); - } -#endif -} - void test_dynalloc() { // low level dynamic memory allocation @@ -128,12 +102,6 @@ void test_dynalloc() CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); - CALL_SUBTEST(check_dynaligned() ); - - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); - CALL_SUBTEST( check_custom_new_delete() ); } // check static allocation, who knows ? diff --git a/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt index 84931e037..9615a6026 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt @@ -4,6 +4,7 @@ add_dependencies(eigen2_check eigen2_buildtests) add_dependencies(buildtests eigen2_buildtests) add_definitions("-DEIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API") +add_definitions("-DEIGEN_NO_EIGEN2_DEPRECATED_WARNING") ei_add_test(eigen2_meta) ei_add_test(eigen2_sizeof) diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp index 8ec9c9202..c0f811459 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp @@ -29,8 +29,6 @@ template void adjoint(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), - identity = SquareMatrixType::Identity(rows, rows), square = SquareMatrixType::Random(rows, rows); VectorType v1 = VectorType::Random(rows), v2 = VectorType::Random(rows), diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp index 4fa16d5ae..dd2dec1ef 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp @@ -23,11 +23,8 @@ template void basicStuff(const MatrixType& m) m2 = MatrixType::Random(rows, cols), m3(rows, cols), mzero = MatrixType::Zero(rows, cols), - identity = Matrix - ::Identity(rows, rows), square = Matrix::Random(rows, rows); VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), vzero = VectorType::Zero(rows); Scalar x = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp index 4391d19b4..22e1cc342 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp @@ -35,11 +35,8 @@ template void cwiseops(const MatrixType& m) mzero = MatrixType::Zero(rows, cols), mones = MatrixType::Ones(rows, cols), identity = Matrix - ::Identity(rows, rows), - square = Matrix::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - vzero = VectorType::Zero(rows), + ::Identity(rows, rows); + VectorType vzero = VectorType::Zero(rows), vones = VectorType::Ones(rows), v3(rows); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp index 70b4ab5f1..514040774 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp @@ -392,6 +392,7 @@ template void geometry(void) #define VERIFY_EULER(I,J,K, X,Y,Z) { \ Vector3 ea = m.eulerAngles(I,J,K); \ Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ + VERIFY_IS_APPROX(m, m1); \ VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ } VERIFY_EULER(0,1,2, X,Y,Z); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp index f83b57249..12d4a71c3 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp @@ -394,6 +394,7 @@ template void geometry(void) #define VERIFY_EULER(I,J,K, X,Y,Z) { \ Vector3 ea = m.eulerAngles(I,J,K); \ Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ + VERIFY_IS_APPROX(m, m1); \ VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ } VERIFY_EULER(0,1,2, X,Y,Z); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp index 5de1dfefa..ccd24a194 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp @@ -25,7 +25,6 @@ template void inverse(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2(rows, cols), - mzero = MatrixType::Zero(rows, cols), identity = MatrixType::Identity(rows, rows); while(ei_abs(m1.determinant()) < RealScalar(0.1) && rows <= 8) diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp index 22f02c396..488f4c485 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp @@ -25,8 +25,7 @@ template void linearStructure(const MatrixType& m) // to test it, hence I consider that we will have tested Random.h MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols); + m3(rows, cols); Scalar s1 = ei_random(); while (ei_abs(s1)<1e-3) s1 = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp index e234abe4b..d34a69999 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp @@ -25,22 +25,12 @@ template void nomalloc(const MatrixType& m) */ typedef typename MatrixType::Scalar Scalar; - typedef Matrix VectorType; int rows = m.rows(); int cols = m.cols(); MatrixType m1 = MatrixType::Random(rows, cols), - m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), - identity = Matrix - ::Identity(rows, rows), - square = Matrix - ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); + m2 = MatrixType::Random(rows, cols); Scalar s1 = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp index c5d3f243d..dee970b63 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp @@ -51,16 +51,10 @@ template void submatrices(const MatrixType& m) MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), m3(rows, cols), - mzero = MatrixType::Zero(rows, cols), ones = MatrixType::Ones(rows, cols), - identity = Matrix - ::Identity(rows, rows), square = Matrix ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - v3 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); + VectorType v1 = VectorType::Random(rows); Scalar s1 = ei_random(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp index 3748c7d71..6f17b7dff 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp @@ -13,7 +13,6 @@ template void triangular(const MatrixType& m) { typedef typename MatrixType::Scalar Scalar; typedef typename NumTraits::Real RealScalar; - typedef Matrix VectorType; RealScalar largerEps = 10*test_precision(); @@ -25,16 +24,7 @@ template void triangular(const MatrixType& m) m3(rows, cols), m4(rows, cols), r1(rows, cols), - r2(rows, cols), - mzero = MatrixType::Zero(rows, cols), - mones = MatrixType::Ones(rows, cols), - identity = Matrix - ::Identity(rows, rows), - square = Matrix - ::Random(rows, rows); - VectorType v1 = VectorType::Random(rows), - v2 = VectorType::Random(rows), - vzero = VectorType::Zero(rows); + r2(rows, cols); MatrixType m1up = m1.template part(); MatrixType m2up = m2.template part(); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/product.h b/gtsam/3rdparty/Eigen/test/eigen2/product.h index 2c9604d9a..ae1b4bae4 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/product.h +++ b/gtsam/3rdparty/Eigen/test/eigen2/product.h @@ -40,8 +40,7 @@ template void product(const MatrixType& m) // to test it, hence I consider that we will have tested Random.h MatrixType m1 = MatrixType::Random(rows, cols), m2 = MatrixType::Random(rows, cols), - m3(rows, cols), - mzero = MatrixType::Zero(rows, cols); + m3(rows, cols); RowSquareMatrixType identity = RowSquareMatrixType::Identity(rows, rows), square = RowSquareMatrixType::Random(rows, rows), @@ -49,9 +48,7 @@ template void product(const MatrixType& m) ColSquareMatrixType square2 = ColSquareMatrixType::Random(cols, cols), res2 = ColSquareMatrixType::Random(cols, cols); - RowVectorType v1 = RowVectorType::Random(rows), - v2 = RowVectorType::Random(rows), - vzero = RowVectorType::Zero(rows); + RowVectorType v1 = RowVectorType::Random(rows); ColVectorType vc2 = ColVectorType::Random(cols), vcres(cols); OtherMajorMatrixType tm1 = m1; diff --git a/gtsam/3rdparty/Eigen/test/eigen2support.cpp b/gtsam/3rdparty/Eigen/test/eigen2support.cpp index ad1d98091..1fa49a8c8 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2support.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2support.cpp @@ -8,6 +8,7 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #define EIGEN2_SUPPORT +#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING #include "main.h" diff --git a/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp b/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp index 5c6ecd875..38689cfbf 100644 --- a/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp +++ b/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp @@ -29,7 +29,21 @@ template void selfadjointeigensolver(const MatrixType& m) MatrixType a = MatrixType::Random(rows,cols); MatrixType a1 = MatrixType::Random(rows,cols); MatrixType symmA = a.adjoint() * a + a1.adjoint() * a1; + MatrixType symmC = symmA; + + // randomly nullify some rows/columns + { + Index count = 1;//internal::random(-cols,cols); + for(Index k=0; k(0,cols-1); + symmA.row(i).setZero(); + symmA.col(i).setZero(); + } + } + symmA.template triangularView().setZero(); + symmC.template triangularView().setZero(); MatrixType b = MatrixType::Random(rows,cols); MatrixType b1 = MatrixType::Random(rows,cols); @@ -40,7 +54,7 @@ template void selfadjointeigensolver(const MatrixType& m) SelfAdjointEigenSolver eiDirect; eiDirect.computeDirect(symmA); // generalized eigen pb - GeneralizedSelfAdjointEigenSolver eiSymmGen(symmA, symmB); + GeneralizedSelfAdjointEigenSolver eiSymmGen(symmC, symmB); VERIFY_IS_EQUAL(eiSymm.info(), Success); VERIFY((symmA.template selfadjointView() * eiSymm.eigenvectors()).isApprox( @@ -57,27 +71,28 @@ template void selfadjointeigensolver(const MatrixType& m) VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmNoEivecs.eigenvalues()); // generalized eigen problem Ax = lBx - eiSymmGen.compute(symmA, symmB,Ax_lBx); + eiSymmGen.compute(symmC, symmB,Ax_lBx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); - VERIFY((symmA.template selfadjointView() * eiSymmGen.eigenvectors()).isApprox( + VERIFY((symmC.template selfadjointView() * eiSymmGen.eigenvectors()).isApprox( symmB.template selfadjointView() * (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); // generalized eigen problem BAx = lx - eiSymmGen.compute(symmA, symmB,BAx_lx); + eiSymmGen.compute(symmC, symmB,BAx_lx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); - VERIFY((symmB.template selfadjointView() * (symmA.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( + VERIFY((symmB.template selfadjointView() * (symmC.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); // generalized eigen problem ABx = lx - eiSymmGen.compute(symmA, symmB,ABx_lx); + eiSymmGen.compute(symmC, symmB,ABx_lx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); - VERIFY((symmA.template selfadjointView() * (symmB.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( + VERIFY((symmC.template selfadjointView() * (symmB.template selfadjointView() * eiSymmGen.eigenvectors())).isApprox( (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); + eiSymm.compute(symmC); MatrixType sqrtSymmA = eiSymm.operatorSqrt(); - VERIFY_IS_APPROX(MatrixType(symmA.template selfadjointView()), sqrtSymmA*sqrtSymmA); - VERIFY_IS_APPROX(sqrtSymmA, symmA.template selfadjointView()*eiSymm.operatorInverseSqrt()); + VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView()), sqrtSymmA*sqrtSymmA); + VERIFY_IS_APPROX(sqrtSymmA, symmC.template selfadjointView()*eiSymm.operatorInverseSqrt()); MatrixType id = MatrixType::Identity(rows, cols); VERIFY_IS_APPROX(id.template selfadjointView().operatorNorm(), RealScalar(1)); @@ -95,15 +110,15 @@ template void selfadjointeigensolver(const MatrixType& m) VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt()); // test Tridiagonalization's methods - Tridiagonalization tridiag(symmA); + Tridiagonalization tridiag(symmC); // FIXME tridiag.matrixQ().adjoint() does not work - VERIFY_IS_APPROX(MatrixType(symmA.template selfadjointView()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint()); + VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint()); if (rows > 1) { // Test matrix with NaN - symmA(0,0) = std::numeric_limits::quiet_NaN(); - SelfAdjointEigenSolver eiSymmNaN(symmA); + symmC(0,0) = std::numeric_limits::quiet_NaN(); + SelfAdjointEigenSolver eiSymmNaN(symmC); VERIFY_IS_EQUAL(eiSymmNaN.info(), NoConvergence); } } @@ -113,8 +128,10 @@ void test_eigensolver_selfadjoint() int s = 0; for(int i = 0; i < g_repeat; i++) { // very important to test 3x3 and 2x2 matrices since we provide special paths for them + CALL_SUBTEST_1( selfadjointeigensolver(Matrix2f()) ); CALL_SUBTEST_1( selfadjointeigensolver(Matrix2d()) ); CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) ); + CALL_SUBTEST_1( selfadjointeigensolver(Matrix3d()) ); CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) ); s = internal::random(1,EIGEN_TEST_MAX_SIZE/4); CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(s,s)) ); diff --git a/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp b/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp index f26fc1329..327537801 100644 --- a/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp @@ -114,6 +114,32 @@ template void lines() } } +template void planes() +{ + using std::abs; + typedef Hyperplane Plane; + typedef Matrix Vector; + + for(int i = 0; i < 10; i++) + { + Vector v0 = Vector::Random(); + Vector v1(v0), v2(v0); + if(internal::random(0,1)>0.25) + v1 += Vector::Random(); + if(internal::random(0,1)>0.25) + v2 += v1 * std::pow(internal::random(0,1),internal::random(1,16)); + if(internal::random(0,1)>0.25) + v2 += Vector::Random() * std::pow(internal::random(0,1),internal::random(1,16)); + + Plane p0 = Plane::Through(v0, v1, v2); + + VERIFY_IS_APPROX(p0.normal().norm(), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v0), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v1), Scalar(1)); + VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v2), Scalar(1)); + } +} + template void hyperplane_alignment() { typedef Hyperplane Plane3a; @@ -153,5 +179,7 @@ void test_geo_hyperplane() CALL_SUBTEST_4( hyperplane(Hyperplane,5>()) ); CALL_SUBTEST_1( lines() ); CALL_SUBTEST_3( lines() ); + CALL_SUBTEST_2( planes() ); + CALL_SUBTEST_5( planes() ); } } diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index ee3030b5d..547765714 100644 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -98,11 +98,19 @@ template void transformations() Matrix3 matrot1, m; Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI)); - Scalar s0 = internal::random(); + Scalar s0 = internal::random(), + s1 = internal::random(); + + while(v0.norm() < test_precision()) v0 = Vector3::Random(); + while(v1.norm() < test_precision()) v1 = Vector3::Random(); + VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0); VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0); - VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); + if(abs(cos(a)) > test_precision()) + { + VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); + } m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint(); VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized())); VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m); @@ -123,11 +131,18 @@ template void transformations() // angle-axis conversion AngleAxisx aa = AngleAxisx(q1); VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1); + + if(abs(aa.angle()) > NumTraits::dummy_precision()) + { + VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); + } aa.fromRotationMatrix(aa.toRotationMatrix()); VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); - VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1); + if(abs(aa.angle()) > NumTraits::dummy_precision()) + { + VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); + } // AngleAxis VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(), @@ -347,7 +362,9 @@ template void transformations() // test transform inversion t0.setIdentity(); t0.translate(v0); - t0.linear().setRandom(); + do { + t0.linear().setRandom(); + } while(t0.linear().jacobiSvd().singularValues()(2)()); Matrix4 t044 = Matrix4::Zero(); t044(3,3) = 1; t044.block(0,0,t0.matrix().rows(),4) = t0.matrix(); @@ -397,6 +414,29 @@ template void transformations() t20 = Translation2(v20) * (Rotation2D(s0) * Eigen::Scaling(s0)); t21 = Translation2(v20) * Rotation2D(s0) * Eigen::Scaling(s0); VERIFY_IS_APPROX(t20,t21); + + Rotation2D R0(s0), R1(s1); + t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0)); + t21 = Translation2(v20) * R0 * Eigen::Scaling(s0); + VERIFY_IS_APPROX(t20,t21); + + t20 = Translation2(v20) * (R0 * R0.inverse() * Eigen::Scaling(s0)); + t21 = Translation2(v20) * Eigen::Scaling(s0); + VERIFY_IS_APPROX(t20,t21); + + VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle()); + VERIFY_IS_APPROX(s1, (R0.slerp(1, R1)).angle()); + VERIFY_IS_APPROX(s0, (R0.slerp(0.5, R0)).angle()); + VERIFY_IS_APPROX(Scalar(0), (R0.slerp(0.5, R0.inverse())).angle()); + + // check basic features + { + Rotation2D r1; // default ctor + r1 = Rotation2D(s0); // copy assignment + VERIFY_IS_APPROX(r1.angle(),s0); + Rotation2D r2(r1); // copy ctor + VERIFY_IS_APPROX(r2.angle(),s0); + } } template void transform_alignment() diff --git a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp index 7c21f0ab3..12c556e59 100644 --- a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp +++ b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp @@ -321,16 +321,23 @@ void jacobisvd_inf_nan() VERIFY(sub(some_inf, some_inf) != sub(some_inf, some_inf)); svd.compute(MatrixType::Constant(10,10,some_inf), ComputeFullU | ComputeFullV); - Scalar some_nan = zero() / zero(); - VERIFY(some_nan != some_nan); - svd.compute(MatrixType::Constant(10,10,some_nan), ComputeFullU | ComputeFullV); + Scalar nan = std::numeric_limits::quiet_NaN(); + VERIFY(nan != nan); + svd.compute(MatrixType::Constant(10,10,nan), ComputeFullU | ComputeFullV); MatrixType m = MatrixType::Zero(10,10); m(internal::random(0,9), internal::random(0,9)) = some_inf; svd.compute(m, ComputeFullU | ComputeFullV); m = MatrixType::Zero(10,10); - m(internal::random(0,9), internal::random(0,9)) = some_nan; + m(internal::random(0,9), internal::random(0,9)) = nan; + svd.compute(m, ComputeFullU | ComputeFullV); + + // regression test for bug 791 + m.resize(3,3); + m << 0, 2*NumTraits::epsilon(), 0.5, + 0, -0.5, 0, + nan, 0, 0; svd.compute(m, ComputeFullU | ComputeFullV); } @@ -434,6 +441,7 @@ void test_jacobisvd() // Test on inf/nan matrix CALL_SUBTEST_7( jacobisvd_inf_nan() ); + CALL_SUBTEST_10( jacobisvd_inf_nan() ); } CALL_SUBTEST_7(( jacobisvd(MatrixXf(internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2), internal::random(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) )); diff --git a/gtsam/3rdparty/Eigen/test/main.h b/gtsam/3rdparty/Eigen/test/main.h index 14f0d2f78..664204866 100644 --- a/gtsam/3rdparty/Eigen/test/main.h +++ b/gtsam/3rdparty/Eigen/test/main.h @@ -17,13 +17,36 @@ #include #include #include + +// The following includes of STL headers have to be done _before_ the +// definition of macros min() and max(). The reason is that many STL +// implementations will not work properly as the min and max symbols collide +// with the STL functions std:min() and std::max(). The STL headers may check +// for the macro definition of min/max and issue a warning or undefine the +// macros. +// +// Still, Windows defines min() and max() in windef.h as part of the regular +// Windows system interfaces and many other Windows APIs depend on these +// macros being available. To prevent the macro expansion of min/max and to +// make Eigen compatible with the Windows environment all function calls of +// std::min() and std::max() have to be written with parenthesis around the +// function name. +// +// All STL headers used by Eigen should be included here. Because main.h is +// included before any Eigen header and because the STL headers are guarded +// against multiple inclusions, no STL header will see our own min/max macro +// definitions. #include #include -#include #include #include #include +#include +// To test that all calls from Eigen code to std::min() and std::max() are +// protected by parenthesis against macro expansion, the min()/max() macros +// are defined here and any not-parenthesized min/max call will cause a +// compiler error. #define min(A,B) please_protect_your_min_with_parentheses #define max(A,B) please_protect_your_max_with_parentheses @@ -383,6 +406,26 @@ void randomPermutationVector(PermutationVectorType& v, typename PermutationVecto } } +template bool isNotNaN(const T& x) +{ + return x==x; +} + +template bool isNaN(const T& x) +{ + return x!=x; +} + +template bool isInf(const T& x) +{ + return x > NumTraits::highest(); +} + +template bool isMinusInf(const T& x) +{ + return x < NumTraits::lowest(); +} + } // end namespace Eigen template struct GetDifferentType; diff --git a/gtsam/3rdparty/Eigen/test/nomalloc.cpp b/gtsam/3rdparty/Eigen/test/nomalloc.cpp index cbd02dd21..8e0402358 100644 --- a/gtsam/3rdparty/Eigen/test/nomalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/nomalloc.cpp @@ -165,6 +165,38 @@ void ctms_decompositions() Eigen::JacobiSVD jSVD; jSVD.compute(A, ComputeFullU | ComputeFullV); } +void test_zerosized() { + // default constructors: + Eigen::MatrixXd A; + Eigen::VectorXd v; + // explicit zero-sized: + Eigen::ArrayXXd A0(0,0); + Eigen::ArrayXd v0(std::ptrdiff_t(0)); // FIXME ArrayXd(0) is ambiguous + + // assigning empty objects to each other: + A=A0; + v=v0; +} + +template void test_reference(const MatrixType& m) { + typedef typename MatrixType::Scalar Scalar; + enum { Flag = MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor}; + enum { TransposeFlag = !MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor}; + typename MatrixType::Index rows = m.rows(), cols=m.cols(); + // Dynamic reference: + typedef Eigen::Ref > Ref; + typedef Eigen::Ref > RefT; + + Ref r1(m); + Ref r2(m.block(rows/3, cols/4, rows/2, cols/2)); + RefT r3(m.transpose()); + RefT r4(m.topLeftCorner(rows/2, cols/2).transpose()); + + VERIFY_RAISES_ASSERT(RefT r5(m)); + VERIFY_RAISES_ASSERT(Ref r6(m.transpose())); + VERIFY_RAISES_ASSERT(Ref r7(Scalar(2) * m)); +} + void test_nomalloc() { // check that our operator new is indeed called: @@ -175,5 +207,6 @@ void test_nomalloc() // Check decomposition modules with dynamic matrices that have a known compile-time max size (ctms) CALL_SUBTEST_4(ctms_decompositions()); - + CALL_SUBTEST_5(test_zerosized()); + CALL_SUBTEST_6(test_reference(Matrix())); } diff --git a/gtsam/3rdparty/Eigen/test/nullary.cpp b/gtsam/3rdparty/Eigen/test/nullary.cpp index 5408d88b2..fbc721a1a 100644 --- a/gtsam/3rdparty/Eigen/test/nullary.cpp +++ b/gtsam/3rdparty/Eigen/test/nullary.cpp @@ -80,7 +80,9 @@ void testVectorType(const VectorType& base) Matrix col_vector(size); row_vector.setLinSpaced(size,low,high); col_vector.setLinSpaced(size,low,high); - VERIFY( row_vector.isApprox(col_vector.transpose(), NumTraits::epsilon())); + // when using the extended precision (e.g., FPU) the relative error might exceed 1 bit + // when computing the squared sum in isApprox, thus the 2x factor. + VERIFY( row_vector.isApprox(col_vector.transpose(), Scalar(2)*NumTraits::epsilon())); Matrix size_changer(size+50); size_changer.setLinSpaced(size,low,high); diff --git a/gtsam/3rdparty/Eigen/test/packetmath.cpp b/gtsam/3rdparty/Eigen/test/packetmath.cpp index 2c0519c41..38aa256ce 100644 --- a/gtsam/3rdparty/Eigen/test/packetmath.cpp +++ b/gtsam/3rdparty/Eigen/test/packetmath.cpp @@ -239,6 +239,12 @@ template void packetmath_real() data2[i] = internal::random(-87,88); } CHECK_CWISE1_IF(internal::packet_traits::HasExp, std::exp, internal::pexp); + { + data1[0] = std::numeric_limits::quiet_NaN(); + packet_helper::HasExp,Packet> h; + h.store(data2, internal::pexp(h.load(data1))); + VERIFY(isNaN(data2[0])); + } for (int i=0; i void packetmath_real() } if(internal::random(0,1)<0.1) data1[internal::random(0, PacketSize)] = 0; - CHECK_CWISE1_IF(internal::packet_traits::HasLog, std::log, internal::plog); CHECK_CWISE1_IF(internal::packet_traits::HasSqrt, std::sqrt, internal::psqrt); + CHECK_CWISE1_IF(internal::packet_traits::HasLog, std::log, internal::plog); + { + data1[0] = std::numeric_limits::quiet_NaN(); + packet_helper::HasLog,Packet> h; + h.store(data2, internal::plog(h.load(data1))); + VERIFY(isNaN(data2[0])); + data1[0] = -1.0f; + h.store(data2, internal::plog(h.load(data1))); + VERIFY(isNaN(data2[0])); +#if !EIGEN_FAST_MATH + h.store(data2, internal::psqrt(h.load(data1))); + VERIFY(isNaN(data2[0])); + VERIFY(isNaN(data2[1])); +#endif + } } template void packetmath_notcomplex() diff --git a/gtsam/3rdparty/Eigen/test/product.h b/gtsam/3rdparty/Eigen/test/product.h index 856b234ac..0b3abe402 100644 --- a/gtsam/3rdparty/Eigen/test/product.h +++ b/gtsam/3rdparty/Eigen/test/product.h @@ -139,4 +139,12 @@ template void product(const MatrixType& m) // inner product Scalar x = square2.row(c) * square2.col(c2); VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum()); + + // outer product + VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.row(r).transpose() * m1.col(c).transpose(), m1.block(r,0,1,cols).transpose() * m1.block(0,c,rows,1).transpose()); + VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols)); + VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols)); } diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 19e81549c..32eb31048 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -183,15 +183,15 @@ void call_ref() VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0); VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0); -// call_ref_1(ac); // does not compile because ac is const +// call_ref_1(ac,a +void check_sparse_abs_determinant(Solver& solver, const typename Solver::MatrixType& A, const DenseMat& dA) +{ + using std::abs; + typedef typename Solver::MatrixType Mat; + typedef typename Mat::Scalar Scalar; + + solver.compute(A); + if (solver.info() != Success) + { + std::cerr << "sparse solver testing: factorization failed (check_sparse_abs_determinant)\n"; + return; + } + Scalar refDet = abs(dA.determinant()); + VERIFY_IS_APPROX(refDet,solver.absDeterminant()); +} template int generate_sparse_spd_problem(Solver& , typename Solver::MatrixType& A, typename Solver::MatrixType& halfA, DenseMat& dA, int maxSize = 300) @@ -324,3 +340,20 @@ template void check_sparse_square_determinant(Solver& solver) check_sparse_determinant(solver, A, dA); } } + +template void check_sparse_square_abs_determinant(Solver& solver) +{ + typedef typename Solver::MatrixType Mat; + typedef typename Mat::Scalar Scalar; + typedef Matrix DenseMatrix; + + // generate the problem + Mat A; + DenseMatrix dA; + generate_sparse_square_problem(solver, A, dA, 30); + A.makeCompressed(); + for (int i = 0; i < g_repeat; i++) { + check_sparse_abs_determinant(solver, A, dA); + } +} + diff --git a/gtsam/3rdparty/Eigen/test/sparselu.cpp b/gtsam/3rdparty/Eigen/test/sparselu.cpp index 37980defc..52371cb12 100644 --- a/gtsam/3rdparty/Eigen/test/sparselu.cpp +++ b/gtsam/3rdparty/Eigen/test/sparselu.cpp @@ -44,6 +44,9 @@ template void test_sparselu_T() check_sparse_square_solving(sparselu_colamd); check_sparse_square_solving(sparselu_amd); check_sparse_square_solving(sparselu_natural); + + check_sparse_square_abs_determinant(sparselu_colamd); + check_sparse_square_abs_determinant(sparselu_amd); } void test_sparselu() diff --git a/gtsam/3rdparty/Eigen/test/sparseqr.cpp b/gtsam/3rdparty/Eigen/test/sparseqr.cpp index 1fe4a98ee..451c0e7f8 100644 --- a/gtsam/3rdparty/Eigen/test/sparseqr.cpp +++ b/gtsam/3rdparty/Eigen/test/sparseqr.cpp @@ -10,12 +10,11 @@ #include template -int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 150) +int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300) { - eigen_assert(maxRows >= maxCols); typedef typename MatrixType::Scalar Scalar; int rows = internal::random(1,maxRows); - int cols = internal::random(1,maxCols); + int cols = internal::random(1,rows); double density = (std::max)(8./(rows*cols), 0.01); A.resize(rows,cols); @@ -54,6 +53,8 @@ template void test_sparseqr_scalar() b = dA * DenseVector::Random(A.cols()); solver.compute(A); + if(internal::random(0,1)>0.5) + solver.factorize(A); // this checks that calling analyzePattern is not needed if the pattern do not change. if (solver.info() != Success) { std::cerr << "sparse QR factorization failed\n"; diff --git a/gtsam/3rdparty/Eigen/test/stable_norm.cpp b/gtsam/3rdparty/Eigen/test/stable_norm.cpp index 549f91fbf..231dd9189 100644 --- a/gtsam/3rdparty/Eigen/test/stable_norm.cpp +++ b/gtsam/3rdparty/Eigen/test/stable_norm.cpp @@ -9,11 +9,6 @@ #include "main.h" -template bool isNotNaN(const T& x) -{ - return x==x; -} - // workaround aggressive optimization in ICC template EIGEN_DONT_INLINE T sub(T a, T b) { return a - b; } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport index c4090ab11..e2769449c 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport @@ -178,11 +178,11 @@ template void glLoadMatrix(const Transform& t) template void glLoadMatrix(const Transform& t) { glLoadMatrix(t.matrix()); } template void glLoadMatrix(const Transform& t) { glLoadMatrix(Transform(t).matrix()); } -static void glRotate(const Rotation2D& rot) +inline void glRotate(const Rotation2D& rot) { glRotatef(rot.angle()*180.f/float(M_PI), 0.f, 0.f, 1.f); } -static void glRotate(const Rotation2D& rot) +inline void glRotate(const Rotation2D& rot) { glRotated(rot.angle()*180.0/M_PI, 0.0, 0.0, 1.0); } @@ -246,18 +246,18 @@ EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glGet,GLenum,_,double, 4,4,Doublev) #ifdef GL_VERSION_2_0 -static void glUniform2fv_ei (GLint loc, const float* v) { glUniform2fv(loc,1,v); } -static void glUniform2iv_ei (GLint loc, const int* v) { glUniform2iv(loc,1,v); } +inline void glUniform2fv_ei (GLint loc, const float* v) { glUniform2fv(loc,1,v); } +inline void glUniform2iv_ei (GLint loc, const int* v) { glUniform2iv(loc,1,v); } -static void glUniform3fv_ei (GLint loc, const float* v) { glUniform3fv(loc,1,v); } -static void glUniform3iv_ei (GLint loc, const int* v) { glUniform3iv(loc,1,v); } +inline void glUniform3fv_ei (GLint loc, const float* v) { glUniform3fv(loc,1,v); } +inline void glUniform3iv_ei (GLint loc, const int* v) { glUniform3iv(loc,1,v); } -static void glUniform4fv_ei (GLint loc, const float* v) { glUniform4fv(loc,1,v); } -static void glUniform4iv_ei (GLint loc, const int* v) { glUniform4iv(loc,1,v); } +inline void glUniform4fv_ei (GLint loc, const float* v) { glUniform4fv(loc,1,v); } +inline void glUniform4iv_ei (GLint loc, const int* v) { glUniform4iv(loc,1,v); } -static void glUniformMatrix2fv_ei (GLint loc, const float* v) { glUniformMatrix2fv(loc,1,false,v); } -static void glUniformMatrix3fv_ei (GLint loc, const float* v) { glUniformMatrix3fv(loc,1,false,v); } -static void glUniformMatrix4fv_ei (GLint loc, const float* v) { glUniformMatrix4fv(loc,1,false,v); } +inline void glUniformMatrix2fv_ei (GLint loc, const float* v) { glUniformMatrix2fv(loc,1,false,v); } +inline void glUniformMatrix3fv_ei (GLint loc, const float* v) { glUniformMatrix3fv(loc,1,false,v); } +inline void glUniformMatrix4fv_ei (GLint loc, const float* v) { glUniformMatrix4fv(loc,1,false,v); } EIGEN_GL_FUNC1_DECLARATION (glUniform,GLint,const) @@ -294,9 +294,9 @@ EIGEN_GL_FUNC1_SPECIALIZATION_MAT(glUniform,GLint,const,float, 4,3,Matrix #ifdef GL_VERSION_3_0 -static void glUniform2uiv_ei (GLint loc, const unsigned int* v) { glUniform2uiv(loc,1,v); } -static void glUniform3uiv_ei (GLint loc, const unsigned int* v) { glUniform3uiv(loc,1,v); } -static void glUniform4uiv_ei (GLint loc, const unsigned int* v) { glUniform4uiv(loc,1,v); } +inline void glUniform2uiv_ei (GLint loc, const unsigned int* v) { glUniform2uiv(loc,1,v); } +inline void glUniform3uiv_ei (GLint loc, const unsigned int* v) { glUniform3uiv(loc,1,v); } +inline void glUniform4uiv_ei (GLint loc, const unsigned int* v) { glUniform4uiv(loc,1,v); } EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 2,2uiv_ei) EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 3,3uiv_ei) @@ -305,9 +305,9 @@ EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,unsigned int, 4,4uiv_ei) #endif #ifdef GL_ARB_gpu_shader_fp64 -static void glUniform2dv_ei (GLint loc, const double* v) { glUniform2dv(loc,1,v); } -static void glUniform3dv_ei (GLint loc, const double* v) { glUniform3dv(loc,1,v); } -static void glUniform4dv_ei (GLint loc, const double* v) { glUniform4dv(loc,1,v); } +inline void glUniform2dv_ei (GLint loc, const double* v) { glUniform2dv(loc,1,v); } +inline void glUniform3dv_ei (GLint loc, const double* v) { glUniform3dv(loc,1,v); } +inline void glUniform4dv_ei (GLint loc, const double* v) { glUniform4dv(loc,1,v); } EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,double, 2,2dv_ei) EIGEN_GL_FUNC1_SPECIALIZATION_VEC(glUniform,GLint,const,double, 3,3dv_ei) diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h index c32437281..78a307e96 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h @@ -110,7 +110,6 @@ void MatrixPowerAtomic::compute2x2(MatrixType& res, RealScalar p) co using std::abs; using std::pow; - ArrayType logTdiag = m_A.diagonal().array().log(); res.coeffRef(0,0) = pow(m_A.coeff(0,0), p); for (Index i=1; i < m_A.cols(); ++i) { diff --git a/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt index 978f9afd8..c47646dfc 100644 --- a/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/doc/examples/CMakeLists.txt @@ -10,12 +10,10 @@ FOREACH(example_src ${examples_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(example_${example} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - GET_TARGET_PROPERTY(example_executable - example_${example} LOCATION) ADD_CUSTOM_COMMAND( TARGET example_${example} POST_BUILD - COMMAND ${example_executable} + COMMAND example_${example} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out ) ADD_DEPENDENCIES(unsupported_examples example_${example}) diff --git a/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt index 4a4157933..f0c5cc2a8 100644 --- a/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/doc/snippets/CMakeLists.txt @@ -14,12 +14,10 @@ FOREACH(snippet_src ${snippets_SRCS}) if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO) target_link_libraries(${compile_snippet_target} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}) endif() - GET_TARGET_PROPERTY(compile_snippet_executable - ${compile_snippet_target} LOCATION) ADD_CUSTOM_COMMAND( TARGET ${compile_snippet_target} POST_BUILD - COMMAND ${compile_snippet_executable} + COMMAND ${compile_snippet_target} ARGS >${CMAKE_CURRENT_BINARY_DIR}/${snippet}.out ) ADD_DEPENDENCIES(unsupported_snippets ${compile_snippet_target}) diff --git a/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt index a94a3b5e5..2e4cfdb2e 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt @@ -29,11 +29,7 @@ ei_add_test(NonLinearOptimization) ei_add_test(NumericalDiff) ei_add_test(autodiff) - -if (NOT CMAKE_CXX_COMPILER MATCHES "clang\\+\\+$") ei_add_test(BVH) -endif() - ei_add_test(matrix_exponential) ei_add_test(matrix_function) ei_add_test(matrix_power) @@ -73,8 +69,9 @@ if(NOT EIGEN_TEST_NO_OPENGL) find_package(GLUT) find_package(GLEW) if(OPENGL_FOUND AND GLUT_FOUND AND GLEW_FOUND) + include_directories(${OPENGL_INCLUDE_DIR} ${GLUT_INCLUDE_DIR} ${GLEW_INCLUDE_DIRS}) ei_add_property(EIGEN_TESTED_BACKENDS "OpenGL, ") - set(EIGEN_GL_LIB ${GLUT_LIBRARIES} ${GLEW_LIBRARIES}) + set(EIGEN_GL_LIB ${GLUT_LIBRARIES} ${GLEW_LIBRARIES} ${OPENGL_LIBRARIES}) ei_add_test(openglsupport "" "${EIGEN_GL_LIB}" ) else() ei_add_property(EIGEN_MISSING_BACKENDS "OpenGL, ") diff --git a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h index ef0a6a9f0..dddda7dd9 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h +++ b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h @@ -1,59 +1,47 @@ /* - Multi-precision floating point number class for C++. + MPFR C++: Multi-precision floating point number class for C++. Based on MPFR library: http://mpfr.org Project homepage: http://www.holoborodko.com/pavel/mpfr Contact e-mail: pavel@holoborodko.com - Copyright (c) 2008-2012 Pavel Holoborodko + Copyright (c) 2008-2014 Pavel Holoborodko Contributors: Dmitriy Gubanov, Konstantin Holoborodko, Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, Tsai Chia Cheng, - Alexei Zubanov, Jauhien Piatlicki, Victor Berger, John Westwood. + Alexei Zubanov, Jauhien Piatlicki, Victor Berger, John Westwood, + Petr Aleksandrov, Orion Poplawski, Charles Karney. - **************************************************************************** - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + Licensing: + (A) MPFR C++ is under GNU General Public License ("GPL"). + + (B) Non-free licenses may also be purchased from the author, for users who + do not want their programs protected by the GPL. - This library is distributed in the hope that it will be useful, + The non-free licenses are for users that wish to use MPFR C++ in + their products but are unwilling to release their software + under the GPL (which would require them to release source code + and allow free redistribution). + + Such users can purchase an unlimited-use license from the author. + Contact us for more details. + + GNU General Public License ("GPL") copyright permissions statement: + ************************************************************************** + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - - **************************************************************************** - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: - - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - - 3. The name of the author may not be used to endorse or promote products - derived from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - SUCH DAMAGE. + You should have received a copy of the GNU General Public License + along with this program. If not, see . */ #ifndef __MPREAL_H__ @@ -65,11 +53,21 @@ #include #include #include +#include #include // Options #define MPREAL_HAVE_INT64_SUPPORT // Enable int64_t support if possible. Available only for MSVC 2010 & GCC. #define MPREAL_HAVE_MSVC_DEBUGVIEW // Enable Debugger Visualizer for "Debug" builds in MSVC. +#define MPREAL_HAVE_DYNAMIC_STD_NUMERIC_LIMITS // Enable extended std::numeric_limits specialization. + // Meaning that "digits", "round_style" and similar members are defined as functions, not constants. + // See std::numeric_limits at the end of the file for more information. + +// Library version +#define MPREAL_VERSION_MAJOR 3 +#define MPREAL_VERSION_MINOR 5 +#define MPREAL_VERSION_PATCHLEVEL 9 +#define MPREAL_VERSION_STRING "3.5.9" // Detect compiler using signatures from http://predef.sourceforge.net/ #if defined(__GNUC__) && defined(__INTEL_COMPILER) @@ -82,6 +80,32 @@ #define IsInf(x) std::isinf(x) // GNU C/C++ (and/or other compilers), just hope for C99 conformance #endif +// A Clang feature extension to determine compiler features. +#ifndef __has_feature + #define __has_feature(x) 0 +#endif + +// Detect support for r-value references (move semantic). Borrowed from Eigen. +#if (__has_feature(cxx_rvalue_references) || \ + defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L || \ + (defined(_MSC_VER) && _MSC_VER >= 1600)) + + #define MPREAL_HAVE_MOVE_SUPPORT + + // Use fields in mpfr_t structure to check if it was initialized / set dummy initialization + #define mpfr_is_initialized(x) (0 != (x)->_mpfr_d) + #define mpfr_set_uninitialized(x) ((x)->_mpfr_d = 0 ) +#endif + +// Detect support for explicit converters. +#if (__has_feature(cxx_explicit_conversions) || \ + defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L || \ + (defined(_MSC_VER) && _MSC_VER >= 1800)) + + #define MPREAL_HAVE_EXPLICIT_CONVERTERS +#endif + +// Detect available 64-bit capabilities #if defined(MPREAL_HAVE_INT64_SUPPORT) #define MPFR_USE_INTMAX_T // Should be defined before mpfr.h @@ -97,7 +121,7 @@ #endif #elif defined (__GNUC__) && defined(__linux__) - #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(__ia64) || defined(__itanium__) || defined(_M_IA64) + #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(__ia64) || defined(__itanium__) || defined(_M_IA64) || defined (__PPC64__) #undef MPREAL_HAVE_INT64_SUPPORT // Remove all shaman dances for x64 builds since #undef MPFR_USE_INTMAX_T // GCC already supports x64 as of "long int" is 64-bit integer, nothing left to do #else @@ -111,7 +135,7 @@ #endif #if defined(MPREAL_HAVE_MSVC_DEBUGVIEW) && defined(_MSC_VER) && defined(_DEBUG) -#define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString(); + #define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString(); #define MPREAL_MSVC_DEBUGVIEW_DATA std::string DebugView; #else #define MPREAL_MSVC_DEBUGVIEW_CODE @@ -149,7 +173,6 @@ public: // Constructors && type conversions mpreal(); mpreal(const mpreal& u); - mpreal(const mpfr_t u); mpreal(const mpf_t u); mpreal(const mpz_t u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); mpreal(const mpq_t u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); @@ -159,6 +182,10 @@ public: mpreal(const unsigned int u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); mpreal(const long int u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); mpreal(const int u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); + + // Construct mpreal from mpfr_t structure. + // shared = true allows to avoid deep copy, so that mpreal and 'u' share the same data & pointers. + mpreal(const mpfr_t u, bool shared = false); #if defined (MPREAL_HAVE_INT64_SUPPORT) mpreal(const uint64_t u, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t mode = mpreal::get_default_rnd()); @@ -170,6 +197,11 @@ public: ~mpreal(); +#ifdef MPREAL_HAVE_MOVE_SUPPORT + mpreal& operator=(mpreal&& v); + mpreal(mpreal&& u); +#endif + // Operations // = // +, -, *, /, ++, --, <<, >> @@ -292,14 +324,34 @@ public: friend bool operator == (const mpreal& a, const double b); // Type Conversion operators + bool toBool (mp_rnd_t mode = GMP_RNDZ) const; long toLong (mp_rnd_t mode = GMP_RNDZ) const; unsigned long toULong (mp_rnd_t mode = GMP_RNDZ) const; + float toFloat (mp_rnd_t mode = GMP_RNDN) const; double toDouble (mp_rnd_t mode = GMP_RNDN) const; long double toLDouble (mp_rnd_t mode = GMP_RNDN) const; +#if defined (MPREAL_HAVE_EXPLICIT_CONVERTERS) + explicit operator bool () const { return toBool(); } + explicit operator int () const { return toLong(); } + explicit operator long () const { return toLong(); } + explicit operator long long () const { return toLong(); } + explicit operator unsigned () const { return toULong(); } + explicit operator unsigned long () const { return toULong(); } + explicit operator unsigned long long () const { return toULong(); } + explicit operator float () const { return toFloat(); } + explicit operator double () const { return toDouble(); } + explicit operator long double () const { return toLDouble(); } +#endif + #if defined (MPREAL_HAVE_INT64_SUPPORT) int64_t toInt64 (mp_rnd_t mode = GMP_RNDZ) const; uint64_t toUInt64 (mp_rnd_t mode = GMP_RNDZ) const; + + #if defined (MPREAL_HAVE_EXPLICIT_CONVERTERS) + explicit operator int64_t () const { return toInt64(); } + explicit operator uint64_t () const { return toUInt64(); } + #endif #endif // Get raw pointers so that mpreal can be directly used in raw mpfr_* functions @@ -308,121 +360,125 @@ public: ::mpfr_srcptr mpfr_srcptr() const; // Convert mpreal to string with n significant digits in base b - // n = 0 -> convert with the maximum available digits - std::string toString(int n = 0, int b = 10, mp_rnd_t mode = mpreal::get_default_rnd()) const; + // n = -1 -> convert with the maximum available digits + std::string toString(int n = -1, int b = 10, mp_rnd_t mode = mpreal::get_default_rnd()) const; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - std::string toString(const std::string& format) const; + std::string toString(const std::string& format) const; #endif - // Math Functions - friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + std::ostream& output(std::ostream& os) const; - friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + // Math Functions + friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode); + friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode); + friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode); + friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode); + friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode); + friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode); + + friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode); + friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode); + friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode); + friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode); + friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode); friend int cmpabs(const mpreal& a,const mpreal& b); - friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode); + friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode); + friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); - friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode); - friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal lgamma (const mpreal& v, int *signp, mp_rnd_t rnd_mode); + friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode); + friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode); + friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode); + friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode); friend int sgn(const mpreal& v); // returns -1 or +1 // MPFR 2.4.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - friend int sinh_cosh (mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal li2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rec_sqrt (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend int sinh_cosh (mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal li2 (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); + friend const mpreal rec_sqrt (const mpreal& v, mp_rnd_t rnd_mode); // MATLAB's semantic equivalents - friend const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); // Remainder after division - friend const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); // Modulus after division + friend const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); // Remainder after division + friend const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); // Modulus after division #endif // MPFR 3.0.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - friend const mpreal digamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal ai (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal digamma (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal ai (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal grandom (gmp_randstate_t& state, mp_rnd_t rnd_mode); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal grandom (unsigned int seed); #endif // Uniformly distributed random number generation in [0,1] using // Mersenne-Twister algorithm by default. // Use parameter to setup seed, e.g.: random((unsigned)time(NULL)) // Check urandom() for more precise control. - friend const mpreal random(unsigned int seed = 0); - + friend const mpreal random(unsigned int seed); + // Exponent and mantissa manipulation friend const mpreal frexp(const mpreal& v, mp_exp_t* exp); friend const mpreal ldexp(const mpreal& v, mp_exp_t exp); @@ -433,31 +489,31 @@ public: // Constants // don't forget to call mpfr_free_cache() for every thread where you are using const-functions - friend const mpreal const_log2 (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal const_pi (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal const_euler (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal const_catalan (mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal const_log2 (mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal const_pi (mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal const_euler (mp_prec_t prec, mp_rnd_t rnd_mode); + friend const mpreal const_catalan (mp_prec_t prec, mp_rnd_t rnd_mode); // returns +inf iff sign>=0 otherwise -inf - friend const mpreal const_infinity(int sign = 1, mp_prec_t prec = mpreal::get_default_prec(), mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal const_infinity(int sign, mp_prec_t prec); // Output/ Input friend std::ostream& operator<<(std::ostream& os, const mpreal& v); friend std::istream& operator>>(std::istream& is, mpreal& v); // Integer Related Functions - friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode); friend const mpreal ceil (const mpreal& v); friend const mpreal floor(const mpreal& v); friend const mpreal round(const mpreal& v); friend const mpreal trunc(const mpreal& v); - friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rint_floor (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rint_round (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal rint_trunc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal remainder ( const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal rint_floor (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal rint_round (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal rint_trunc (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode); + friend const mpreal remainder ( const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); + friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); // Miscellaneous Functions friend const mpreal nexttoward (const mpreal& x, const mpreal& y); @@ -524,19 +580,22 @@ public: // Efficient swapping of two mpreal values - needed for std algorithms friend void swap(mpreal& x, mpreal& y); - friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); - friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); + friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode); private: // Human friendly Debug Preview in Visual Studio. // Put one of these lines: // - // mpfr::mpreal= ; Show value only + // mpfr::mpreal= ; Show value only // mpfr::mpreal=, bits ; Show value & precision // // at the beginning of // [Visual Studio Installation Folder]\Common7\Packages\Debugger\autoexp.dat MPREAL_MSVC_DEBUGVIEW_DATA + + // "Smart" resources deallocation. Checks if instance initialized before deletion. + void clear(::mpfr_ptr); }; ////////////////////////////////////////////////////////////////////////// @@ -551,64 +610,89 @@ public: // Default constructor: creates mp number and initializes it to 0. inline mpreal::mpreal() { - mpfr_init2(mp,mpreal::get_default_prec()); - mpfr_set_ui(mp,0,mpreal::get_default_rnd()); + mpfr_init2 (mpfr_ptr(), mpreal::get_default_prec()); + mpfr_set_ui(mpfr_ptr(), 0, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpreal& u) { - mpfr_init2(mp,mpfr_get_prec(u.mp)); - mpfr_set(mp,u.mp,mpreal::get_default_rnd()); + mpfr_init2(mpfr_ptr(),mpfr_get_prec(u.mpfr_srcptr())); + mpfr_set (mpfr_ptr(),u.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } -inline mpreal::mpreal(const mpfr_t u) +#ifdef MPREAL_HAVE_MOVE_SUPPORT +inline mpreal::mpreal(mpreal&& other) { - mpfr_init2(mp,mpfr_get_prec(u)); - mpfr_set(mp,u,mpreal::get_default_rnd()); + mpfr_set_uninitialized(mpfr_ptr()); // make sure "other" holds no pinter to actual data + mpfr_swap(mpfr_ptr(), other.mpfr_ptr()); + + MPREAL_MSVC_DEBUGVIEW_CODE; +} + +inline mpreal& mpreal::operator=(mpreal&& other) +{ + mpfr_swap(mpfr_ptr(), other.mpfr_ptr()); + + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; +} +#endif + +inline mpreal::mpreal(const mpfr_t u, bool shared) +{ + if(shared) + { + std::memcpy(mpfr_ptr(), u, sizeof(mpfr_t)); + } + else + { + mpfr_init2(mpfr_ptr(), mpfr_get_prec(u)); + mpfr_set (mpfr_ptr(), u, mpreal::get_default_rnd()); + } MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpf_t u) { - mpfr_init2(mp,(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) - mpfr_set_f(mp,u,mpreal::get_default_rnd()); + mpfr_init2(mpfr_ptr(),(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) + mpfr_set_f(mpfr_ptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpz_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_z(mp,u,mode); + mpfr_init2(mpfr_ptr(), prec); + mpfr_set_z(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const mpq_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_q(mp,u,mode); + mpfr_init2(mpfr_ptr(), prec); + mpfr_set_q(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const double u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp, prec); + mpfr_init2(mpfr_ptr(), prec); #if (MPREAL_DOUBLE_BITS_OVERFLOW > -1) - if(fits_in_bits(u, MPREAL_DOUBLE_BITS_OVERFLOW)) - { - mpfr_set_d(mp, u, mode); - }else - throw conversion_overflow(); + if(fits_in_bits(u, MPREAL_DOUBLE_BITS_OVERFLOW)) + { + mpfr_set_d(mpfr_ptr(), u, mode); + }else + throw conversion_overflow(); #else - mpfr_set_d(mp, u, mode); + mpfr_set_d(mpfr_ptr(), u, mode); #endif MPREAL_MSVC_DEBUGVIEW_CODE; @@ -616,40 +700,40 @@ inline mpreal::mpreal(const double u, mp_prec_t prec, mp_rnd_t mode) inline mpreal::mpreal(const long double u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_ld(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_ld(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const unsigned long int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_ui(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const unsigned int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_ui(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const long int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_si(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const int u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_si(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } @@ -657,16 +741,16 @@ inline mpreal::mpreal(const int u, mp_prec_t prec, mp_rnd_t mode) #if defined (MPREAL_HAVE_INT64_SUPPORT) inline mpreal::mpreal(const uint64_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_uj(mp, u, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_uj(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const int64_t u, mp_prec_t prec, mp_rnd_t mode) { - mpfr_init2(mp,prec); - mpfr_set_sj(mp, u, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_sj(mpfr_ptr(), u, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } @@ -674,23 +758,31 @@ inline mpreal::mpreal(const int64_t u, mp_prec_t prec, mp_rnd_t mode) inline mpreal::mpreal(const char* s, mp_prec_t prec, int base, mp_rnd_t mode) { - mpfr_init2(mp, prec); - mpfr_set_str(mp, s, base, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_str(mpfr_ptr(), s, base, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mpreal::mpreal(const std::string& s, mp_prec_t prec, int base, mp_rnd_t mode) { - mpfr_init2(mp, prec); - mpfr_set_str(mp, s.c_str(), base, mode); + mpfr_init2 (mpfr_ptr(), prec); + mpfr_set_str(mpfr_ptr(), s.c_str(), base, mode); MPREAL_MSVC_DEBUGVIEW_CODE; } +inline void mpreal::clear(::mpfr_ptr x) +{ +#ifdef MPREAL_HAVE_MOVE_SUPPORT + if(mpfr_is_initialized(x)) +#endif + mpfr_clear(x); +} + inline mpreal::~mpreal() { - mpfr_clear(mp); + clear(mpfr_ptr()); } // internal namespace needed for template magic @@ -761,6 +853,9 @@ const mpreal sqrt(const int v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); const mpreal sqrt(const long double v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); const mpreal sqrt(const double v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +// abs +inline const mpreal abs(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()); + ////////////////////////////////////////////////////////////////////////// // pow const mpreal pow(const mpreal& a, const unsigned int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); @@ -813,6 +908,11 @@ const mpreal pow(const double a, const unsigned int b, mp_rnd_t rnd_mode = mprea const mpreal pow(const double a, const long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); +inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::get_default_rnd()); + ////////////////////////////////////////////////////////////////////////// // Estimate machine epsilon for the given precision // Returns smallest eps such that 1.0 + eps != 1.0 @@ -860,15 +960,15 @@ inline mpreal& mpreal::operator=(const mpreal& v) { if (this != &v) { - mp_prec_t tp = mpfr_get_prec(mp); - mp_prec_t vp = mpfr_get_prec(v.mp); + mp_prec_t tp = mpfr_get_prec( mpfr_srcptr()); + mp_prec_t vp = mpfr_get_prec(v.mpfr_srcptr()); - if(tp != vp){ - mpfr_clear(mp); - mpfr_init2(mp, vp); - } + if(tp != vp){ + clear(mpfr_ptr()); + mpfr_init2(mpfr_ptr(), vp); + } - mpfr_set(mp, v.mp, mpreal::get_default_rnd()); + mpfr_set(mpfr_ptr(), v.mpfr_srcptr(), mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } @@ -877,7 +977,7 @@ inline mpreal& mpreal::operator=(const mpreal& v) inline mpreal& mpreal::operator=(const mpf_t v) { - mpfr_set_f(mp, v, mpreal::get_default_rnd()); + mpfr_set_f(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -885,7 +985,7 @@ inline mpreal& mpreal::operator=(const mpf_t v) inline mpreal& mpreal::operator=(const mpz_t v) { - mpfr_set_z(mp, v, mpreal::get_default_rnd()); + mpfr_set_z(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -893,7 +993,7 @@ inline mpreal& mpreal::operator=(const mpz_t v) inline mpreal& mpreal::operator=(const mpq_t v) { - mpfr_set_q(mp, v, mpreal::get_default_rnd()); + mpfr_set_q(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -901,7 +1001,7 @@ inline mpreal& mpreal::operator=(const mpq_t v) inline mpreal& mpreal::operator=(const long double v) { - mpfr_set_ld(mp, v, mpreal::get_default_rnd()); + mpfr_set_ld(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -910,22 +1010,22 @@ inline mpreal& mpreal::operator=(const long double v) inline mpreal& mpreal::operator=(const double v) { #if (MPREAL_DOUBLE_BITS_OVERFLOW > -1) - if(fits_in_bits(v, MPREAL_DOUBLE_BITS_OVERFLOW)) - { - mpfr_set_d(mp,v,mpreal::get_default_rnd()); - }else - throw conversion_overflow(); + if(fits_in_bits(v, MPREAL_DOUBLE_BITS_OVERFLOW)) + { + mpfr_set_d(mpfr_ptr(),v,mpreal::get_default_rnd()); + }else + throw conversion_overflow(); #else - mpfr_set_d(mp,v,mpreal::get_default_rnd()); + mpfr_set_d(mpfr_ptr(),v,mpreal::get_default_rnd()); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator=(const unsigned long int v) { - mpfr_set_ui(mp, v, mpreal::get_default_rnd()); + mpfr_set_ui(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -933,7 +1033,7 @@ inline mpreal& mpreal::operator=(const unsigned long int v) inline mpreal& mpreal::operator=(const unsigned int v) { - mpfr_set_ui(mp, v, mpreal::get_default_rnd()); + mpfr_set_ui(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -941,7 +1041,7 @@ inline mpreal& mpreal::operator=(const unsigned int v) inline mpreal& mpreal::operator=(const long int v) { - mpfr_set_si(mp, v, mpreal::get_default_rnd()); + mpfr_set_si(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -949,7 +1049,7 @@ inline mpreal& mpreal::operator=(const long int v) inline mpreal& mpreal::operator=(const int v) { - mpfr_set_si(mp, v, mpreal::get_default_rnd()); + mpfr_set_si(mpfr_ptr(), v, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; @@ -966,15 +1066,15 @@ inline mpreal& mpreal::operator=(const char* s) mpfr_t t; - mpfr_init2(t, mpfr_get_prec(mp)); + mpfr_init2(t, mpfr_get_prec(mpfr_srcptr())); if(0 == mpfr_set_str(t, s, 10, mpreal::get_default_rnd())) { - mpfr_set(mp, t, mpreal::get_default_rnd()); + mpfr_set(mpfr_ptr(), t, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } - mpfr_clear(t); + clear(t); return *this; } @@ -989,15 +1089,15 @@ inline mpreal& mpreal::operator=(const std::string& s) mpfr_t t; - mpfr_init2(t, mpfr_get_prec(mp)); + mpfr_init2(t, mpfr_get_prec(mpfr_srcptr())); if(0 == mpfr_set_str(t, s.c_str(), 10, mpreal::get_default_rnd())) { - mpfr_set(mp, t, mpreal::get_default_rnd()); + mpfr_set(mpfr_ptr(), t, mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; } - mpfr_clear(t); + clear(t); return *this; } @@ -1006,7 +1106,7 @@ inline mpreal& mpreal::operator=(const std::string& s) // + Addition inline mpreal& mpreal::operator+=(const mpreal& v) { - mpfr_add(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_add(mpfr_ptr(), mpfr_srcptr(), v.mpfr_srcptr(), mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1020,14 +1120,14 @@ inline mpreal& mpreal::operator+=(const mpf_t u) inline mpreal& mpreal::operator+=(const mpz_t u) { - mpfr_add_z(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_z(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const mpq_t u) { - mpfr_add_q(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_q(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1042,7 +1142,7 @@ inline mpreal& mpreal::operator+= (const long double u) inline mpreal& mpreal::operator+= (const double u) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_add_d(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_d(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); #else *this += mpreal(u); #endif @@ -1053,28 +1153,28 @@ inline mpreal& mpreal::operator+= (const double u) inline mpreal& mpreal::operator+=(const unsigned long int u) { - mpfr_add_ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const unsigned int u) { - mpfr_add_ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const long int u) { - mpfr_add_si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator+=(const int u) { - mpfr_add_si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_add_si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1094,9 +1194,9 @@ inline const mpreal mpreal::operator+()const { return mpreal(*this); } inline const mpreal operator+(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_add(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); + mpfr_add(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } inline mpreal& mpreal::operator++() @@ -1127,21 +1227,21 @@ inline const mpreal mpreal::operator-- (int) // - Subtraction inline mpreal& mpreal::operator-=(const mpreal& v) { - mpfr_sub(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_sub(mpfr_ptr(),mpfr_srcptr(),v.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const mpz_t v) { - mpfr_sub_z(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_z(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const mpq_t v) { - mpfr_sub_q(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_q(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1156,7 +1256,7 @@ inline mpreal& mpreal::operator-=(const long double v) inline mpreal& mpreal::operator-=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_sub_d(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_d(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); #else *this -= mpreal(v); #endif @@ -1167,28 +1267,28 @@ inline mpreal& mpreal::operator-=(const double v) inline mpreal& mpreal::operator-=(const unsigned long int v) { - mpfr_sub_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const unsigned int v) { - mpfr_sub_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const long int v) { - mpfr_sub_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator-=(const int v) { - mpfr_sub_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_sub_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1196,15 +1296,15 @@ inline mpreal& mpreal::operator-=(const int v) inline const mpreal mpreal::operator-()const { mpreal u(*this); - mpfr_neg(u.mp,u.mp,mpreal::get_default_rnd()); + mpfr_neg(u.mpfr_ptr(),u.mpfr_srcptr(),mpreal::get_default_rnd()); return u; } inline const mpreal operator-(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_sub(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); + mpfr_sub(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } inline const mpreal operator-(const double b, const mpreal& a) @@ -1252,21 +1352,21 @@ inline const mpreal operator-(const int b, const mpreal& a) // * Multiplication inline mpreal& mpreal::operator*= (const mpreal& v) { - mpfr_mul(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_mul(mpfr_ptr(),mpfr_srcptr(),v.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const mpz_t v) { - mpfr_mul_z(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_z(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const mpq_t v) { - mpfr_mul_q(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_q(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1281,7 +1381,7 @@ inline mpreal& mpreal::operator*=(const long double v) inline mpreal& mpreal::operator*=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_mul_d(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_d(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); #else *this *= mpreal(v); #endif @@ -1291,58 +1391,58 @@ inline mpreal& mpreal::operator*=(const double v) inline mpreal& mpreal::operator*=(const unsigned long int v) { - mpfr_mul_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const unsigned int v) { - mpfr_mul_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const long int v) { - mpfr_mul_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator*=(const int v) { - mpfr_mul_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_mul_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline const mpreal operator*(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_mul(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); + mpfr_mul(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } ////////////////////////////////////////////////////////////////////////// // / Division inline mpreal& mpreal::operator/=(const mpreal& v) { - mpfr_div(mp,mp,v.mp,mpreal::get_default_rnd()); + mpfr_div(mpfr_ptr(),mpfr_srcptr(),v.mpfr_srcptr(),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const mpz_t v) { - mpfr_div_z(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_z(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const mpq_t v) { - mpfr_div_q(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_q(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1357,7 +1457,7 @@ inline mpreal& mpreal::operator/=(const long double v) inline mpreal& mpreal::operator/=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_div_d(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_d(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); #else *this /= mpreal(v); #endif @@ -1367,72 +1467,72 @@ inline mpreal& mpreal::operator/=(const double v) inline mpreal& mpreal::operator/=(const unsigned long int v) { - mpfr_div_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const unsigned int v) { - mpfr_div_ui(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_ui(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const long int v) { - mpfr_div_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator/=(const int v) { - mpfr_div_si(mp,mp,v,mpreal::get_default_rnd()); + mpfr_div_si(mpfr_ptr(),mpfr_srcptr(),v,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline const mpreal operator/(const mpreal& a, const mpreal& b) { - mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_ptr()), mpfr_get_prec(b.mpfr_ptr()))); - mpfr_div(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); - return c; + mpreal c(0, (std::max)(mpfr_get_prec(a.mpfr_srcptr()), mpfr_get_prec(b.mpfr_srcptr()))); + mpfr_div(c.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), mpreal::get_default_rnd()); + return c; } inline const mpreal operator/(const unsigned long int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); mpfr_ui_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const unsigned int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); mpfr_ui_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const long int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); - mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(),mpreal::get_default_rnd()); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); + mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const int b, const mpreal& a) { - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); - mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(),mpreal::get_default_rnd()); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); + mpfr_si_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; } inline const mpreal operator/(const double b, const mpreal& a) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); - mpfr_d_div(x.mpfr_ptr(), b, a.mpfr_srcptr(),mpreal::get_default_rnd()); + mpreal x(0, mpfr_get_prec(a.mpfr_srcptr())); + mpfr_d_div(x.mpfr_ptr(), b, a.mpfr_srcptr(), mpreal::get_default_rnd()); return x; #else mpreal x(0, mpfr_get_prec(a.mpfr_ptr())); @@ -1445,56 +1545,56 @@ inline const mpreal operator/(const double b, const mpreal& a) // Shifts operators - Multiplication/Division by power of 2 inline mpreal& mpreal::operator<<=(const unsigned long int u) { - mpfr_mul_2ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_mul_2ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator<<=(const unsigned int u) { - mpfr_mul_2ui(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_mul_2ui(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator<<=(const long int u) { - mpfr_mul_2si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_mul_2si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator<<=(const int u) { - mpfr_mul_2si(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_mul_2si(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const unsigned long int u) { - mpfr_div_2ui(mp,mp,u,mpreal::get_default_rnd()); + mpfr_div_2ui(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const unsigned int u) { - mpfr_div_2ui(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_div_2ui(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const long int u) { - mpfr_div_2si(mp,mp,u,mpreal::get_default_rnd()); + mpfr_div_2si(mpfr_ptr(),mpfr_srcptr(),u,mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::operator>>=(const int u) { - mpfr_div_2si(mp,mp,static_cast(u),mpreal::get_default_rnd()); + mpfr_div_2si(mpfr_ptr(),mpfr_srcptr(),static_cast(u),mpreal::get_default_rnd()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1543,7 +1643,7 @@ inline const mpreal operator>>(const mpreal& v, const int k) inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_mul_2ui(x.mp,v.mp,k,rnd_mode); + mpfr_mul_2ui(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } @@ -1551,61 +1651,63 @@ inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_m inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_mul_2si(x.mp,v.mp,k,rnd_mode); + mpfr_mul_2si(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_div_2ui(x.mp,v.mp,k,rnd_mode); + mpfr_div_2ui(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { mpreal x(v); - mpfr_div_2si(x.mp,v.mp,k,rnd_mode); + mpfr_div_2si(x.mpfr_ptr(),v.mpfr_srcptr(),k,rnd_mode); return x; } ////////////////////////////////////////////////////////////////////////// //Boolean operators -inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p(a.mp,b.mp) !=0); } -inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p(a.mp,b.mp) !=0); } -inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p(a.mp,b.mp) !=0); } -inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p(a.mp,b.mp) !=0); } -inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p(a.mp,b.mp) !=0); } -inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p(a.mp,b.mp) !=0); } +inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } +inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p (a.mpfr_srcptr(),b.mpfr_srcptr()) !=0 ); } -inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mpfr_srcptr(),b) == 0 ); } +inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d (a.mpfr_srcptr(),b) == 0 ); } -inline bool isnan (const mpreal& v){ return (mpfr_nan_p(v.mp) != 0); } -inline bool isinf (const mpreal& v){ return (mpfr_inf_p(v.mp) != 0); } -inline bool isfinite (const mpreal& v){ return (mpfr_number_p(v.mp) != 0); } -inline bool iszero (const mpreal& v){ return (mpfr_zero_p(v.mp) != 0); } -inline bool isint (const mpreal& v){ return (mpfr_integer_p(v.mp) != 0); } +inline bool isnan (const mpreal& op){ return (mpfr_nan_p (op.mpfr_srcptr()) != 0 ); } +inline bool isinf (const mpreal& op){ return (mpfr_inf_p (op.mpfr_srcptr()) != 0 ); } +inline bool isfinite (const mpreal& op){ return (mpfr_number_p (op.mpfr_srcptr()) != 0 ); } +inline bool iszero (const mpreal& op){ return (mpfr_zero_p (op.mpfr_srcptr()) != 0 ); } +inline bool isint (const mpreal& op){ return (mpfr_integer_p(op.mpfr_srcptr()) != 0 ); } #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) -inline bool isregular(const mpreal& v){ return (mpfr_regular_p(v.mp));} +inline bool isregular(const mpreal& op){ return (mpfr_regular_p(op.mpfr_srcptr()));} #endif ////////////////////////////////////////////////////////////////////////// // Type Converters -inline long mpreal::toLong (mp_rnd_t mode) const { return mpfr_get_si(mp, mode); } -inline unsigned long mpreal::toULong (mp_rnd_t mode) const { return mpfr_get_ui(mp, mode); } -inline double mpreal::toDouble (mp_rnd_t mode) const { return mpfr_get_d (mp, mode); } -inline long double mpreal::toLDouble(mp_rnd_t mode) const { return mpfr_get_ld(mp, mode); } +inline bool mpreal::toBool (mp_rnd_t /*mode*/) const { return mpfr_zero_p (mpfr_srcptr()) == 0; } +inline long mpreal::toLong (mp_rnd_t mode) const { return mpfr_get_si (mpfr_srcptr(), mode); } +inline unsigned long mpreal::toULong (mp_rnd_t mode) const { return mpfr_get_ui (mpfr_srcptr(), mode); } +inline float mpreal::toFloat (mp_rnd_t mode) const { return mpfr_get_flt(mpfr_srcptr(), mode); } +inline double mpreal::toDouble (mp_rnd_t mode) const { return mpfr_get_d (mpfr_srcptr(), mode); } +inline long double mpreal::toLDouble(mp_rnd_t mode) const { return mpfr_get_ld (mpfr_srcptr(), mode); } #if defined (MPREAL_HAVE_INT64_SUPPORT) -inline int64_t mpreal::toInt64 (mp_rnd_t mode) const{ return mpfr_get_sj(mp, mode); } -inline uint64_t mpreal::toUInt64(mp_rnd_t mode) const{ return mpfr_get_uj(mp, mode); } +inline int64_t mpreal::toInt64 (mp_rnd_t mode) const{ return mpfr_get_sj(mpfr_srcptr(), mode); } +inline uint64_t mpreal::toUInt64(mp_rnd_t mode) const{ return mpfr_get_uj(mpfr_srcptr(), mode); } #endif inline ::mpfr_ptr mpreal::mpfr_ptr() { return mp; } @@ -1629,7 +1731,7 @@ inline std::string mpreal::toString(const std::string& format) const if( !format.empty() ) { - if(!(mpfr_asprintf(&s,format.c_str(),mp) < 0)) + if(!(mpfr_asprintf(&s, format.c_str(), mpfr_srcptr()) < 0)) { out = std::string(s); @@ -1644,20 +1746,19 @@ inline std::string mpreal::toString(const std::string& format) const inline std::string mpreal::toString(int n, int b, mp_rnd_t mode) const { + // TODO: Add extended format specification (f, e, rounding mode) as it done in output operator (void)b; (void)mode; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - // Use MPFR native function for output - char format[128]; - int digits; + std::ostringstream format; - digits = n > 0 ? n : bits2digits(mpfr_get_prec(mp)); + int digits = (n >= 0) ? n : bits2digits(mpfr_get_prec(mpfr_srcptr())); + + format << "%." << digits << "RNg"; - sprintf(format,"%%.%dRNg",digits); // Default format - - return toString(std::string(format)); + return toString(format.str()); #else @@ -1675,8 +1776,8 @@ inline std::string mpreal::toString(int n, int b, mp_rnd_t mode) const if(mpfr_zero_p(mp)) return "0"; if(mpfr_nan_p(mp)) return "NaN"; - s = mpfr_get_str(NULL,&exp,b,0,mp,mode); - ns = mpfr_get_str(NULL,&exp,b,n,mp,mode); + s = mpfr_get_str(NULL, &exp, b, 0, mp, mode); + ns = mpfr_get_str(NULL, &exp, b, (std::max)(0,n), mp, mode); if(s!=NULL && ns!=NULL) { @@ -1761,17 +1862,43 @@ inline std::string mpreal::toString(int n, int b, mp_rnd_t mode) const ////////////////////////////////////////////////////////////////////////// // I/O +inline std::ostream& mpreal::output(std::ostream& os) const +{ + std::ostringstream format; + const std::ios::fmtflags flags = os.flags(); + + format << ((flags & std::ios::showpos) ? "%+" : "%"); + if (os.precision() >= 0) + format << '.' << os.precision() << "R*" + << ((flags & std::ios::floatfield) == std::ios::fixed ? 'f' : + (flags & std::ios::floatfield) == std::ios::scientific ? 'e' : + 'g'); + else + format << "R*e"; + + char *s = NULL; + if(!(mpfr_asprintf(&s, format.str().c_str(), + mpfr::mpreal::get_default_rnd(), + mpfr_srcptr()) + < 0)) + { + os << std::string(s); + mpfr_free_str(s); + } + return os; +} + inline std::ostream& operator<<(std::ostream& os, const mpreal& v) { - return os<(os.precision())); + return v.output(os); } inline std::istream& operator>>(std::istream &is, mpreal& v) { - // ToDo, use cout::hexfloat and other flags to setup base + // TODO: use cout::hexfloat and other flags to setup base std::string tmp; is >> tmp; - mpfr_set_str(v.mp, tmp.c_str(), 10, mpreal::get_default_rnd()); + mpfr_set_str(v.mpfr_ptr(), tmp.c_str(), 10, mpreal::get_default_rnd()); return is; } @@ -1784,53 +1911,53 @@ inline mp_prec_t digits2bits(int d) { const double LOG2_10 = 3.3219280948873624; - return (mp_prec_t) std::ceil( d * LOG2_10 ); + return mp_prec_t(std::ceil( d * LOG2_10 )); } inline int bits2digits(mp_prec_t b) { const double LOG10_2 = 0.30102999566398119; - return (int) std::floor( b * LOG10_2 ); + return int(std::floor( b * LOG10_2 )); } ////////////////////////////////////////////////////////////////////////// // Set/Get number properties -inline int sgn(const mpreal& v) +inline int sgn(const mpreal& op) { - int r = mpfr_signbit(v.mp); - return (r>0?-1:1); + int r = mpfr_signbit(op.mpfr_srcptr()); + return (r > 0? -1 : 1); } inline mpreal& mpreal::setSign(int sign, mp_rnd_t RoundingMode) { - mpfr_setsign(mp,mp,(sign < 0 ? 1 : 0),RoundingMode); + mpfr_setsign(mpfr_ptr(), mpfr_srcptr(), (sign < 0 ? 1 : 0), RoundingMode); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline int mpreal::getPrecision() const { - return mpfr_get_prec(mp); + return int(mpfr_get_prec(mpfr_srcptr())); } inline mpreal& mpreal::setPrecision(int Precision, mp_rnd_t RoundingMode) { - mpfr_prec_round(mp, Precision, RoundingMode); + mpfr_prec_round(mpfr_ptr(), Precision, RoundingMode); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::setInf(int sign) { - mpfr_set_inf(mp,sign); + mpfr_set_inf(mpfr_ptr(), sign); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } inline mpreal& mpreal::setNan() { - mpfr_set_nan(mp); + mpfr_set_nan(mpfr_ptr()); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } @@ -1839,9 +1966,9 @@ inline mpreal& mpreal::setZero(int sign) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - mpfr_set_zero(mp, sign); + mpfr_set_zero(mpfr_ptr(), sign); #else - mpfr_set_si(mp, 0, (mpfr_get_default_rounding_mode)()); + mpfr_set_si(mpfr_ptr(), 0, (mpfr_get_default_rounding_mode)()); setSign(sign); #endif @@ -1851,23 +1978,23 @@ inline mpreal& mpreal::setZero(int sign) inline mp_prec_t mpreal::get_prec() const { - return mpfr_get_prec(mp); + return mpfr_get_prec(mpfr_srcptr()); } inline void mpreal::set_prec(mp_prec_t prec, mp_rnd_t rnd_mode) { - mpfr_prec_round(mp,prec,rnd_mode); + mpfr_prec_round(mpfr_ptr(),prec,rnd_mode); MPREAL_MSVC_DEBUGVIEW_CODE; } inline mp_exp_t mpreal::get_exp () { - return mpfr_get_exp(mp); + return mpfr_get_exp(mpfr_srcptr()); } inline int mpreal::set_exp (mp_exp_t e) { - int x = mpfr_set_exp(mp, e); + int x = mpfr_set_exp(mpfr_ptr(), e); MPREAL_MSVC_DEBUGVIEW_CODE; return x; } @@ -1885,7 +2012,7 @@ inline const mpreal ldexp(const mpreal& v, mp_exp_t exp) mpreal x(v); // rounding is not important since we just increasing the exponent - mpfr_mul_2si(x.mp,x.mp,exp,mpreal::get_default_rnd()); + mpfr_mul_2si(x.mpfr_ptr(), x.mpfr_srcptr(), exp, mpreal::get_default_rnd()); return x; } @@ -1900,9 +2027,9 @@ inline mpreal machine_epsilon(const mpreal& x) /* the smallest eps such that x + eps != x */ if( x < 0) { - return nextabove(-x)+x; + return nextabove(-x) + x; }else{ - return nextabove(x)-x; + return nextabove( x) - x; } } @@ -1922,37 +2049,37 @@ inline mpreal maxval(mp_prec_t prec) inline bool isEqualUlps(const mpreal& a, const mpreal& b, int maxUlps) { - return abs(a - b) <= machine_epsilon((max)(abs(a), abs(b))) * maxUlps; + return abs(a - b) <= machine_epsilon((max)(abs(a), abs(b))) * maxUlps; } inline bool isEqualFuzzy(const mpreal& a, const mpreal& b, const mpreal& eps) { - return abs(a - b) <= (min)(abs(a), abs(b)) * eps; + return abs(a - b) <= eps; } inline bool isEqualFuzzy(const mpreal& a, const mpreal& b) { - return isEqualFuzzy(a, b, machine_epsilon((min)(abs(a), abs(b)))); + return isEqualFuzzy(a, b, machine_epsilon((max)(1, (min)(abs(a), abs(b))))); } inline const mpreal modf(const mpreal& v, mpreal& n) { - mpreal frac(v); + mpreal f(v); // rounding is not important since we are using the same number - mpfr_frac(frac.mp,frac.mp,mpreal::get_default_rnd()); - mpfr_trunc(n.mp,v.mp); - return frac; + mpfr_frac (f.mpfr_ptr(),f.mpfr_srcptr(),mpreal::get_default_rnd()); + mpfr_trunc(n.mpfr_ptr(),v.mpfr_srcptr()); + return f; } inline int mpreal::check_range (int t, mp_rnd_t rnd_mode) { - return mpfr_check_range(mp,t,rnd_mode); + return mpfr_check_range(mpfr_ptr(),t,rnd_mode); } inline int mpreal::subnormalize (int t,mp_rnd_t rnd_mode) { - int r = mpfr_subnormalize(mp,t,rnd_mode); + int r = mpfr_subnormalize(mpfr_ptr(),t,rnd_mode); MPREAL_MSVC_DEBUGVIEW_CODE; return r; } @@ -2005,8 +2132,11 @@ inline mp_exp_t mpreal::get_emax_max (void) mpfr_##f(y.mpfr_ptr(), x.mpfr_srcptr(), r); \ return y; -inline const mpreal sqr (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sqr ); } -inline const mpreal sqrt (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sqrt); } +inline const mpreal sqr (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) +{ MPREAL_UNARY_MATH_FUNCTION_BODY(sqr ); } + +inline const mpreal sqrt (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) +{ MPREAL_UNARY_MATH_FUNCTION_BODY(sqrt); } inline const mpreal sqrt(const unsigned long int x, mp_rnd_t r) { @@ -2032,14 +2162,14 @@ inline const mpreal sqrt(const int v, mp_rnd_t rnd_mode) else return mpreal().setNan(); // NaN } -inline const mpreal root(const mpreal& x, unsigned long int k, mp_rnd_t r) +inline const mpreal root(const mpreal& x, unsigned long int k, mp_rnd_t r = mpreal::get_default_rnd()) { mpreal y(0, mpfr_get_prec(x.mpfr_srcptr())); mpfr_root(y.mpfr_ptr(), x.mpfr_srcptr(), k, r); return y; } -inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t r) +inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t r = mpreal::get_default_rnd()) { mpreal y(0, mpfr_get_prec(a.mpfr_srcptr())); mpfr_dim(y.mpfr_ptr(), a.mpfr_srcptr(), b.mpfr_srcptr(), r); @@ -2048,161 +2178,130 @@ inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t r) inline int cmpabs(const mpreal& a,const mpreal& b) { - return mpfr_cmpabs(a.mp,b.mp); + return mpfr_cmpabs(a.mpfr_ptr(), b.mpfr_srcptr()); } -inline int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) +inline int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - return mpfr_sin_cos(s.mp,c.mp,v.mp,rnd_mode); + return mpfr_sin_cos(s.mpfr_ptr(), c.mpfr_ptr(), v.mpfr_srcptr(), rnd_mode); } inline const mpreal sqrt (const long double v, mp_rnd_t rnd_mode) { return sqrt(mpreal(v),rnd_mode); } inline const mpreal sqrt (const double v, mp_rnd_t rnd_mode) { return sqrt(mpreal(v),rnd_mode); } -inline const mpreal cbrt (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cbrt ); } -inline const mpreal fabs (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } -inline const mpreal abs (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } -inline const mpreal log (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log ); } -inline const mpreal log2 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log2 ); } -inline const mpreal log10 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log10); } -inline const mpreal exp (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp ); } -inline const mpreal exp2 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp2 ); } -inline const mpreal exp10 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp10); } -inline const mpreal cos (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cos ); } -inline const mpreal sin (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sin ); } -inline const mpreal tan (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(tan ); } -inline const mpreal sec (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sec ); } -inline const mpreal csc (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(csc ); } -inline const mpreal cot (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cot ); } -inline const mpreal acos (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(acos); } -inline const mpreal asin (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(asin); } -inline const mpreal atan (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(atan); } +inline const mpreal cbrt (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cbrt ); } +inline const mpreal fabs (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } +inline const mpreal abs (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(abs ); } +inline const mpreal log (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log ); } +inline const mpreal log2 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log2 ); } +inline const mpreal log10 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log10); } +inline const mpreal exp (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp ); } +inline const mpreal exp2 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp2 ); } +inline const mpreal exp10 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(exp10); } +inline const mpreal cos (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cos ); } +inline const mpreal sin (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sin ); } +inline const mpreal tan (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(tan ); } +inline const mpreal sec (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sec ); } +inline const mpreal csc (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(csc ); } +inline const mpreal cot (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cot ); } +inline const mpreal acos (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(acos ); } +inline const mpreal asin (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(asin ); } +inline const mpreal atan (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(atan ); } -inline const mpreal acot (const mpreal& v, mp_rnd_t r) { return atan (1/v, r); } -inline const mpreal asec (const mpreal& v, mp_rnd_t r) { return acos (1/v, r); } -inline const mpreal acsc (const mpreal& v, mp_rnd_t r) { return asin (1/v, r); } -inline const mpreal acoth (const mpreal& v, mp_rnd_t r) { return atanh(1/v, r); } -inline const mpreal asech (const mpreal& v, mp_rnd_t r) { return acosh(1/v, r); } -inline const mpreal acsch (const mpreal& v, mp_rnd_t r) { return asinh(1/v, r); } +inline const mpreal acot (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return atan (1/v, r); } +inline const mpreal asec (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return acos (1/v, r); } +inline const mpreal acsc (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return asin (1/v, r); } +inline const mpreal acoth (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return atanh(1/v, r); } +inline const mpreal asech (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return acosh(1/v, r); } +inline const mpreal acsch (const mpreal& v, mp_rnd_t r = mpreal::get_default_rnd()) { return asinh(1/v, r); } -inline const mpreal cosh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(cosh ); } -inline const mpreal sinh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sinh ); } -inline const mpreal tanh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(tanh ); } -inline const mpreal sech (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(sech ); } -inline const mpreal csch (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(csch ); } -inline const mpreal coth (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(coth ); } -inline const mpreal acosh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(acosh); } -inline const mpreal asinh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(asinh); } -inline const mpreal atanh (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(atanh); } +inline const mpreal cosh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(cosh ); } +inline const mpreal sinh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sinh ); } +inline const mpreal tanh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(tanh ); } +inline const mpreal sech (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(sech ); } +inline const mpreal csch (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(csch ); } +inline const mpreal coth (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(coth ); } +inline const mpreal acosh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(acosh); } +inline const mpreal asinh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(asinh); } +inline const mpreal atanh (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(atanh); } -inline const mpreal log1p (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(log1p ); } -inline const mpreal expm1 (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(expm1 ); } -inline const mpreal eint (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(eint ); } -inline const mpreal gamma (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(gamma ); } -inline const mpreal lngamma (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(lngamma); } -inline const mpreal zeta (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(zeta ); } -inline const mpreal erf (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(erf ); } -inline const mpreal erfc (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(erfc ); } -inline const mpreal besselj0(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(j0 ); } -inline const mpreal besselj1(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(j1 ); } -inline const mpreal bessely0(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(y0 ); } -inline const mpreal bessely1(const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(y1 ); } +inline const mpreal log1p (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(log1p ); } +inline const mpreal expm1 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(expm1 ); } +inline const mpreal eint (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(eint ); } +inline const mpreal gamma (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(gamma ); } +inline const mpreal lngamma (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(lngamma); } +inline const mpreal zeta (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(zeta ); } +inline const mpreal erf (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(erf ); } +inline const mpreal erfc (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(erfc ); } +inline const mpreal besselj0(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(j0 ); } +inline const mpreal besselj1(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(j1 ); } +inline const mpreal bessely0(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(y0 ); } +inline const mpreal bessely1(const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(y1 ); } -inline const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode) +inline const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_atan2(a.mp, y.mp, x.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_atan2(a.mpfr_ptr(), y.mpfr_srcptr(), x.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_hypot(a.mp, x.mp, y.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_hypot(a.mpfr_ptr(), x.mpfr_srcptr(), y.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_remainder(a.mp, x.mp, y.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_remainder(a.mpfr_ptr(), x.mpfr_srcptr(), y.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { - mpreal a; - mp_prec_t yp, xp; - - yp = y.get_prec(); - xp = x.get_prec(); - - a.set_prec(yp>xp?yp:xp); - - mpfr_remquo(a.mp,q, x.mp, y.mp, rnd_mode); - + mpreal a(0,(std::max)(y.getPrecision(), x.getPrecision())); + mpfr_remquo(a.mpfr_ptr(),q, x.mpfr_srcptr(), y.mpfr_srcptr(), rnd_mode); return a; } -inline const mpreal fac_ui (unsigned long int v, mp_prec_t prec, mp_rnd_t rnd_mode) +inline const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::get_default_prec(), + mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(0, prec); - mpfr_fac_ui(x.mp,v,rnd_mode); + mpfr_fac_ui(x.mpfr_ptr(),v,rnd_mode); return x; } -inline const mpreal lgamma (const mpreal& v, int *signp, mp_rnd_t rnd_mode) +inline const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(v); int tsignp; - if(signp) mpfr_lgamma(x.mp,signp,v.mp,rnd_mode); - else mpfr_lgamma(x.mp,&tsignp,v.mp,rnd_mode); + if(signp) mpfr_lgamma(x.mpfr_ptr(), signp,v.mpfr_srcptr(),rnd_mode); + else mpfr_lgamma(x.mpfr_ptr(),&tsignp,v.mpfr_srcptr(),rnd_mode); return x; } -inline const mpreal besseljn (long n, const mpreal& x, mp_rnd_t r) +inline const mpreal besseljn (long n, const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { - mpreal y(0, mpfr_get_prec(x.mpfr_srcptr())); + mpreal y(0, x.getPrecision()); mpfr_jn(y.mpfr_ptr(), n, x.mpfr_srcptr(), r); return y; } -inline const mpreal besselyn (long n, const mpreal& x, mp_rnd_t r) +inline const mpreal besselyn (long n, const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { - mpreal y(0, mpfr_get_prec(x.mpfr_srcptr())); + mpreal y(0, x.getPrecision()); mpfr_yn(y.mpfr_ptr(), n, x.mpfr_srcptr(), r); return y; } -inline const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) +inline const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t p1, p2, p3; @@ -2217,7 +2316,7 @@ inline const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, m return a; } -inline const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) +inline const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t p1, p2, p3; @@ -2232,7 +2331,7 @@ inline const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, m return a; } -inline const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode) +inline const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t p1, p2; @@ -2247,7 +2346,7 @@ inline const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode) return a; } -inline const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode) +inline const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x; mpfr_ptr* t; @@ -2264,23 +2363,23 @@ inline const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_m // MPFR 2.4.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) -inline int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) +inline int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { return mpfr_sinh_cosh(s.mp,c.mp,v.mp,rnd_mode); } -inline const mpreal li2 (const mpreal& x, mp_rnd_t r) +inline const mpreal li2 (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(li2); } -inline const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal rem (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { /* R = rem(X,Y) if Y != 0, returns X - n * Y where n = trunc(X/Y). */ return fmod(x, y, rnd_mode); } -inline const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { (void)rnd_mode; @@ -2305,7 +2404,7 @@ inline const mpreal mod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) return m; } -inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) +inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal a; mp_prec_t yp, xp; @@ -2320,7 +2419,7 @@ inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) return a; } -inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) +inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(v); mpfr_rec_sqrt(x.mp,v.mp,rnd_mode); @@ -2331,41 +2430,41 @@ inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) ////////////////////////////////////////////////////////////////////////// // MPFR 3.0.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) -inline const mpreal digamma (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(digamma); } -inline const mpreal ai (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(ai); } +inline const mpreal digamma (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(digamma); } +inline const mpreal ai (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(ai); } #endif // MPFR 3.0.0 Specifics ////////////////////////////////////////////////////////////////////////// // Constants -inline const mpreal const_log2 (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_log2 (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_log2(x.mpfr_ptr(), r); return x; } -inline const mpreal const_pi (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_pi (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_pi(x.mpfr_ptr(), r); return x; } -inline const mpreal const_euler (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_euler (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_euler(x.mpfr_ptr(), r); return x; } -inline const mpreal const_catalan (mp_prec_t p, mp_rnd_t r) +inline const mpreal const_catalan (mp_prec_t p = mpreal::get_default_prec(), mp_rnd_t r = mpreal::get_default_rnd()) { mpreal x(0, p); mpfr_const_catalan(x.mpfr_ptr(), r); return x; } -inline const mpreal const_infinity (int sign, mp_prec_t p, mp_rnd_t /*r*/) +inline const mpreal const_infinity (int sign = 1, mp_prec_t p = mpreal::get_default_prec()) { mpreal x(0, p); mpfr_set_inf(x.mpfr_ptr(), sign); @@ -2402,12 +2501,12 @@ inline const mpreal trunc(const mpreal& v) return x; } -inline const mpreal rint (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint ); } -inline const mpreal rint_ceil (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_ceil ); } -inline const mpreal rint_floor (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_floor); } -inline const mpreal rint_round (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_round); } -inline const mpreal rint_trunc (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_trunc); } -inline const mpreal frac (const mpreal& x, mp_rnd_t r) { MPREAL_UNARY_MATH_FUNCTION_BODY(frac ); } +inline const mpreal rint (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint ); } +inline const mpreal rint_ceil (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_ceil ); } +inline const mpreal rint_floor (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_floor); } +inline const mpreal rint_round (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_round); } +inline const mpreal rint_trunc (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(rint_trunc); } +inline const mpreal frac (const mpreal& x, mp_rnd_t r = mpreal::get_default_rnd()) { MPREAL_UNARY_MATH_FUNCTION_BODY(frac ); } ////////////////////////////////////////////////////////////////////////// // Miscellaneous Functions @@ -2415,14 +2514,14 @@ inline void swap (mpreal& a, mpreal& b) { mpfr_swap(a.mp,b inline const mpreal (max)(const mpreal& x, const mpreal& y){ return (x>y?x:y); } inline const mpreal (min)(const mpreal& x, const mpreal& y){ return (x= MPFR_VERSION_NUM(3,0,0)) +#if (MPFR_VERSION >= MPFR_VERSION_NUM(3,1,0)) // use gmp_randinit_default() to init state, gmp_randclear() to clear -inline const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode) +inline const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x; mpfr_urandom(x.mp,state,rnd_mode); return x; } + +inline const mpreal grandom (gmp_randstate_t& state, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) +{ + mpreal x; + mpfr_grandom(x.mp, NULL, state, rnd_mode); + return x; +} + #endif #if (MPFR_VERSION <= MPFR_VERSION_NUM(2,4,2)) @@ -2480,7 +2587,7 @@ inline const mpreal random2 (mp_size_t size, mp_exp_t exp) // a = random(seed); <- initialization & first random number generation // a = random(); <- next random numbers generation // seed != 0 -inline const mpreal random(unsigned int seed) +inline const mpreal random(unsigned int seed = 0) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) @@ -2504,6 +2611,25 @@ inline const mpreal random(unsigned int seed) } +#if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) +inline const mpreal grandom(unsigned int seed = 0) +{ + static gmp_randstate_t state; + static bool isFirstTime = true; + + if(isFirstTime) + { + gmp_randinit_default(state); + gmp_randseed_ui(state,0); + isFirstTime = false; + } + + if(seed != 0) gmp_randseed_ui(state,seed); + + return mpfr::grandom(state); +} +#endif + ////////////////////////////////////////////////////////////////////////// // Set/Get global properties inline void mpreal::set_default_prec(mp_prec_t prec) @@ -2523,21 +2649,21 @@ inline bool mpreal::fits_in_bits(double x, int n) return IsInf(x) || (std::modf ( std::ldexp ( std::frexp ( x, &i ), n ), &t ) == 0.0); } -inline const mpreal pow(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow(x.mp,x.mp,b.mp,rnd_mode); return x; } -inline const mpreal pow(const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow_z(x.mp,x.mp,b,rnd_mode); return x; } -inline const mpreal pow(const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow_ui(x.mp,x.mp,b,rnd_mode); @@ -2549,7 +2675,7 @@ inline const mpreal pow(const mpreal& a, const unsigned int b, mp_rnd_t rnd_mode return pow(a,static_cast(b),rnd_mode); } -inline const mpreal pow(const mpreal& a, const long int b, mp_rnd_t rnd_mode) +inline const mpreal pow(const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_pow_si(x.mp,x.mp,b,rnd_mode); @@ -2571,7 +2697,7 @@ inline const mpreal pow(const mpreal& a, const double b, mp_rnd_t rnd_mode) return pow(a,mpreal(b),rnd_mode); } -inline const mpreal pow(const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode) +inline const mpreal pow(const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::get_default_rnd()) { mpreal x(a); mpfr_ui_pow(x.mp,a,b.mp,rnd_mode); @@ -2586,13 +2712,13 @@ inline const mpreal pow(const unsigned int a, const mpreal& b, mp_rnd_t rnd_mode inline const mpreal pow(const long int a, const mpreal& b, mp_rnd_t rnd_mode) { if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const int a, const mpreal& b, mp_rnd_t rnd_mode) { if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const long double a, const mpreal& b, mp_rnd_t rnd_mode) @@ -2621,13 +2747,13 @@ inline const mpreal pow(const unsigned long int a, const unsigned int b, mp_rnd_ inline const mpreal pow(const unsigned long int a, const long int b, mp_rnd_t rnd_mode) { if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const int b, mp_rnd_t rnd_mode) { if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const long double b, mp_rnd_t rnd_mode) @@ -2824,7 +2950,7 @@ inline const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode) // Non-throwing swap C++ idiom: http://en.wikibooks.org/wiki/More_C%2B%2B_Idioms/Non-throwing_swap namespace std { - // only allowed to extend namespace std with specializations + // we are allowed to extend namespace std with specializations only template <> inline void swap(mpfr::mpreal& x, mpfr::mpreal& y) { @@ -2852,20 +2978,6 @@ namespace std static const bool tinyness_before = true; static const float_denorm_style has_denorm = denorm_absent; - - inline static float_round_style round_style() - { - mp_rnd_t r = mpfr::mpreal::get_default_rnd(); - - switch (r) - { - case MPFR_RNDN: return round_to_nearest; - case MPFR_RNDZ: return round_toward_zero; - case MPFR_RNDU: return round_toward_infinity; - case MPFR_RNDD: return round_toward_neg_infinity; - default: return round_indeterminate; - } - } inline static mpfr::mpreal (min) (mp_prec_t precision = mpfr::mpreal::get_default_prec()) { return mpfr::minval(precision); } inline static mpfr::mpreal (max) (mp_prec_t precision = mpfr::mpreal::get_default_prec()) { return mpfr::maxval(precision); } @@ -2873,7 +2985,7 @@ namespace std // Returns smallest eps such that 1 + eps != 1 (classic machine epsilon) inline static mpfr::mpreal epsilon(mp_prec_t precision = mpfr::mpreal::get_default_prec()) { return mpfr::machine_epsilon(precision); } - + // Returns smallest eps such that x + eps != x (relative machine epsilon) inline static mpfr::mpreal epsilon(const mpfr::mpreal& x) { return mpfr::machine_epsilon(x); } @@ -2881,7 +2993,7 @@ namespace std { mp_rnd_t r = mpfr::mpreal::get_default_rnd(); - if(r == MPFR_RNDN) return mpfr::mpreal(0.5, precision); + if(r == GMP_RNDN) return mpfr::mpreal(0.5, precision); else return mpfr::mpreal(1.0, precision); } @@ -2896,10 +3008,30 @@ namespace std MPREAL_PERMISSIVE_EXPR static const int min_exponent10 = (int) (MPFR_EMIN_DEFAULT * 0.3010299956639811); MPREAL_PERMISSIVE_EXPR static const int max_exponent10 = (int) (MPFR_EMAX_DEFAULT * 0.3010299956639811); - // Should be constant according to standard, but 'digits' depends on precision in MPFR +#ifdef MPREAL_HAVE_DYNAMIC_STD_NUMERIC_LIMITS - inline static int digits() { return mpfr::mpreal::get_default_prec(); } - inline static int digits(const mpfr::mpreal& x) { return x.getPrecision(); } + // Following members should be constant according to standard, but they can be variable in MPFR + // So we define them as functions here. + // + // This is preferable way for std::numeric_limits specialization. + // But it is incompatible with standard std::numeric_limits and might not work with other libraries, e.g. boost. + // See below for compatible implementation. + inline static float_round_style round_style() + { + mp_rnd_t r = mpfr::mpreal::get_default_rnd(); + + switch (r) + { + case GMP_RNDN: return round_to_nearest; + case GMP_RNDZ: return round_toward_zero; + case GMP_RNDU: return round_toward_infinity; + case GMP_RNDD: return round_toward_neg_infinity; + default: return round_indeterminate; + } + } + + inline static int digits() { return int(mpfr::mpreal::get_default_prec()); } + inline static int digits(const mpfr::mpreal& x) { return x.getPrecision(); } inline static int digits10(mp_prec_t precision = mpfr::mpreal::get_default_prec()) { @@ -2915,6 +3047,25 @@ namespace std { return digits10(precision); } +#else + // Digits and round_style are NOT constants when it comes to mpreal. + // If possible, please use functions digits() and round_style() defined above. + // + // These (default) values are preserved for compatibility with existing libraries, e.g. boost. + // Change them accordingly to your application. + // + // For example, if you use 256 bits of precision uniformly in your program, then: + // digits = 256 + // digits10 = 77 + // max_digits10 = 78 + // + // Approximate formula for decimal digits is: digits10 = floor(log10(2) * digits). See bits2digits() for more details. + + static const std::float_round_style round_style = round_to_nearest; + static const int digits = 53; + static const int digits10 = 15; + static const int max_digits10 = 16; +#endif }; } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp index 13f92169e..de79f1538 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp @@ -104,9 +104,7 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const // 1) the roots found are correct // 2) the roots have distinct moduli - typedef typename POLYNOMIAL::Scalar Scalar; typedef typename REAL_ROOTS::Scalar Real; - typedef PolynomialSolver PolynomialSolverType; //Test realRoots std::vector< Real > calc_realRoots; diff --git a/gtsam/3rdparty/ceres/eigen.h b/gtsam/3rdparty/ceres/eigen.h index 18a602cf4..a25fde97f 100644 --- a/gtsam/3rdparty/ceres/eigen.h +++ b/gtsam/3rdparty/ceres/eigen.h @@ -31,7 +31,7 @@ #ifndef CERES_INTERNAL_EIGEN_H_ #define CERES_INTERNAL_EIGEN_H_ -#include +#include namespace ceres { diff --git a/gtsam/3rdparty/ceres/fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h index db1591636..455fce383 100644 --- a/gtsam/3rdparty/ceres/fixed_array.h +++ b/gtsam/3rdparty/ceres/fixed_array.h @@ -33,7 +33,7 @@ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #include -#include +#include #include #include diff --git a/gtsam/3rdparty/ceres/jet.h b/gtsam/3rdparty/ceres/jet.h index 12d4e8bc9..4a7683f50 100644 --- a/gtsam/3rdparty/ceres/jet.h +++ b/gtsam/3rdparty/ceres/jet.h @@ -162,7 +162,7 @@ #include #include -#include +#include #include namespace ceres { diff --git a/gtsam/3rdparty/gtsam_eigen_includes.h.in b/gtsam/3rdparty/gtsam_eigen_includes.h.in deleted file mode 100644 index f53e37f07..000000000 --- a/gtsam/3rdparty/gtsam_eigen_includes.h.in +++ /dev/null @@ -1,33 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file gtsam_eigen_includes.h - * @brief File to include the Eigen headers that we use - generated by CMake - * @author Richard Roberts - */ - -#pragma once - -#ifndef MKL_BLAS -#define MKL_BLAS MKL_DOMAIN_BLAS -#endif - -#cmakedefine EIGEN_USE_MKL_ALL // This is also defined in config.h -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Dense> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/QR> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD> -#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry> - - - - diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt index ea3dd0298..cc02a6ed2 100644 --- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt @@ -3,6 +3,7 @@ include_directories(.) # Find sources. file(GLOB metis_sources *.c) # Build libmetis. +add_definitions(-fPIC) add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources}) if(UNIX) target_link_libraries(metis m) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 0ebd6c07d..e26d85ff8 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -131,12 +131,6 @@ else() set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) endif() -# Set dataset paths -set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp" - APPEND PROPERTY COMPILE_DEFINITIONS - "SOURCE_TREE_DATASET_DIR=\"${PROJECT_SOURCE_DIR}/examples/Data\"" - "INSTALLED_DATASET_DIR=\"${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data\"") - # Special cases if(MSVC) set_property(SOURCE diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 336ea7e05..b8388057d 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -95,7 +95,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void save(Archive& ar, const unsigned int version) const + void save(Archive& ar, const unsigned int /*version*/) const { // Copy to an STL container and serialize that FastVector > map(this->size()); @@ -103,7 +103,7 @@ private: ar & BOOST_SERIALIZATION_NVP(map); } template - void load(Archive& ar, const unsigned int version) + void load(Archive& ar, const unsigned int /*version*/) { // Load into STL container and then fill our map FastVector > map; diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 78155d308..f01156bd6 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -129,7 +129,7 @@ public: protected: /// Assignment operator, protected because only the Value or DERIVED /// assignment operators should be used. - DerivedValue& operator=(const DerivedValue& rhs) { + DerivedValue& operator=(const DerivedValue& /*rhs*/) { // Nothing to do, do not call base class assignment operator return *this; } diff --git a/gtsam/base/FastDefaultAllocator.h b/gtsam/base/FastDefaultAllocator.h index 156a87f55..bf5cfc498 100644 --- a/gtsam/base/FastDefaultAllocator.h +++ b/gtsam/base/FastDefaultAllocator.h @@ -17,8 +17,7 @@ */ #pragma once - -#include +#include // Configuration from CMake #if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL # ifdef GTSAM_USE_TBB @@ -85,4 +84,4 @@ namespace gtsam }; } -} \ No newline at end of file +} diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 4b5d1caf1..380836d1d 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -74,7 +74,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 0a76c08b0..65d532191 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -19,9 +19,9 @@ #pragma once #include -#include #include #include +#include namespace gtsam { @@ -70,7 +70,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 69e841e57..8c23ae9e5 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,26 +18,22 @@ #pragma once -#include - -#include -#include -#include -#include -#include -#include #include #include +#include +#include -BOOST_MPL_HAS_XXX_TRAIT_DEF(print) +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { -// This is used internally to allow this container to be Testable even when it -// contains non-testable elements. -template -struct FastSetTestableHelper; - /** * FastSet is a thin wrapper around std::set that uses the boost * fast_pool_allocator instead of the default STL allocator. This is just a @@ -45,12 +41,16 @@ struct FastSetTestableHelper; * we've seen that the fast_pool_allocator can lead to speedups of several %. * @addtogroup base */ -template -class FastSet: public std::set, typename internal::FastDefaultAllocator::type> { +template +class FastSet: public std::set, + typename internal::FastDefaultAllocator::type> { + + BOOST_CONCEPT_ASSERT ((IsTestable )); public: - typedef std::set, typename internal::FastDefaultAllocator::type> Base; + typedef std::set, + typename internal::FastDefaultAllocator::type> Base; /** Default constructor */ FastSet() { @@ -59,23 +59,23 @@ public: /** Constructor from a range, passes through to base class */ template explicit FastSet(INPUTITERATOR first, INPUTITERATOR last) : - Base(first, last) { + Base(first, last) { } /** Constructor from a iterable container, passes through to base class */ template explicit FastSet(const INPUTCONTAINER& container) : - Base(container.begin(), container.end()) { + Base(container.begin(), container.end()) { } /** Copy constructor from another FastSet */ FastSet(const FastSet& x) : - Base(x) { + Base(x) { } /** Copy constructor from the base set class */ FastSet(const Base& x) : - Base(x) { + Base(x) { } #ifdef GTSAM_ALLOCATOR_BOOSTPOOL @@ -85,7 +85,7 @@ public: // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. if(x.size() > 0) - Base::insert(x.begin(), x.end()); + Base::insert(x.begin(), x.end()); } #endif @@ -95,81 +95,40 @@ public: } /** Handy 'exists' function */ - bool exists(const VALUE& e) const { return this->find(e) != this->end(); } + bool exists(const VALUE& e) const { + return this->find(e) != this->end(); + } - /** Print to implement Testable */ - void print(const std::string& str = "") const { FastSetTestableHelper::print(*this, str); } + /** Print to implement Testable: pretty basic */ + void print(const std::string& str = "") const { + for (typename Base::const_iterator it = this->begin(); it != this->end(); ++it) + traits::Print(*it, str); + } /** Check for equality within tolerance to implement Testable */ - bool equals(const FastSet& other, double tol = 1e-9) const { return FastSetTestableHelper::equals(*this, other, tol); } + bool equals(const FastSet& other, double tol = 1e-9) const { + typename Base::const_iterator it1 = this->begin(), it2 = other.begin(); + while (it1 != this->end()) { + if (it2 == other.end() || !traits::Equals(*it2, *it2, tol)) + return false; + ++it1; + ++it2; + } + return true; + } /** insert another set: handy for MATLAB access */ void merge(const FastSet& other) { - Base::insert(other.begin(),other.end()); + Base::insert(other.begin(), other.end()); } private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; -// This is the default Testable interface for *non*Testable elements, which -// uses stream operators. -template -struct FastSetTestableHelper { - - typedef FastSet Set; - - static void print(const Set& set, const std::string& str) { - std::cout << str << "\n"; - for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it) - std::cout << " " << *it << "\n"; - std::cout.flush(); - } - - static bool equals(const Set& set1, const Set& set2, double tol) { - typename Set::const_iterator it1 = set1.begin(); - typename Set::const_iterator it2 = set2.begin(); - while (it1 != set1.end()) { - if (it2 == set2.end() || - fabs((double)(*it1) - (double)(*it2)) > tol) - return false; - ++it1; - ++it2; - } - return true; - } -}; - -// This is the Testable interface for Testable elements -template -struct FastSetTestableHelper >::type> { - - typedef FastSet Set; - - static void print(const Set& set, const std::string& str) { - std::cout << str << "\n"; - for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it) - it->print(" "); - std::cout.flush(); - } - - static bool equals(const Set& set1, const Set& set2, double tol) { - typename Set::const_iterator it1 = set1.begin(); - typename Set::const_iterator it2 = set2.begin(); - while (it1 != set1.end()) { - if (it2 == set2.end() || - !it1->equals(*it2, tol)) - return false; - ++it1; - ++it2; - } - return true; - } -}; - } diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index be3eaa981..7d1cb970c 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -18,12 +18,10 @@ #pragma once -#include +#include #include #include - -#include -#include +#include namespace gtsam { @@ -35,20 +33,27 @@ namespace gtsam { * @addtogroup base */ template -class FastVector: public std::vector::type> { +class FastVector: public std::vector::type> { public: - typedef std::vector::type> Base; + typedef std::vector::type> Base; /** Default constructor */ - FastVector() {} + FastVector() { + } /** Constructor from size */ - explicit FastVector(size_t size) : Base(size) {} + explicit FastVector(size_t size) : + Base(size) { + } /** Constructor from size and initial values */ - explicit FastVector(size_t size, const VALUE& initial) : Base(size, initial) {} + explicit FastVector(size_t size, const VALUE& initial) : + Base(size, initial) { + } /** Constructor from a range, passes through to base class */ template @@ -56,33 +61,22 @@ public: // This if statement works around a bug in boost pool allocator and/or // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. - if(first != last) + if (first != last) Base::assign(first, last); } - /** Copy constructor from a FastList */ - FastVector(const FastList& x) { - if(x.size() > 0) - Base::assign(x.begin(), x.end()); - } - - /** Copy constructor from a FastSet */ - FastVector(const FastSet& x) { - if(x.size() > 0) - Base::assign(x.begin(), x.end()); - } - /** Copy constructor from the base class */ - FastVector(const Base& x) : Base(x) {} + FastVector(const Base& x) : + Base(x) { + } /** Copy constructor from a standard STL container */ template - FastVector(const std::vector& x) - { + FastVector(const std::vector& x) { // This if statement works around a bug in boost pool allocator and/or // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. - if(x.size() > 0) + if (x.size() > 0) Base::assign(x.begin(), x.end()); } @@ -95,7 +89,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index c67f7dd61..e83a64ba9 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include // operator typeid namespace gtsam { @@ -183,7 +183,7 @@ public: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("GenericValue", boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("value", value_); diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index aec4b5f1c..39541416e 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -13,16 +13,20 @@ * @file Group.h * * @brief Concept check class for variable types with Group properties - * @date Nov 5, 2011 + * @date November, 2011 * @author Alex Cunningham + * @author Frank Dellaert */ #pragma once +#include + #include #include #include #include +#include namespace gtsam { @@ -83,21 +87,119 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) { && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol); } -/// Macro to add group traits, additive flavor -#define GTSAM_ADDITIVE_GROUP(GROUP) \ - typedef additive_group_tag group_flavor; \ - static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \ - static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \ - static GROUP Inverse(const GROUP &g) { return -g;} +namespace internal { -/// Macro to add group traits, multiplicative flavor -#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \ - typedef additive_group_tag group_flavor; \ - static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \ - static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \ - static GROUP Inverse(const GROUP &g) { return g.inverse();} +/// A helper class that implements the traits interface for multiplicative groups. +/// Assumes existence of identity, operator* and inverse method +template +struct MultiplicativeGroupTraits { + typedef group_tag structure_category; + typedef multiplicative_group_tag group_flavor; + static Class Identity() { return Class::identity(); } + static Class Compose(const Class &g, const Class & h) { return g * h;} + static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} + static Class Inverse(const Class &g) { return g.inverse();} +}; -} // \ namespace gtsam +/// Both multiplicative group traits and Testable +template +struct MultiplicativeGroup : MultiplicativeGroupTraits, Testable {}; + +/// A helper class that implements the traits interface for additive groups. +/// Assumes existence of identity and three additive operators +template +struct AdditiveGroupTraits { + typedef group_tag structure_category; + typedef additive_group_tag group_flavor; + static Class Identity() { return Class::identity(); } + static Class Compose(const Class &g, const Class & h) { return g + h;} + static Class Between(const Class &g, const Class & h) { return h - g;} + static Class Inverse(const Class &g) { return -g;} +}; + +/// Both additive group traits and Testable +template +struct AdditiveGroup : AdditiveGroupTraits, Testable {}; + +} // namespace internal + +/// compose multiple times +template +BOOST_CONCEPT_REQUIRES(((IsGroup)),(G)) // +compose_pow(const G& g, size_t n) { + if (n == 0) return traits::Identity(); + else if (n == 1) return g; + else return traits::Compose(compose_pow(g, n - 1), g); +} + +/// Template to construct the direct product of two arbitrary groups +/// Assumes nothing except group structure and Testable from G and H +template +class DirectProduct: public std::pair { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); + +public: + /// Default constructor yields identity + DirectProduct():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + DirectProduct(const G& g, const H& h):std::pair(g,h) {} + + // identity + static DirectProduct identity() { return DirectProduct(); } + + DirectProduct operator*(const DirectProduct& other) const { + return DirectProduct(traits::Compose(this->first, other.first), + traits::Compose(this->second, other.second)); + } + DirectProduct inverse() const { + return DirectProduct(this->first.inverse(), this->second.inverse()); + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : + internal::MultiplicativeGroupTraits > {}; + +/// Template to construct the direct sum of two additive groups +/// Assumes existence of three additive operators for both groups +template +class DirectSum: public std::pair { + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + + const G& g() const { return this->first; } + const H& h() const { return this->second;} + +public: + /// Default constructor yields identity + DirectSum():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + DirectSum(const G& g, const H& h):std::pair(g,h) {} + + // identity + static DirectSum identity() { return DirectSum(); } + + DirectSum operator+(const DirectSum& other) const { + return DirectSum(g()+other.g(), h()+other.h()); + } + DirectSum operator-(const DirectSum& other) const { + return DirectSum(g()-other.g(), h()-other.h()); + } + DirectSum operator-() const { + return DirectSum(- g(), - h()); + } +}; + +// Define direct sums to be a model of the Additive Group concept +template +struct traits > : + internal::AdditiveGroupTraits > {}; + +} // namespace gtsam /** * Macros for using the IsGroup diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index bf2056cc8..05c7bc20f 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -136,8 +136,10 @@ namespace internal { /// A helper class that implements the traits interface for GTSAM lie groups. /// To use this for your gtsam type, define: /// template<> struct traits : public internal::LieGroupTraits {}; +/// Assumes existence of: identity, dimension, localCoordinates, retract, +/// and additionally Logmap, Expmap, compose, between, and inverse template -struct LieGroupTraits : Testable { +struct LieGroupTraits { typedef lie_group_tag structure_category; /// @name Group @@ -167,12 +169,10 @@ struct LieGroupTraits : Testable { ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { return origin.retract(v, Horigin, Hv); } - /// @} /// @name Lie Group /// @{ - static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { return Class::Logmap(m, Hm); } @@ -195,11 +195,12 @@ struct LieGroupTraits : Testable { ChartJacobian H = boost::none) { return m.inverse(H); } - /// @} - }; +/// Both LieGroupTraits and Testable +template struct LieGroup: LieGroupTraits, Testable {}; + } // \ namepsace internal /** @@ -248,7 +249,7 @@ public: // log and exp map without Jacobians g = traits::Expmap(v); v = traits::Logmap(g); - // log and expnential map with Jacobians + // log and exponential map with Jacobians g = traits::Expmap(v, Hg); v = traits::Logmap(g, Hg); } diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp index 69054fc1c..cf5b57536 100644 --- a/gtsam/base/LieMatrix.cpp +++ b/gtsam/base/LieMatrix.cpp @@ -15,7 +15,7 @@ * @author Richard Roberts and Alex Cunningham */ -#include +#include namespace gtsam { diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 90b7207a2..e24f339e4 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -11,147 +11,16 @@ /** * @file LieMatrix.h - * @brief A wrapper around Matrix providing Lie compatibility - * @author Richard Roberts and Alex Cunningham + * @brief External deprecation warning, see LieMatrix_Deprecated.h for details + * @author Paul Drews */ #pragma once -#include - #ifdef _MSC_VER #pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.") #else #warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead." #endif -#include -#include - -namespace gtsam { - -/** - * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ -struct LieMatrix : public Matrix { - - /// @name Constructors - /// @{ - enum { dimension = Eigen::Dynamic }; - - /** default constructor - only for serialize */ - LieMatrix() {} - - /** initialize from a normal matrix */ - LieMatrix(const Matrix& v) : Matrix(v) {} - - template - LieMatrix(const M& v) : Matrix(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} -#endif - - /** constructor with size and initial data, row order ! */ - LieMatrix(size_t m, size_t n, const double* const data) : - Matrix(Eigen::Map(data, m, n)) {} - - /// @} - /// @name Testable interface - /// @{ - - /** print @param s optional string naming the object */ - GTSAM_EXPORT void print(const std::string& name="") const; - - /** equality up to tolerance */ - inline bool equals(const LieMatrix& expected, double tol=1e-5) const { - return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** get the underlying matrix */ - inline Matrix matrix() const { - return static_cast(*this); - } - - /// @} - - /// @name Group - /// @{ - LieMatrix compose(const LieMatrix& q) { return (*this)+q;} - LieMatrix between(const LieMatrix& q) { return q-(*this);} - LieMatrix inverse() { return -(*this);} - /// @} - - /// @name Manifold - /// @{ - Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} - LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} - /// @} - - /// @name Lie Group - /// @{ - static Vector Logmap(const LieMatrix& p) {return p.vector();} - static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} - /// @} - - /// @name VectorSpace requirements - /// @{ - - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return size(); } - - /** Convert to vector, is done row-wise - TODO why? */ - inline Vector vector() const { - Vector result(size()); - typedef Eigen::Matrix RowMajor; - Eigen::Map(&result(0), rows(), cols()) = *this; - return result; - } - - /** identity - NOTE: no known size at compile time - so zero length */ - inline static LieMatrix identity() { - throw std::runtime_error("LieMatrix::identity(): Don't use this function"); - return LieMatrix(); - } - /// @} - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Matrix", - boost::serialization::base_object(*this)); - - } - -}; - - -template<> -struct traits : public internal::VectorSpace { - - // Override Retract, as the default version does not know how to initialize - static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { - if (H1) *H1 = Eye(origin); - if (H2) *H2 = Eye(origin); - typedef const Eigen::Matrix RowMajor; - return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); - } - -}; - -} // \namespace gtsam +#include "gtsam/base/LieMatrix_Deprecated.h" diff --git a/gtsam/base/LieMatrix_Deprecated.h b/gtsam/base/LieMatrix_Deprecated.h new file mode 100644 index 000000000..15b4401f2 --- /dev/null +++ b/gtsam/base/LieMatrix_Deprecated.h @@ -0,0 +1,151 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieMatrix.h + * @brief A wrapper around Matrix providing Lie compatibility + * @author Richard Roberts and Alex Cunningham + */ + +#pragma once + +#include + +#include +#include + +namespace gtsam { + +/** + * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. + */ +struct LieMatrix : public Matrix { + + /// @name Constructors + /// @{ + enum { dimension = Eigen::Dynamic }; + + /** default constructor - only for serialize */ + LieMatrix() {} + + /** initialize from a normal matrix */ + LieMatrix(const Matrix& v) : Matrix(v) {} + + template + LieMatrix(const M& v) : Matrix(v) {} + +// Currently TMP constructor causes ICE on MSVS 2013 +#if (_MSC_VER < 1800) + /** initialize from a fixed size normal vector */ + template + LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} +#endif + + /** constructor with size and initial data, row order ! */ + LieMatrix(size_t m, size_t n, const double* const data) : + Matrix(Eigen::Map(data, m, n)) {} + + /// @} + /// @name Testable interface + /// @{ + + /** print @param s optional string naming the object */ + GTSAM_EXPORT void print(const std::string& name="") const; + + /** equality up to tolerance */ + inline bool equals(const LieMatrix& expected, double tol=1e-5) const { + return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); + } + + /// @} + /// @name Standard Interface + /// @{ + + /** get the underlying matrix */ + inline Matrix matrix() const { + return static_cast(*this); + } + + /// @} + + /// @name Group + /// @{ + LieMatrix compose(const LieMatrix& q) { return (*this)+q;} + LieMatrix between(const LieMatrix& q) { return q-(*this);} + LieMatrix inverse() { return -(*this);} + /// @} + + /// @name Manifold + /// @{ + Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} + LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} + /// @} + + /// @name Lie Group + /// @{ + static Vector Logmap(const LieMatrix& p) {return p.vector();} + static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} + /// @} + + /// @name VectorSpace requirements + /// @{ + + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return size(); } + + /** Convert to vector, is done row-wise - TODO why? */ + inline Vector vector() const { + Vector result(size()); + typedef Eigen::Matrix RowMajor; + Eigen::Map(&result(0), rows(), cols()) = *this; + return result; + } + + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieMatrix identity() { + throw std::runtime_error("LieMatrix::identity(): Don't use this function"); + return LieMatrix(); + } + /// @} + +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("Matrix", + boost::serialization::base_object(*this)); + + } + +}; + + +template<> +struct traits : public internal::VectorSpace { + + // Override Retract, as the default version does not know how to initialize + static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(origin); + if (H2) *H2 = Eye(origin); + typedef const Eigen::Matrix RowMajor; + return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); + } + +}; + +} // \namespace gtsam diff --git a/gtsam/base/LieScalar.cpp b/gtsam/base/LieScalar.cpp index 5f59c29bc..4c5a6a257 100644 --- a/gtsam/base/LieScalar.cpp +++ b/gtsam/base/LieScalar.cpp @@ -8,7 +8,7 @@ -#include +#include namespace gtsam { void LieScalar::print(const std::string& name) const { diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 9f6c56b28..650e2bb21 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -11,7 +11,7 @@ /** * @file LieScalar.h - * @brief A wrapper around scalar providing Lie compatibility + * @brief External deprecation warning, see LieScalar_Deprecated.h for details * @author Kai Ni */ @@ -23,69 +23,4 @@ #warning "LieScalar.h is deprecated. Please use double/float instead." #endif -#include -#include - -namespace gtsam { - - /** - * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ - struct GTSAM_EXPORT LieScalar { - - enum { dimension = 1 }; - - /** default constructor */ - LieScalar() : d_(0.0) {} - - /** wrap a double */ - /*explicit*/ LieScalar(double d) : d_(d) {} - - /** access the underlying value */ - double value() const { return d_; } - - /** Automatic conversion to underlying value */ - operator double() const { return d_; } - - /** convert vector */ - Vector1 vector() const { Vector1 v; v< - struct traits : public internal::ScalarTraits {}; - -} // \namespace gtsam +#include diff --git a/gtsam/base/LieScalar_Deprecated.h b/gtsam/base/LieScalar_Deprecated.h new file mode 100644 index 000000000..6ffc76d37 --- /dev/null +++ b/gtsam/base/LieScalar_Deprecated.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieScalar.h + * @brief A wrapper around scalar providing Lie compatibility + * @author Kai Ni + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. + */ + struct GTSAM_EXPORT LieScalar { + + enum { dimension = 1 }; + + /** default constructor */ + LieScalar() : d_(0.0) {} + + /** wrap a double */ + /*explicit*/ LieScalar(double d) : d_(d) {} + + /** access the underlying value */ + double value() const { return d_; } + + /** Automatic conversion to underlying value */ + operator double() const { return d_; } + + /** convert vector */ + Vector1 vector() const { Vector1 v; v< + struct traits : public internal::ScalarTraits {}; + +} // \namespace gtsam diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index 84afdabc8..3e4eeecf2 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -15,8 +15,8 @@ * @author Alex Cunningham */ +#include #include -#include using namespace std; diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index c2003df3c..813431775 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -11,8 +11,8 @@ /** * @file LieVector.h - * @brief A wrapper around vector providing Lie compatibility - * @author Alex Cunningham + * @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details. + * @author Paul Drews */ #pragma once @@ -23,100 +23,4 @@ #warning "LieVector.h is deprecated. Please use Eigen::Vector instead." #endif -#include - -namespace gtsam { - -/** - * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ -struct LieVector : public Vector { - - enum { dimension = Eigen::Dynamic }; - - /** default constructor - should be unnecessary */ - LieVector() {} - - /** initialize from a normal vector */ - LieVector(const Vector& v) : Vector(v) {} - - template - LieVector(const V& v) : Vector(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieVector(const Eigen::Matrix& v) : Vector(v) {} -#endif - - /** wrap a double */ - LieVector(double d) : Vector((Vector(1) << d).finished()) {} - - /** constructor with size and initial data, row order ! */ - GTSAM_EXPORT LieVector(size_t m, const double* const data); - - /// @name Testable - /// @{ - GTSAM_EXPORT void print(const std::string& name="") const; - bool equals(const LieVector& expected, double tol=1e-5) const { - return gtsam::equal(vector(), expected.vector(), tol); - } - /// @} - - /// @name Group - /// @{ - LieVector compose(const LieVector& q) { return (*this)+q;} - LieVector between(const LieVector& q) { return q-(*this);} - LieVector inverse() { return -(*this);} - /// @} - - /// @name Manifold - /// @{ - Vector localCoordinates(const LieVector& q) { return between(q).vector();} - LieVector retract(const Vector& v) {return compose(LieVector(v));} - /// @} - - /// @name Lie Group - /// @{ - static Vector Logmap(const LieVector& p) {return p.vector();} - static LieVector Expmap(const Vector& v) { return LieVector(v);} - /// @} - - /// @name VectorSpace requirements - /// @{ - - /** get the underlying vector */ - Vector vector() const { - return static_cast(*this); - } - - /** Returns dimensionality of the tangent space */ - size_t dim() const { return this->size(); } - - /** identity - NOTE: no known size at compile time - so zero length */ - static LieVector identity() { - throw std::runtime_error("LieVector::identity(): Don't use this function"); - return LieVector(); - } - - /// @} - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Vector", - boost::serialization::base_object(*this)); - } -}; - - -template<> -struct traits : public internal::VectorSpace {}; - -} // \namespace gtsam +#include diff --git a/gtsam/base/LieVector_Deprecated.h b/gtsam/base/LieVector_Deprecated.h new file mode 100644 index 000000000..67c42c748 --- /dev/null +++ b/gtsam/base/LieVector_Deprecated.h @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieVector.h + * @brief A wrapper around vector providing Lie compatibility + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. + */ +struct LieVector : public Vector { + + enum { dimension = Eigen::Dynamic }; + + /** default constructor - should be unnecessary */ + LieVector() {} + + /** initialize from a normal vector */ + LieVector(const Vector& v) : Vector(v) {} + + template + LieVector(const V& v) : Vector(v) {} + +// Currently TMP constructor causes ICE on MSVS 2013 +#if (_MSC_VER < 1800) + /** initialize from a fixed size normal vector */ + template + LieVector(const Eigen::Matrix& v) : Vector(v) {} +#endif + + /** wrap a double */ + LieVector(double d) : Vector((Vector(1) << d).finished()) {} + + /** constructor with size and initial data, row order ! */ + GTSAM_EXPORT LieVector(size_t m, const double* const data); + + /// @name Testable + /// @{ + GTSAM_EXPORT void print(const std::string& name="") const; + bool equals(const LieVector& expected, double tol=1e-5) const { + return gtsam::equal(vector(), expected.vector(), tol); + } + /// @} + + /// @name Group + /// @{ + LieVector compose(const LieVector& q) { return (*this)+q;} + LieVector between(const LieVector& q) { return q-(*this);} + LieVector inverse() { return -(*this);} + /// @} + + /// @name Manifold + /// @{ + Vector localCoordinates(const LieVector& q) { return between(q).vector();} + LieVector retract(const Vector& v) {return compose(LieVector(v));} + /// @} + + /// @name Lie Group + /// @{ + static Vector Logmap(const LieVector& p) {return p.vector();} + static LieVector Expmap(const Vector& v) { return LieVector(v);} + /// @} + + /// @name VectorSpace requirements + /// @{ + + /** get the underlying vector */ + Vector vector() const { + return static_cast(*this); + } + + /** Returns dimensionality of the tangent space */ + size_t dim() const { return this->size(); } + + /** identity - NOTE: no known size at compile time - so zero length */ + static LieVector identity() { + throw std::runtime_error("LieVector::identity(): Don't use this function"); + return LieVector(); + } + + /// @} + +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("Vector", + boost::serialization::base_object(*this)); + } +}; + + +template<> +struct traits : public internal::VectorSpace {}; + +} // \namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 6998b3b18..d7ea9ea4c 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -46,7 +46,7 @@ struct manifold_tag {}; * There may be multiple possible retractions for a given manifold, which can be chosen * between depending on the computational complexity. The important criteria for * the creation for the retract and localCoordinates functions is that they be - * inverse operations. The new notion of a Chart guarantees that. + * inverse operations. * */ @@ -90,9 +90,9 @@ struct ManifoldImpl { /// A helper that implements the traits interface for GTSAM manifolds. /// To use this for your class type, define: -/// template<> struct traits : public Manifold { }; +/// template<> struct traits : public internal::ManifoldTraits { }; template -struct Manifold: Testable, ManifoldImpl { +struct ManifoldTraits: ManifoldImpl { // Check that Class has the necessary machinery BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); @@ -116,6 +116,9 @@ struct Manifold: Testable, ManifoldImpl { } }; +/// Both ManifoldTraits and Testable +template struct Manifold: ManifoldTraits, Testable {}; + } // \ namespace internal /// Check invariants for Manifold type @@ -135,7 +138,7 @@ class IsManifold { public: typedef typename traits::structure_category structure_category_tag; - static const size_t dim = traits::dimension; + static const int dim = traits::dimension; typedef typename traits::ManifoldType ManifoldType; typedef typename traits::TangentVector TangentVector; @@ -165,6 +168,53 @@ struct FixedDimension { "FixedDimension instantiated for dymanically-sized type."); }; +/// Helper class to construct the product manifold of two other manifolds, M1 and M2 +/// Assumes nothing except manifold structure from M1 and M2 +template +class ProductManifold: public std::pair { + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + +protected: + enum { dimension1 = traits::dimension }; + enum { dimension2 = traits::dimension }; + +public: + enum { dimension = dimension1 + dimension2 }; + inline static size_t Dim() { return dimension;} + inline size_t dim() const { return dimension;} + + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + /// Default constructor yields identity + ProductManifold():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two original manifold values + ProductManifold(const M1& m1, const M2& m2):std::pair(m1,m2) {} + + /// Retract delta to manifold + ProductManifold retract(const TangentVector& xi) const { + M1 m1 = traits::Retract(this->first, xi.template head()); + M2 m2 = traits::Retract(this->second, xi.template tail()); + return ProductManifold(m1,m2); + } + + /// Compute the coordinates in the tangent space + TangentVector localCoordinates(const ProductManifold& other) const { + typename traits::TangentVector v1 = traits::Local(this->first, other.first); + typename traits::TangentVector v2 = traits::Local(this->second, other.second); + TangentVector v; + v << v1, v2; + return v; + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::Manifold > { +}; + } // \ namespace gtsam ///** diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 7bcd32b9f..9e56dfb6c 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -180,6 +182,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec } /* ************************************************************************* */ +//3 argument call void print(const Matrix& A, const string &s, ostream& stream) { size_t m = A.rows(), n = A.cols(); @@ -198,6 +201,12 @@ void print(const Matrix& A, const string &s, ostream& stream) { stream << "];" << endl; } +/* ************************************************************************* */ +//1 or 2 argument call +void print(const Matrix& A, const string &s){ + print(A, s, cout); +} + /* ************************************************************************* */ void save(const Matrix& A, const string &s, const string& filename) { fstream stream(filename.c_str(), fstream::out | fstream::app); @@ -697,6 +706,26 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, return ss.str(); } +/* ************************************************************************* */ +void inplace_QR(Matrix& A){ + size_t rows = A.rows(); + size_t cols = A.cols(); + size_t size = std::min(rows,cols); + typedef Eigen::internal::plain_diag_type::type HCoeffsType; + typedef Eigen::internal::plain_row_type::type RowVectorType; + HCoeffsType hCoeffs(size); + RowVectorType temp(cols); + +#ifdef GTSAM_USE_SYSTEM_EIGEN + // System-Eigen is used, and MKL is off + Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data()); +#else + // Patched Eigen is used, and MKL is either on or off + Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data()); +#endif + + zeroBelowDiagonal(A); +} } // namespace gtsam diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index c3cbfa341..41ffa27b5 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -21,11 +21,16 @@ // \callgraph #pragma once - #include +#include // Configuration from CMake + +#include +#include +#include +#include #include #include -#include + /** * Matrix is a typedef in the gtsam namespace @@ -198,9 +203,14 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) { } /** - * print a matrix + * print without optional string, must specify cout yourself */ -GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout); +GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream); + +/** + * print with optional string to cout + */ +GTSAM_EXPORT void print(const Matrix& A, const std::string& s = ""); /** * save a matrix to file, which can be loaded by matlab @@ -367,21 +377,7 @@ GTSAM_EXPORT std::pair qr(const Matrix& A); * @param A is the input matrix, and is the output * @param clear_below_diagonal enables zeroing out below diagonal */ -template -void inplace_QR(MATRIX& A) { - size_t rows = A.rows(); - size_t cols = A.cols(); - size_t size = std::min(rows,cols); - - typedef Eigen::internal::plain_diag_type::type HCoeffsType; - typedef Eigen::internal::plain_row_type::type RowVectorType; - HCoeffsType hCoeffs(size); - RowVectorType temp(cols); - - Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data()); - - zeroBelowDiagonal(A); -} +void inplace_QR(Matrix& A); /** * Imperative algorithm for in-place full elimination with @@ -541,7 +537,7 @@ namespace boost { // split version - sends sizes ahead template - void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) { + void save(Archive & ar, const gtsam::Matrix & m, unsigned int /*version*/) { const size_t rows = m.rows(), cols = m.cols(); ar << BOOST_SERIALIZATION_NVP(rows); ar << BOOST_SERIALIZATION_NVP(cols); @@ -549,7 +545,7 @@ namespace boost { } template - void load(Archive & ar, gtsam::Matrix & m, unsigned int version) { + void load(Archive & ar, gtsam::Matrix & m, unsigned int /*version*/) { size_t rows, cols; ar >> BOOST_SERIALIZATION_NVP(rows); ar >> BOOST_SERIALIZATION_NVP(cols); diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 2ea9d672e..d0e2297ef 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -18,8 +18,8 @@ */ #pragma once - -#include +#include // Configuration from CMake +#include #ifndef OPTIONALJACOBIAN_NOBOOST #include @@ -83,7 +83,7 @@ public: #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t none) : + OptionalJacobian(boost::none_t /*none*/) : map_(NULL) { } @@ -142,7 +142,7 @@ public: #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t none) : + OptionalJacobian(boost::none_t /*none*/) : pointer_(NULL) { } @@ -168,5 +168,20 @@ public: Jacobian* operator->(){ return pointer_; } }; +// forward declare +template struct traits; + +/** + * @brief: meta-function to generate JacobianTA optional reference + * Used mainly by Expressions + * @param T return type + * @param A argument type + */ +template +struct MakeOptionalJacobian { + typedef OptionalJacobian::dimension, + traits::dimension> type; +}; + } // namespace gtsam diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h new file mode 100644 index 000000000..90aeb54d1 --- /dev/null +++ b/gtsam/base/ProductLieGroup.h @@ -0,0 +1,197 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------1------------------------------------------- */ + +/** + * @file ProductLieGroup.h + * @date May, 2015 + * @author Frank Dellaert + * @brief Group product of two Lie Groups + */ + +#pragma once + +#include +#include // pair + +namespace gtsam { + +/// Template to construct the product Lie group of two other Lie groups +/// Assumes Lie group structure for G and H +template +class ProductLieGroup: public std::pair { + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + typedef std::pair Base; + +protected: + enum {dimension1 = traits::dimension}; + enum {dimension2 = traits::dimension}; + +public: + /// Default constructor yields identity + ProductLieGroup():Base(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductLieGroup(const G& g, const H& h):Base(g,h) {} + + // Construct from base + ProductLieGroup(const Base& base):Base(base) {} + + /// @name Group + /// @{ + typedef multiplicative_group_tag group_flavor; + static ProductLieGroup identity() {return ProductLieGroup();} + + ProductLieGroup operator*(const ProductLieGroup& other) const { + return ProductLieGroup(traits::Compose(this->first,other.first), + traits::Compose(this->second,other.second)); + } + ProductLieGroup inverse() const { + return ProductLieGroup(this->first.inverse(), this->second.inverse()); + } + ProductLieGroup compose(const ProductLieGroup& g) const { + return (*this) * g; + } + ProductLieGroup between(const ProductLieGroup& g) const { + return this->inverse() * g; + } + /// @} + + /// @name Manifold + /// @{ + enum {dimension = dimension1 + dimension2}; + inline static size_t Dim() {return dimension;} + inline size_t dim() const {return dimension;} + + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + static ProductLieGroup Retract(const TangentVector& v) { + return ProductLieGroup::ChartAtOrigin::Retract(v); + } + static TangentVector LocalCoordinates(const ProductLieGroup& g) { + return ProductLieGroup::ChartAtOrigin::Local(g); + } + ProductLieGroup retract(const TangentVector& v) const { + return compose(ProductLieGroup::ChartAtOrigin::Retract(v)); + } + TangentVector localCoordinates(const ProductLieGroup& g) const { + return ProductLieGroup::ChartAtOrigin::Local(between(g)); + } + /// @} + + /// @name Lie Group + /// @{ +protected: + typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix Jacobian1; + typedef Eigen::Matrix Jacobian2; + +public: + ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup inverse(ChartJacobian D) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Inverse(this->first, D ? &D_g_first : 0); + H h = traits::Inverse(this->second, D ? &D_h_second : 0); + if (D) { + D->setZero(); + D->template topLeftCorner() = D_g_first; + D->template bottomRightCorner() = D_h_second; + } + return ProductLieGroup(g,h); + } + static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet"); + G g = traits::Expmap(v.template head()); + H h = traits::Expmap(v.template tail()); + return ProductLieGroup(g,h); + } + static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet"); + typename traits::TangentVector v1 = traits::Logmap(p.first); + typename traits::TangentVector v2 = traits::Logmap(p.second); + TangentVector v; + v << v1, v2; + return v; + } + struct ChartAtOrigin { + static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) { + return Logmap(m, Hm); + } + static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) { + return Expmap(v, Hv); + } + }; + ProductLieGroup expmap(const TangentVector& v) const { + return compose(ProductLieGroup::Expmap(v)); + } + TangentVector logmap(const ProductLieGroup& g) const { + return ProductLieGroup::Logmap(between(g)); + } + static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) { + return ProductLieGroup::ChartAtOrigin::Retract(v,H1); + } + static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) { + return ProductLieGroup::ChartAtOrigin::Local(g,H1); + } + ProductLieGroup retract(const TangentVector& v, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Jacobian D_g_v; + ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); + ProductLieGroup h = compose(g,H1,H2); + if (H2) *H2 = (*H2) * D_g_v; + return h; + } + TangentVector localCoordinates(const ProductLieGroup& g, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ProductLieGroup h = between(g,H1,H2); + Jacobian D_v_h; + TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); + if (H1) *H1 = D_v_h * (*H1); + if (H2) *H2 = D_v_h * (*H2); + return v; + } + /// @} + +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::LieGroupTraits< + ProductLieGroup > { +}; +} // namespace gtsam + diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 546b6a7f1..7cca63092 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -20,6 +20,7 @@ #include #include #include +#include namespace gtsam { @@ -61,13 +62,13 @@ VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( // Split conditional - // Create one big conditionals with many frontal variables. - gttic(Construct_conditional); - const size_t varDim = offset(nFrontals); - VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); - Ab.full() = matrix_.topRows(varDim); - Ab.full().triangularView().setZero(); - gttoc(Construct_conditional); + // Create one big conditionals with many frontal variables. + gttic(Construct_conditional); + const size_t varDim = offset(nFrontals); + VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); + Ab.full() = matrix_.topRows(varDim); + Ab.full().triangularView().setZero(); + gttoc(Construct_conditional); gttic(Remaining_factor); // Take lower-right block of Ab_ to get the remaining factor diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 5dcc79a68..1f81ca1f9 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -17,9 +17,21 @@ */ #pragma once -#include #include +#include #include +#include +#include +#include +#include +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { @@ -199,6 +211,7 @@ namespace gtsam { void checkBlock(DenseIndex block) const { + static_cast(block); //Disable unused varibale warnings. assert(matrix_.rows() == matrix_.cols()); assert(matrix_.cols() == variableColOffsets_.back()); assert(block >= 0); @@ -235,7 +248,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // Fill in the lower triangle part of the matrix, so boost::serialization won't // complain about uninitialized data with an input_stream_error exception // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error @@ -246,13 +259,8 @@ namespace gtsam { } }; - /* ************************************************************************* */ - class CholeskyFailed : public gtsam::ThreadsafeException - { - public: - CholeskyFailed() throw() {} - virtual ~CholeskyFailed() throw() {} - }; + /// Foward declare exception class + class CholeskyFailed; } diff --git a/gtsam/base/SymmetricBlockMatrixBlockExpr.h b/gtsam/base/SymmetricBlockMatrixBlockExpr.h index 6a65b2748..dd999ae6c 100644 --- a/gtsam/base/SymmetricBlockMatrixBlockExpr.h +++ b/gtsam/base/SymmetricBlockMatrixBlockExpr.h @@ -93,7 +93,7 @@ namespace gtsam /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a /// SymmetricBlockMatrix. - SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char dummy) : + SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char /*dummy*/) : xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0)) { initIndices(firstBlock, firstBlock, blocks, blocks); diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 04d3fc676..f976be0e7 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -17,13 +17,14 @@ #pragma once -#include -#include -#include +#include +#include + #include #include -#include -#include +#include +#include +#include namespace gtsam { diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h new file mode 100644 index 000000000..d464e9f21 --- /dev/null +++ b/gtsam/base/ThreadsafeException.h @@ -0,0 +1,155 @@ +/* ---------------------------------------------------------------------------- + + * 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 ThreadSafeException.h + * @brief Base exception type that uses tbb_exception if GTSAM is compiled with TBB + * @author Richard Roberts + * @date Aug 21, 2010 + * @addtogroup base + */ + +#pragma once + +#include // for GTSAM_USE_TBB + +#include +#include +#include +#include + +#ifdef GTSAM_USE_TBB +#include +#include +#include +#endif + +namespace gtsam { + +/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. +template +class ThreadsafeException: +#ifdef GTSAM_USE_TBB + public tbb::tbb_exception +#else +public std::exception +#endif +{ +#ifdef GTSAM_USE_TBB +private: + typedef tbb::tbb_exception Base; +protected: + typedef std::basic_string, + tbb::tbb_allocator > String; +#else +private: + typedef std::exception Base; +protected: + typedef std::string String; +#endif + +protected: + bool dynamic_; ///< Whether this object was moved + mutable boost::optional description_; ///< Optional description + + /// Default constructor is protected - may only be created from derived classes + ThreadsafeException() : + dynamic_(false) { + } + + /// Copy constructor is protected - may only be created from derived classes + ThreadsafeException(const ThreadsafeException& other) : + Base(other), dynamic_(false) { + } + + /// Construct with description string + ThreadsafeException(const std::string& description) : + dynamic_(false), description_( + String(description.begin(), description.end())) { + } + + /// Default destructor doesn't have the throw() + virtual ~ThreadsafeException() throw () { + } + +public: + // Implement functions for tbb_exception +#ifdef GTSAM_USE_TBB + virtual tbb::tbb_exception* move() throw () { + void* cloneMemory = scalable_malloc(sizeof(DERIVED)); + if (!cloneMemory) { + std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; + exit(-1); + } + DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); + clone->dynamic_ = true; + return clone; + } + + virtual void destroy() throw () { + if (dynamic_) { + DERIVED* derivedPtr = static_cast(this); + derivedPtr->~DERIVED(); + scalable_free(derivedPtr); + } + } + + virtual void throw_self() { + throw *static_cast(this); + } + + virtual const char* name() const throw () { + return typeid(DERIVED).name(); + } +#endif + + virtual const char* what() const throw () { + return description_ ? description_->c_str() : ""; + } +}; + +/// Thread-safe runtime error exception +class RuntimeErrorThreadsafe: public ThreadsafeException { +public: + /// Construct with a string describing the exception + RuntimeErrorThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Thread-safe out of range exception +class OutOfRangeThreadsafe: public ThreadsafeException { +public: + /// Construct with a string describing the exception + OutOfRangeThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Thread-safe invalid argument exception +class InvalidArgumentThreadsafe: public ThreadsafeException< + InvalidArgumentThreadsafe> { +public: + /// Construct with a string describing the exception + InvalidArgumentThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Indicate Cholesky factorization failure +class CholeskyFailed : public gtsam::ThreadsafeException +{ +public: + CholeskyFailed() throw() {} + virtual ~CholeskyFailed() throw() {} +}; + +} // namespace gtsam diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index a12f453f4..9537baa31 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -18,9 +18,10 @@ #pragma once -#include -#include #include +#include +#include + namespace gtsam { @@ -120,7 +121,7 @@ namespace gtsam { virtual Vector localCoordinates_(const Value& value) const = 0; /** Assignment operator */ - virtual Value& operator=(const Value& rhs) { + virtual Value& operator=(const Value& /*rhs*/) { //needs a empty definition so recursion in implicit derived assignment operators work return *this; } @@ -165,7 +166,7 @@ namespace gtsam { * */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) { } }; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index a9ef8fe10..ed6373f5b 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -16,20 +16,18 @@ * @author Frank Dellaert */ +#include +#include +#include +#include #include #include -#include #include #include #include #include -#include -#include -#include #include - -#include - +#include using namespace std; @@ -55,6 +53,7 @@ Vector delta(size_t n, size_t i, double value) { } /* ************************************************************************* */ +//3 argument call void print(const Vector& v, const string& s, ostream& stream) { size_t n = v.size(); @@ -65,6 +64,12 @@ void print(const Vector& v, const string& s, ostream& stream) { stream << "];" << endl; } +/* ************************************************************************* */ +//1 or 2 argument call +void print(const Vector& v, const string& s) { + print(v, s, cout); +} + /* ************************************************************************* */ void save(const Vector& v, const string &s, const string& filename) { fstream stream(filename.c_str(), fstream::out | fstream::app); @@ -217,11 +222,7 @@ double norm_2(const Vector& v) { /* ************************************************************************* */ Vector reciprocal(const Vector &a) { - size_t n = a.size(); - Vector b(n); - for( size_t i = 0; i < n; i++ ) - b(i) = 1.0/a(i); - return b; + return a.array().inverse(); } /* ************************************************************************* */ diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 74cd248a1..20477a205 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -18,13 +18,17 @@ // \callgraph + #pragma once -#include -#include -#include +#ifndef MKL_BLAS +#define MKL_BLAS MKL_DOMAIN_BLAS +#endif + #include -#include +#include +#include +#include namespace gtsam { @@ -97,9 +101,14 @@ GTSAM_EXPORT bool zero(const Vector& v); inline size_t dim(const Vector& v) { return v.size(); } /** - * print with optional string + * print without optional string, must specify cout yourself */ -GTSAM_EXPORT void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout); +GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream); + +/** + * print with optional string to cout + */ +GTSAM_EXPORT void print(const Vector& v, const std::string& s = ""); /** * save a vector to file, which can be loaded by matlab @@ -346,14 +355,14 @@ namespace boost { // split version - copies into an STL vector for serialization template - void save(Archive & ar, const gtsam::Vector & v, unsigned int version) { + void save(Archive & ar, const gtsam::Vector & v, unsigned int /*version*/) { const size_t size = v.size(); ar << BOOST_SERIALIZATION_NVP(size); ar << make_nvp("data", make_array(v.data(), v.size())); } template - void load(Archive & ar, gtsam::Vector & v, unsigned int version) { + void load(Archive & ar, gtsam::Vector & v, unsigned int /*version*/) { size_t size; ar >> BOOST_SERIALIZATION_NVP(size); v.resize(size); @@ -362,12 +371,12 @@ namespace boost { // split version - copies into an STL vector for serialization template - void save(Archive & ar, const Eigen::Matrix & v, unsigned int version) { + void save(Archive & ar, const Eigen::Matrix & v, unsigned int /*version*/) { ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); } template - void load(Archive & ar, Eigen::Matrix & v, unsigned int version) { + void load(Archive & ar, Eigen::Matrix & v, unsigned int /*version*/) { ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); } diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 094e6f162..c356dcc07 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -20,17 +20,10 @@ template struct traits; namespace internal { -/// VectorSpace Implementation for Fixed sizes +/// VectorSpaceTraits Implementation for Fixed sizes template struct VectorSpaceImpl { - /// @name Group - /// @{ - static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} - static Class Between(const Class& v1, const Class& v2) { return v2-v1;} - static Class Inverse(const Class& m) { return -m;} - /// @} - /// @name Manifold /// @{ typedef Eigen::Matrix TangentVector; @@ -68,21 +61,21 @@ struct VectorSpaceImpl { return Class(v); } - static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v1 + v2; } - static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v2 - v1; } - static Class Inverse(const Class& v, ChartJacobian H) { + static Class Inverse(const Class& v, ChartJacobian H = boost::none) { if (H) *H = - Jacobian::Identity(); return -v; } @@ -90,7 +83,7 @@ struct VectorSpaceImpl { /// @} }; -/// VectorSpace implementation for dynamic types. +/// VectorSpaceTraits implementation for dynamic types. template struct VectorSpaceImpl { @@ -166,11 +159,11 @@ struct VectorSpaceImpl { /// @} }; -/// A helper that implements the traits interface for GTSAM lie groups. +/// A helper that implements the traits interface for GTSAM vector space types. /// To use this for your gtsam type, define: -/// template<> struct traits : public VectorSpace { }; +/// template<> struct traits : public VectorSpaceTraits { }; template -struct VectorSpace: Testable, VectorSpaceImpl { +struct VectorSpaceTraits: VectorSpaceImpl { typedef vector_space_tag structure_category; @@ -185,9 +178,12 @@ struct VectorSpace: Testable, VectorSpaceImpl { enum { dimension = Class::dimension}; typedef Class ManifoldType; /// @} - }; +/// Both VectorSpaceTRaits and Testable +template +struct VectorSpace: Testable, VectorSpaceTraits {}; + /// A helper that implements the traits interface for GTSAM lie groups. /// To use this for your gtsam type, define: /// template<> struct traits : public ScalarTraits { }; @@ -401,7 +397,8 @@ struct DynamicTraits { return result; } - static Dynamic Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) { + static_cast(H); throw std::runtime_error("Expmap not defined for dynamic types"); } diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index b075d73b3..c6a3eb034 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -199,6 +199,7 @@ namespace gtsam { } void checkBlock(DenseIndex block) const { + static_cast(block); //Disable unused varibale warnings. assert(matrix_.cols() == variableColOffsets_.back()); assert(block < (DenseIndex)variableColOffsets_.size() - 1); assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); @@ -220,7 +221,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); ar & BOOST_SERIALIZATION_NVP(rowStart_); diff --git a/gtsam/base/debug.cpp b/gtsam/base/debug.cpp index 6abc98642..1c4d08dcd 100644 --- a/gtsam/base/debug.cpp +++ b/gtsam/base/debug.cpp @@ -17,6 +17,8 @@ */ #include +#include // for GTSAM_USE_TBB + #ifdef GTSAM_USE_TBB #include #endif diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp index 464624bd4..19870fdeb 100644 --- a/gtsam/base/tests/testFastContainers.cpp +++ b/gtsam/base/tests/testFastContainers.cpp @@ -7,13 +7,14 @@ * @author Alex Cunningham */ -#include +#include +#include +#include #include #include -#include -#include +#include using namespace boost::assign; using namespace gtsam; @@ -21,7 +22,7 @@ using namespace gtsam; /* ************************************************************************* */ TEST( testFastContainers, KeySet ) { - FastVector init_vector; + FastVector init_vector; init_vector += 2, 3, 4, 5; FastSet actSet(init_vector); diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp new file mode 100644 index 000000000..bce9a6036 --- /dev/null +++ b/gtsam/base/tests/testGroup.cpp @@ -0,0 +1,144 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testGroup.cpp + * @brief Unit tests for groups + * @author Frank Dellaert + **/ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Symmetric group +template +class Symmetric: private Eigen::PermutationMatrix { + Symmetric(const Eigen::PermutationMatrix& P) : + Eigen::PermutationMatrix(P) { + } +public: + static Symmetric identity() { return Symmetric(); } + Symmetric() { + Eigen::PermutationMatrix::setIdentity(); + } + static Symmetric Transposition(int i, int j) { + Symmetric g; + return g.applyTranspositionOnTheRight(i, j); + } + Symmetric operator*(const Symmetric& other) const { + return Eigen::PermutationMatrix::operator*(other); + } + bool operator==(const Symmetric& other) const { + for (size_t i = 0; i < N; i++) + if (this->indices()[i] != other.indices()[i]) + return false; + return true; + } + Symmetric inverse() const { + return Eigen::PermutationMatrix(Eigen::PermutationMatrix::inverse()); + } + friend std::ostream &operator<<(std::ostream &os, const Symmetric& m) { + for (size_t i = 0; i < N; i++) + os << m.indices()[i] << " "; + return os; + } + void print(const std::string& s = "") const { + std::cout << s << *this << std::endl; + } + bool equals(const Symmetric& other, double tol = 0) const { + return this->indices() == other.indices(); + } +}; + +/// Define permutation group traits to be a model of the Multiplicative Group concept +template +struct traits > : internal::MultiplicativeGroupTraits >, + Testable > { +}; + +} // namespace gtsam + +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +typedef Symmetric<2> S2; +TEST(Group, S2) { + S2 e, s1 = S2::Transposition(0, 1); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, s1)); +} + +//****************************************************************************** +typedef Symmetric<3> S3; +TEST(Group, S3) { + S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, s1)); + EXPECT(assert_equal(s1, s1 * e)); + EXPECT(assert_equal(s1, e * s1)); + EXPECT(assert_equal(e, s1 * s1)); + S3 g = s1 * s2; // 1 2 0 + EXPECT(assert_equal(s1, g * s2)); + EXPECT(assert_equal(e, compose_pow(g, 0))); + EXPECT(assert_equal(g, compose_pow(g, 1))); + EXPECT(assert_equal(e, compose_pow(g, 3))); // g is generator of Z3 subgroup +} + +//****************************************************************************** +// The direct product of S2=Z2 and S3 is the symmetry group of a hexagon, +// i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon) +namespace gtsam { +typedef DirectProduct Dih6; + +std::ostream &operator<<(std::ostream &os, const Dih6& m) { + os << "( " << m.first << ", " << m.second << ")"; + return os; +} + +// Provide traits with Testable + +template<> +struct traits : internal::MultiplicativeGroupTraits { + static void Print(const Dih6& m, const string& s = "") { + cout << s << m << endl; + } + static bool Equals(const Dih6& m1, const Dih6& m2, double tol = 1e-8) { + return m1 == m2; + } +}; +} // namespace gtsam + +TEST(Group, Dih6) { + Dih6 e, g(S2::Transposition(0, 1), + S3::Transposition(0, 1) * S3::Transposition(1, 2)); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT(check_group_invariants(e, g)); + EXPECT(assert_equal(e, compose_pow(g, 0))); + EXPECT(assert_equal(g, compose_pow(g, 1))); + EXPECT(assert_equal(e, compose_pow(g, 6))); // g is generator of Z6 subgroup +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 7cc649e66..09caadabd 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 060087c2a..07e666fbe 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; @@ -37,8 +37,8 @@ TEST(LieScalar , Concept) { //****************************************************************************** TEST(LieScalar , Invariants) { LieScalar lie1(2), lie2(3); - check_group_invariants(lie1, lie2); - check_manifold_invariants(lie1, lie2); + CHECK(check_group_invariants(lie1, lie2)); + CHECK(check_manifold_invariants(lie1, lie2)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 81e03c63c..ed3afac8c 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index fceb2f4b4..1db28bcc8 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -16,6 +16,8 @@ * @date Feb 7, 2012 */ +#include + #include #include #include @@ -60,10 +62,10 @@ TEST (Serialization, FastMap) { /* ************************************************************************* */ TEST (Serialization, FastSet) { - FastSet set; - set.insert(1.0); - set.insert(2.0); - set.insert(3.0); + KeySet set; + set.insert(1); + set.insert(2); + set.insert(3); EXPECT(equality(set)); EXPECT(equalityXML(set)); diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp index d8c62b121..364834e2a 100644 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ b/gtsam/base/tests/testTestableAssertions.cpp @@ -15,8 +15,8 @@ */ #include +#include -#include #include using namespace gtsam; diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp index c9083f781..8fe5e53ef 100644 --- a/gtsam/base/tests/testTreeTraversal.cpp +++ b/gtsam/base/tests/testTreeTraversal.cpp @@ -45,11 +45,11 @@ struct TestForest { }; TestForest makeTestForest() { - // 0 1 - // / \ - // 2 3 - // | - // 4 + // 0 1 + // / | + // 2 3 + // | + // 4 TestForest forest; forest.roots_.push_back(boost::make_shared(0)); forest.roots_.push_back(boost::make_shared(1)); diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 36f1c2f5f..b76746ba0 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -16,24 +16,28 @@ * @date Oct 5, 2010 */ -#include -#include -#include -#include -#include -#include -#include -#include - #include #include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + namespace gtsam { namespace internal { -GTSAM_EXPORT boost::shared_ptr timingRoot( +GTSAM_EXPORT boost::shared_ptr gTimingRoot( new TimingOutline("Total", getTicTocID("Total"))); -GTSAM_EXPORT boost::weak_ptr timingCurrent(timingRoot); +GTSAM_EXPORT boost::weak_ptr gCurrentTimer(gTimingRoot); /* ************************************************************************* */ // Implementation of TimingOutline @@ -50,8 +54,8 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) { } /* ************************************************************************* */ -TimingOutline::TimingOutline(const std::string& label, size_t myId) : - myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( +TimingOutline::TimingOutline(const std::string& label, size_t id) : + id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( 0), lastChildOrder_(0), label_(label) { #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); @@ -153,7 +157,7 @@ const boost::shared_ptr& TimingOutline::child(size_t child, } /* ************************************************************************* */ -void TimingOutline::ticInternal() { +void TimingOutline::tic() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(timer_.is_stopped()); timer_.start(); @@ -169,7 +173,7 @@ void TimingOutline::ticInternal() { } /* ************************************************************************* */ -void TimingOutline::tocInternal() { +void TimingOutline::toc() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(!timer_.is_stopped()); @@ -212,7 +216,6 @@ void TimingOutline::finishedIteration() { } /* ************************************************************************* */ -// Generate or retrieve a unique global ID number that will be used to look up tic_/toc statements size_t getTicTocID(const char *descriptionC) { const std::string description(descriptionC); // Global (static) map from strings to ID numbers and current next ID number @@ -232,37 +235,33 @@ size_t getTicTocID(const char *descriptionC) { } /* ************************************************************************* */ -void ticInternal(size_t id, const char *labelC) { +void tic(size_t id, const char *labelC) { const std::string label(labelC); - if (ISDEBUG("timing-verbose")) - std::cout << "gttic_(" << id << ", " << label << ")" << std::endl; boost::shared_ptr node = // - timingCurrent.lock()->child(id, label, timingCurrent); - timingCurrent = node; - node->ticInternal(); + gCurrentTimer.lock()->child(id, label, gCurrentTimer); + gCurrentTimer = node; + node->tic(); } /* ************************************************************************* */ -void tocInternal(size_t id, const char *label) { - if (ISDEBUG("timing-verbose")) - std::cout << "gttoc(" << id << ", " << label << ")" << std::endl; - boost::shared_ptr current(timingCurrent.lock()); - if (id != current->myId_) { - timingRoot->print(); +void toc(size_t id, const char *label) { + boost::shared_ptr current(gCurrentTimer.lock()); + if (id != current->id_) { + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".") % label % current->label_).str()); } if (!current->parent_.lock()) { - timingRoot->print(); + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root") % label).str()); } - current->tocInternal(); - timingCurrent = current->parent_; + current->toc(); + gCurrentTimer = current->parent_; } } // namespace internal diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 7a43ef884..d0bca4a9d 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -17,12 +17,16 @@ */ #pragma once -#include -#include -#include -#include -#include #include +#include +#include // for GTSAM_USE_TBB + +#include +#include +#include + +#include +#include // This file contains the GTSAM timing instrumentation library, a low-overhead method for // learning at a medium-fine level how much time various components of an algorithm take @@ -125,16 +129,21 @@ namespace gtsam { namespace internal { + // Generate/retrieve a unique global ID number that will be used to look up tic/toc statements GTSAM_EXPORT size_t getTicTocID(const char *description); - GTSAM_EXPORT void ticInternal(size_t id, const char *label); - GTSAM_EXPORT void tocInternal(size_t id, const char *label); + + // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, and call tic method + GTSAM_EXPORT void tic(size_t id, const char *label); + + // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of gCurrentTimer + GTSAM_EXPORT void toc(size_t id, const char *label); /** * Timing Entry, arranged in a tree */ class GTSAM_EXPORT TimingOutline { protected: - size_t myId_; + size_t id_; size_t t_; size_t tWall_; double t2_ ; ///< cache the \sum t_i^2 @@ -176,29 +185,38 @@ namespace gtsam { void print2(const std::string& outline = "", const double parentTotal = -1.0) const; const boost::shared_ptr& child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); - void ticInternal(); - void tocInternal(); + void tic(); + void toc(); void finishedIteration(); - GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); + GTSAM_EXPORT friend void toc(size_t id, const char *label); }; // \TimingOutline /** - * No documentation + * Small class that calls internal::tic at construction, and internol::toc when destroyed */ class AutoTicToc { - private: + private: size_t id_; - const char *label_; + const char* label_; bool isSet_; - public: - AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } - void stop() { tocInternal(id_, label_); isSet_ = false; } - ~AutoTicToc() { if(isSet_) stop(); } + + public: + AutoTicToc(size_t id, const char* label) + : id_(id), label_(label), isSet_(true) { + tic(id_, label_); + } + void stop() { + toc(id_, label_); + isSet_ = false; + } + ~AutoTicToc() { + if (isSet_) stop(); + } }; - GTSAM_EXTERN_EXPORT boost::shared_ptr timingRoot; - GTSAM_EXTERN_EXPORT boost::weak_ptr timingCurrent; + GTSAM_EXTERN_EXPORT boost::shared_ptr gTimingRoot; + GTSAM_EXTERN_EXPORT boost::weak_ptr gCurrentTimer; } // Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) @@ -210,7 +228,7 @@ namespace gtsam { // tic #define gttic_(label) \ static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) + ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label) // toc #define gttoc_(label) \ @@ -228,26 +246,26 @@ namespace gtsam { // indicate iteration is finished inline void tictoc_finishedIteration_() { - ::gtsam::internal::timingRoot->finishedIteration(); } + ::gtsam::internal::gTimingRoot->finishedIteration(); } // print inline void tictoc_print_() { - ::gtsam::internal::timingRoot->print(); } + ::gtsam::internal::gTimingRoot->print(); } // print mean and standard deviation inline void tictoc_print2_() { - ::gtsam::internal::timingRoot->print2(); } + ::gtsam::internal::gTimingRoot->print2(); } // get a node by label and assign it to variable #define tictoc_getNode(variable, label) \ static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ const boost::shared_ptr variable = \ - ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent); + ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer); // reset inline void tictoc_reset_() { - ::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); - ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; } + ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); + ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; } #ifdef ENABLE_TIMING #define gttic(label) gttic_(label) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 3edd7d076..12a29e45b 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -1,19 +1,19 @@ /* ---------------------------------------------------------------------------- -* 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) + * 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 + * See LICENSE for the license information -* -------------------------------------------------------------------------- */ + * -------------------------------------------------------------------------- */ /** -* @file treeTraversal-inst.h -* @author Richard Roberts -* @date April 9, 2013 -*/ + * @file treeTraversal-inst.h + * @author Richard Roberts + * @date April 9, 2013 + */ #pragma once #include @@ -22,6 +22,7 @@ #include #include #include +#include // for GTSAM_USE_TBB #include #include @@ -33,192 +34,197 @@ namespace gtsam { - /** Internal functions used for traversing trees */ - namespace treeTraversal { +/** Internal functions used for traversing trees */ +namespace treeTraversal { - /* ************************************************************************* */ - namespace { - // Internal node used in DFS preorder stack - template - struct TraversalNode { - bool expanded; - const boost::shared_ptr& treeNode; - DATA& parentData; - typename FastList::iterator dataPointer; - TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : - expanded(false), treeNode(_treeNode), parentData(_parentData) {} - }; - - // Do nothing - default argument for post-visitor for tree traversal - struct no_op { - template - void operator()(const boost::shared_ptr& node, const DATA& data) {} - }; - - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting - * its children, and will be passed, by reference, the \c DATA object returned by the - * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost) - { - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - // Depth first traversal stack - typedef TraversalNode TraversalNode; - typedef FastList Stack; - Stack stack; - FastList dataList; // List to store node data as it is returned from the pre-order visitor - - // Add roots to stack (insert such that they are visited and processed in order - { - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& root, forest.roots()) - stack.insert(insertLocation, TraversalNode(root, rootData)); - } - - // Traverse - while(!stack.empty()) - { - // Get next node - TraversalNode& node = stack.front(); - - if(node.expanded) { - // If already expanded, then the data stored in the node is no longer needed, so visit - // then delete it. - (void) visitorPost(node.treeNode, *node.dataPointer); - dataList.erase(node.dataPointer); - stack.pop_front(); - } else { - // If not already visited, visit the node and add its children (use reverse iterators so - // children are processed in the order they appear) - node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData)); - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& child, node.treeNode->children) - stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); - node.expanded = true; - } - } - assert(dataList.empty()); - } - - /** Traverse a forest depth-first, with a pre-order visit but no post-order visit. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) - { - no_op visitorPost; - DepthFirstForest(forest, rootData, visitorPre, visitorPost); - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting - * its children, and will be passed, by reference, the \c DATA object returned by the - * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold = 10) - { -#ifdef GTSAM_USE_TBB - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - tbb::task::spawn_root_and_wait(internal::CreateRootTask( - forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold)); -#else - DepthFirstForest(forest, rootData, visitorPre, visitorPost); -#endif - } - - - /* ************************************************************************* */ - /** Traversal function for CloneForest */ - namespace { - template - boost::shared_ptr - CloneForestVisitorPre(const boost::shared_ptr& node, const boost::shared_ptr& parentPointer) - { - // Clone the current node and add it to its cloned parent - boost::shared_ptr clone = boost::make_shared(*node); - clone->children.clear(); - parentPointer->children.push_back(clone); - return clone; - } - } - - /** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child - * pointers for a clone of the original tree. - * @param forest The forest of trees to clone. The method \c forest.roots() should exist and - * return a collection of shared pointers to \c FOREST::Node. - * @return The new collection of roots. */ - template - FastVector > CloneForest(const FOREST& forest) - { - typedef typename FOREST::Node Node; - boost::shared_ptr rootContainer = boost::make_shared(); - DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); - return FastVector >(rootContainer->children.begin(), rootContainer->children.end()); - } - - - /* ************************************************************************* */ - /** Traversal function for PrintForest */ - namespace { - struct PrintForestVisitorPre - { - const KeyFormatter& formatter; - PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {} - template std::string operator()(const boost::shared_ptr& node, const std::string& parentString) - { - // Print the current node - node->print(parentString + "-", formatter); - // Increment the indentation - return parentString + "| "; - } - }; - } - - /** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. - * To print each node, this function calls the \c print function of the tree nodes. */ - template - void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) { - PrintForestVisitorPre visitor(keyFormatter); - DepthFirstForest(forest, str, visitor); - } +/* ************************************************************************* */ +namespace { +// Internal node used in DFS preorder stack +template +struct TraversalNode { + bool expanded; + const boost::shared_ptr& treeNode; + DATA& parentData; + typename FastList::iterator dataPointer; + TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : + expanded(false), treeNode(_treeNode), parentData(_parentData) { } +}; + +// Do nothing - default argument for post-visitor for tree traversal +struct no_op { + template + void operator()(const boost::shared_ptr& node, const DATA& data) { + } +}; + +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, + VISITOR_POST& visitorPost) { + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + // Depth first traversal stack + typedef TraversalNode TraversalNode; + typedef FastList Stack; + Stack stack; + FastList dataList; // List to store node data as it is returned from the pre-order visitor + + // Add roots to stack (insert such that they are visited and processed in order + { + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& root, forest.roots()) + stack.insert(insertLocation, TraversalNode(root, rootData)); + } + + // Traverse + while (!stack.empty()) { + // Get next node + TraversalNode& node = stack.front(); + + if (node.expanded) { + // If already expanded, then the data stored in the node is no longer needed, so visit + // then delete it. + (void) visitorPost(node.treeNode, *node.dataPointer); + dataList.erase(node.dataPointer); + stack.pop_front(); + } else { + // If not already visited, visit the node and add its children (use reverse iterators so + // children are processed in the order they appear) + node.dataPointer = dataList.insert(dataList.end(), + visitorPre(node.treeNode, node.parentData)); + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& child, node.treeNode->children) + stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); + node.expanded = true; + } + } + assert(dataList.empty()); +} + +/** Traverse a forest depth-first, with a pre-order visit but no post-order visit. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) { + no_op visitorPost; + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForestParallel(FOREST& forest, DATA& rootData, + VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, + int problemSizeThreshold = 10) { +#ifdef GTSAM_USE_TBB + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); +#else + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +#endif +} + +/* ************************************************************************* */ +/** Traversal function for CloneForest */ +namespace { +template +boost::shared_ptr CloneForestVisitorPre( + const boost::shared_ptr& node, + const boost::shared_ptr& parentPointer) { + // Clone the current node and add it to its cloned parent + boost::shared_ptr clone = boost::make_shared(*node); + clone->children.clear(); + parentPointer->children.push_back(clone); + return clone; +} +} + +/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child + * pointers for a clone of the original tree. + * @param forest The forest of trees to clone. The method \c forest.roots() should exist and + * return a collection of shared pointers to \c FOREST::Node. + * @return The new collection of roots. */ +template +FastVector > CloneForest( + const FOREST& forest) { + typedef typename FOREST::Node Node; + boost::shared_ptr rootContainer = boost::make_shared(); + DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); + return FastVector >(rootContainer->children.begin(), + rootContainer->children.end()); +} + +/* ************************************************************************* */ +/** Traversal function for PrintForest */ +namespace { +struct PrintForestVisitorPre { + const KeyFormatter& formatter; + PrintForestVisitorPre(const KeyFormatter& formatter) : + formatter(formatter) { + } + template std::string operator()( + const boost::shared_ptr& node, const std::string& parentString) { + // Print the current node + node->print(parentString + "-", formatter); + // Increment the indentation + return parentString + "| "; + } +}; +} + +/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. + * To print each node, this function calls the \c print function of the tree nodes. */ +template +void PrintForest(const FOREST& forest, std::string str, + const KeyFormatter& keyFormatter) { + PrintForestVisitorPre visitor(keyFormatter); + DepthFirstForest(forest, str, visitor); +} +} } diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 7986f9534..c1df47a01 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -24,6 +24,7 @@ #ifdef GTSAM_USE_TBB # include +# include # undef max // TBB seems to include windows.h and we don't want these macros # undef min # undef ERROR diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp deleted file mode 100644 index 3f86dc0c1..000000000 --- a/gtsam/base/types.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file types.h - * @brief Typedefs for easier changing of types - * @author Richard Roberts - * @date Aug 21, 2010 - * @addtogroup base - */ - -#include - -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - std::string _defaultKeyFormatter(Key key) { - const Symbol asSymbol(key); - if(asSymbol.chr() > 0) - return (std::string)asSymbol; - else - return boost::lexical_cast(key); - } - -} \ No newline at end of file diff --git a/gtsam/base/types.h b/gtsam/base/types.h index aca04a14b..84c94e73d 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -20,14 +20,11 @@ #pragma once #include -#include +#include +#include +#include // for GTSAM_USE_TBB #include -#include -#include -#include -#include -#include #ifdef GTSAM_USE_TBB #include @@ -58,22 +55,9 @@ namespace gtsam { /// Integer nonlinear key type typedef size_t Key; - /// Typedef for a function to format a key, i.e. to convert it to a string - typedef boost::function KeyFormatter; - - // Helper function for DefaultKeyFormatter - GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - - /// The default KeyFormatter, which is used if no KeyFormatter is passed to - /// a nonlinear 'print' function. Automatically detects plain integer keys - /// and Symbol keys. - static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; - - /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; - /* ************************************************************************* */ /** * Helper class that uses templates to select between two types based on @@ -148,104 +132,6 @@ namespace gtsam { return ListOfOneContainer(element); } - /* ************************************************************************* */ - /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. - template - class ThreadsafeException : -#ifdef GTSAM_USE_TBB - public tbb::tbb_exception -#else - public std::exception -#endif - { -#ifdef GTSAM_USE_TBB - private: - typedef tbb::tbb_exception Base; - protected: - typedef std::basic_string, tbb::tbb_allocator > String; -#else - private: - typedef std::exception Base; - protected: - typedef std::string String; -#endif - - protected: - bool dynamic_; ///< Whether this object was moved - mutable boost::optional description_; ///< Optional description - - /// Default constructor is protected - may only be created from derived classes - ThreadsafeException() : dynamic_(false) {} - - /// Copy constructor is protected - may only be created from derived classes - ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {} - - /// Construct with description string - ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {} - - /// Default destructor doesn't have the throw() - virtual ~ThreadsafeException() throw () {} - - public: - // Implement functions for tbb_exception -#ifdef GTSAM_USE_TBB - virtual tbb::tbb_exception* move() throw () { - void* cloneMemory = scalable_malloc(sizeof(DERIVED)); - if (!cloneMemory) { - std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; - exit(-1); - } - DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); - clone->dynamic_ = true; - return clone; - } - - virtual void destroy() throw() { - if (dynamic_) { - DERIVED* derivedPtr = static_cast(this); - derivedPtr->~DERIVED(); - scalable_free(derivedPtr); - } - } - - virtual void throw_self() { - throw *static_cast(this); } - - virtual const char* name() const throw() { - return typeid(DERIVED).name(); } -#endif - - virtual const char* what() const throw() { - return description_ ? description_->c_str() : ""; } - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class RuntimeErrorThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class OutOfRangeThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe invalid argument exception - class InvalidArgumentThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - /* ************************************************************************* */ #ifdef __clang__ # pragma clang diagnostic push diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 64136511d..20073152e 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -25,7 +25,7 @@ #define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@" // Paths to example datasets distributed with GTSAM -#define GTSAM_SOURCE_TREE_DATASET_DIR "@CMAKE_SOURCE_DIR@/examples/Data" +#define GTSAM_SOURCE_TREE_DATASET_DIR "@PROJECT_SOURCE_DIR@/examples/Data" #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data" // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices) @@ -42,6 +42,9 @@ // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake) #cmakedefine GTSAM_USE_TBB +// Whether we are using system-Eigen or our own patched version +#cmakedefine GTSAM_USE_SYSTEM_EIGEN + // Whether Eigen will use MKL (if MKL was found and GTSAM_WITH_EIGEN_MKL is enabled in CMake) #cmakedefine GTSAM_USE_EIGEN_MKL #cmakedefine EIGEN_USE_MKL_ALL // This is also defined in gtsam_eigen_includes.h diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 9319a1541..c1648888e 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -462,46 +462,48 @@ namespace gtsam { // cannot just create a root Choice node on the label: if the label is not the // highest label, we need to do a complicated and expensive recursive call. template template - typename DecisionTree::NodePtr DecisionTree::compose( - Iterator begin, Iterator end, const L& label) const { + typename DecisionTree::NodePtr DecisionTree::compose(Iterator begin, + Iterator end, const L& label) const { // find highest label among branches boost::optional highestLabel; - boost::optional nrChoices; + size_t nrChoices = 0; for (Iterator it = begin; it != end; it++) { - if (it->root_->isLeaf()) continue; - boost::shared_ptr c = boost::dynamic_pointer_cast (it->root_); + if (it->root_->isLeaf()) + continue; + boost::shared_ptr c = + boost::dynamic_pointer_cast(it->root_); if (!highestLabel || c->label() > *highestLabel) { highestLabel.reset(c->label()); - nrChoices.reset(c->nrChoices()); + nrChoices = c->nrChoices(); } } // if label is already in correct order, just put together a choice on label - if (!highestLabel || label > *highestLabel) { + if (!nrChoices || !highestLabel || label > *highestLabel) { boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); for (Iterator it = begin; it != end; it++) choiceOnLabel->push_back(it->root_); return Choice::Unique(choiceOnLabel); - } - - // Set up a new choice on the highest label - boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices)); - // now, for all possible values of highestLabel - for (size_t index = 0; index < *nrChoices; index++) { - // make a new set of functions for composing by iterating over the given - // functions, and selecting the appropriate branch. - std::vector functions; - for (Iterator it = begin; it != end; it++) { - // by restricting the input functions to value i for labelBelow - DecisionTree chosen = it->choose(*highestLabel, index); - functions.push_back(chosen); + } else { + // Set up a new choice on the highest label + boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, nrChoices)); + // now, for all possible values of highestLabel + for (size_t index = 0; index < nrChoices; index++) { + // make a new set of functions for composing by iterating over the given + // functions, and selecting the appropriate branch. + std::vector functions; + for (Iterator it = begin; it != end; it++) { + // by restricting the input functions to value i for labelBelow + DecisionTree chosen = it->choose(*highestLabel, index); + functions.push_back(chosen); + } + // We then recurse, for all values of the highest label + NodePtr fi = compose(functions.begin(), functions.end(), label); + choiceOnHighestLabel->push_back(fi); } - // We then recurse, for all values of the highest label - NodePtr fi = compose(functions.begin(), functions.end(), label); - choiceOnHighestLabel->push_back(fi); + return Choice::Unique(choiceOnHighestLabel); } - return Choice::Unique(choiceOnHighestLabel); } /*********************************************************************************/ @@ -667,9 +669,10 @@ namespace gtsam { void DecisionTree::dot(const std::string& name, bool showZero) const { std::ofstream os((name + ".dot").c_str()); dot(os, showZero); - system( + int result = system( ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str()); - } + if (result==-1) throw std::runtime_error("DecisionTree::dot system call failed"); +} /*********************************************************************************/ diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index de5ec8042..dcc336f89 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -90,7 +90,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 77be1d277..c2128c776 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -39,8 +39,8 @@ namespace gtsam { } /* ************************************************************************* */ - FastSet DiscreteFactorGraph::keys() const { - FastSet keys; + KeySet DiscreteFactorGraph::keys() const { + KeySet keys; BOOST_FOREACH(const sharedFactor& factor, *this) if (factor) keys.insert(factor->begin(), factor->end()); return keys; diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index c5b11adf1..cdfd7d522 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -120,7 +120,7 @@ public: } /** Return the set of variables involved in the factors (set union) */ - FastSet keys() const; + KeySet keys() const; /** return product of all factors as a single factor */ DecisionTreeFactor product() const; diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 978781d63..1078b4c61 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index e90ae32a4..d95c47f7b 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { @@ -159,7 +158,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 6ae8ec14b..81463ac06 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { @@ -109,7 +108,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("Cal3DS2", boost::serialization::base_object(*this)); diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index d4f4cabe5..cfbdde07c 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -153,7 +153,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 2127fb200..9982cec47 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -23,7 +23,6 @@ #pragma once #include -#include namespace gtsam { @@ -134,7 +133,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("Cal3Unified", boost::serialization::base_object(*this)); diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 3ec29bbd2..c131d46f7 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -83,10 +83,21 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, } /* ************************************************************************* */ -Point2 Cal3_S2::calibrate(const Point2& p) const { - const double u = p.x(), v = p.y(); - return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), - (1 / fy_) * (v - v0_)); +Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal, + OptionalJacobian<2,2> Dp) const { + const double u = p.x(), v = p.y(); + double delta_u = u - u0_, delta_v = v - v0_; + double inv_fx = 1/ fx_, inv_fy = 1/fy_; + double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy; + Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), + inv_fy_delta_v); + if(Dcal) + *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, -inv_fx * point.y(), + -inv_fx, inv_fx_s_inv_fy, + 0, -inv_fy * point.y(), 0, 0, -inv_fy; + if(Dp) + *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy; + return point; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index b9e2ef581..4e62ca7e9 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -21,7 +21,6 @@ #pragma once -#include #include namespace gtsam { @@ -157,9 +156,12 @@ public: /** * convert image coordinates uv to intrinsic coordinates xy * @param p point in image coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const; /** * convert homogeneous image coordinates to intrinsic coordinates @@ -211,7 +213,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 651a7fabb..365a6c40e 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -144,7 +144,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(b_); diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 1f5f1f8a5..2d27b4dc7 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -19,88 +19,168 @@ #include #include +using namespace std; + namespace gtsam { /* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Pose3& pose) : - pose_(pose) { +Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + double uv = u * v, uu = u * u, vv = v * v; + Matrix26 Dpn_pose; + Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; + return Dpn_pose; } /* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Vector &v) : - pose_(Pose3::Expmap(v)) { +Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + Matrix23 Dpn_point; + Dpn_point << // + Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), // + /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2); + Dpn_point *= d; + return Dpn_point; } /* ************************************************************************* */ -Point2 CalibratedCamera::project_to_camera(const Point3& P, - OptionalJacobian<2,3> H1) { - if (H1) { - double d = 1.0 / P.z(), d2 = d * d; - *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; +Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) { + const double st = sin(pose2.theta()), ct = cos(pose2.theta()); + const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); + const Rot3 wRc(x, y, z); + const Point3 t(pose2.x(), pose2.y(), height); + return Pose3(wRc, t); +} + +/* ************************************************************************* */ +Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target, + const Point3& upVector) { + Point3 zc = target - eye; + zc = zc / zc.norm(); + Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down + xc = xc / xc.norm(); + Point3 yc = zc.cross(xc); + return Pose3(Rot3(xc, yc, zc), eye); +} + +/* ************************************************************************* */ +bool PinholeBase::equals(const PinholeBase &camera, double tol) const { + return pose_.equals(camera.pose(), tol); +} + +/* ************************************************************************* */ +void PinholeBase::print(const string& s) const { + pose_.print(s + ".pose"); +} + +/* ************************************************************************* */ +const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const { + if (H) { + H->setZero(); + H->block(0, 0, 6, 6) = I_6x6; } - return Point2(P.x() / P.z(), P.y() / P.z()); + return pose_; } /* ************************************************************************* */ -Point3 CalibratedCamera::backproject_from_camera(const Point2& p, - const double scale) { - return Point3(p.x() * scale, p.y() * scale, scale); +Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { + double d = 1.0 / pc.z(); + const double u = pc.x() * d, v = pc.y() * d; + if (Dpoint) + *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; + return Point2(u, v); +} + +/* ************************************************************************* */ +Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { + if (Dpoint) { + Matrix32 Dpoint3_pc; + Matrix23 Duv_point3; + Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3); + *Dpoint = Duv_point3 * Dpoint3_pc; + return uv; + } else + return Project(pc.point3()); +} + +/* ************************************************************************* */ +pair PinholeBase::projectSafe(const Point3& pw) const { + const Point3 pc = pose().transform_to(pw); + const Point2 pn = Project(pc); + return make_pair(pn, pc.z() > 0); +} + +/* ************************************************************************* */ +Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint) const { + + Matrix3 Rt; // calculated by transform_to if needed + const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw CheiralityException(); +#endif + const Point2 pn = Project(q); + + if (Dpose || Dpoint) { + const double d = 1.0 / q.z(); + if (Dpose) + *Dpose = PinholeBase::Dpose(pn, d); + if (Dpoint) + *Dpoint = PinholeBase::Dpoint(pn, d, Rt); + } + return pn; +} + +/* ************************************************************************* */ +Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 2> Dpoint) const { + + // world to camera coordinate + Matrix23 Dpc_rot; + Matrix2 Dpc_point; + const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0, + Dpose ? &Dpc_point : 0); + + // camera to normalized image coordinate + Matrix2 Dpn_pc; + const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); + + // chain the Jacobian matrices + if (Dpose) { + // only rotation is important + Matrix26 Dpc_pose; + Dpc_pose.setZero(); + Dpc_pose.leftCols<3>() = Dpc_rot; + *Dpose = Dpn_pc * Dpc_pose; // 2x2 * 2x6 + } + if (Dpoint) + *Dpoint = Dpn_pc * Dpc_point; // 2x2 * 2*2 + return pn; +} +/* ************************************************************************* */ +Point3 PinholeBase::backproject_from_camera(const Point2& p, + const double depth) { + return Point3(p.x() * depth, p.y() * depth, depth); } /* ************************************************************************* */ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { - double st = sin(pose2.theta()), ct = cos(pose2.theta()); - Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - Rot3 wRc(x, y, z); - Point3 t(pose2.x(), pose2.y(), height); - Pose3 pose3(wRc, t); - return CalibratedCamera(pose3); + return CalibratedCamera(LevelPose(pose2, height)); +} + +/* ************************************************************************* */ +CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, + const Point3& target, const Point3& upVector) { + return CalibratedCamera(LookatPose(eye, target, upVector)); } /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const { - -#ifdef CALIBRATEDCAMERA_CHAIN_RULE - Matrix36 Dpose_; - Matrix3 Dpoint_; - Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0); -#else - Point3 q = pose_.transform_to(point); -#endif - Point2 intrinsic = project_to_camera(q); - - // Check if point is in front of camera - if (q.z() <= 0) - throw CheiralityException(); - - if (Dpose || Dpoint) { -#ifdef CALIBRATEDCAMERA_CHAIN_RULE - // just implement chain rule - if(Dpose && Dpoint) { - Matrix23 H; - project_to_camera(q,H); - if (Dpose) *Dpose = H * (*Dpose_); - if (Dpoint) *Dpoint = H * (*Dpoint_); - } -#else - // optimized version, see CalibratedCamera.nb - const double z = q.z(), d = 1.0 / z; - const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; - if (Dpose) - *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), - -uv, -u, 0., -d, d * v; - if (Dpoint) { - const Matrix3 R(pose_.rotation().matrix()); - Matrix23 Dpoint_; - Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), - R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2), - R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); - *Dpoint = d * Dpoint_; - } -#endif - } - return intrinsic; + OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { + return project2(point, Dcamera, Dpoint); } /* ************************************************************************* */ diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 9e907f1d5..4d605ef4e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -19,10 +19,16 @@ #pragma once #include -#include +#include +#include +#include +#include +#include namespace gtsam { +class Point2; + class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: @@ -31,6 +37,199 @@ public: } }; +/** + * A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT PinholeBase { + +public: + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef Point2 Measurement; + +private: + + Pose3 pose_; ///< 3D pose of camera + +protected: + + /// @name Derivatives + /// @{ + + /** + * Calculate Jacobian with respect to pose + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + */ + static Matrix26 Dpose(const Point2& pn, double d); + + /** + * Calculate Jacobian with respect to point + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Rt transposed rotation matrix + */ + static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt); + + /// @} + +public: + + /// @name Static functions + /// @{ + + /** + * Create a level pose at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static Pose3 LevelPose(const Pose2& pose2, double height); + + /** + * Create a camera pose at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + */ + static Pose3 LookatPose(const Point3& eye, const Point3& target, + const Point3& upVector); + + /// @} + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholeBase() { + } + + /** constructor with pose */ + explicit PinholeBase(const Pose3& pose) : + pose_(pose) { + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholeBase(const Vector &v) : + pose_(Pose3::Expmap(v)) { + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const PinholeBase &camera, double tol = 1e-9) const; + + /// print + void print(const std::string& s = "PinholeBase") const; + + /// @} + /// @name Standard Interface + /// @{ + + /// return pose, constant version + const Pose3& pose() const { + return pose_; + } + + /// get rotation + const Rot3& rotation() const { + return pose_.rotation(); + } + + /// get translation + const Point3& translation() const { + return pose_.translation(); + } + + /// return pose, with derivative + const Pose3& getPose(OptionalJacobian<6, 6> H) const; + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /** + * Project from 3D point in camera coordinates into image + * Does *not* throw a CheiralityException, even if pc behind image plane + * @param pc point in camera coordinates + */ + static Point2 Project(const Point3& pc, // + OptionalJacobian<2, 3> Dpoint = boost::none); + + /** + * Project from 3D point at infinity in camera coordinates into image + * Does *not* throw a CheiralityException, even if pc behind image plane + * @param pc point in camera coordinates + */ + static Point2 Project(const Unit3& pc, // + OptionalJacobian<2, 2> Dpoint = boost::none); + + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) const; + + /** Project point into the image + * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION + * @param point 3D point in world coordinates + * @return the intrinsic coordinates of the projected point + */ + Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + + /** Project point at infinity into the image + * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION + * @param point 3D point in world coordinates + * @return the intrinsic coordinates of the projected point + */ + Point2 project2(const Unit3& point, + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const; + + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + static Point3 backproject_from_camera(const Point2& p, const double depth); + + /// @} + /// @name Advanced interface + /// @{ + + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair translationInterval() { + return std::make_pair(3, 5); + } + + /// @} + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(pose_); + } + +}; +// end of class PinholeBase + /** * A Calibrated camera class [R|-R't], calibration K=I. * If calibration is known, it is more computationally efficient @@ -38,13 +237,13 @@ public: * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT CalibratedCamera { -private: - Pose3 pose_; // 6DOF pose +class GTSAM_EXPORT CalibratedCamera: public PinholeBase { public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ @@ -54,26 +253,40 @@ public: } /// construct with pose - explicit CalibratedCamera(const Pose3& pose); + explicit CalibratedCamera(const Pose3& pose) : + PinholeBase(pose) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param pose2 specifies the location and viewing direction + * @param height specifies the height of the camera (along the positive Z-axis) + * (theta 0 = looking in direction of positive X axis) + */ + static CalibratedCamera Level(const Pose2& pose2, double height); + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + */ + static CalibratedCamera Lookat(const Point3& eye, const Point3& target, + const Point3& upVector); /// @} /// @name Advanced Constructors /// @{ /// construct from vector - explicit CalibratedCamera(const Vector &v); - - /// @} - /// @name Testable - /// @{ - - virtual void print(const std::string& s = "") const { - pose_.print(s); - } - - /// check equality to another camera - bool equals(const CalibratedCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol); + explicit CalibratedCamera(const Vector &v) : + PinholeBase(v) { } /// @} @@ -84,19 +297,6 @@ public: virtual ~CalibratedCamera() { } - /// return pose - inline const Pose3& pose() const { - return pose_; - } - - /** - * Create a level camera at the given 2D pose and height - * @param pose2 specifies the location and viewing direction - * @param height specifies the height of the camera (along the positive Z-axis) - * (theta 0 = looking in direction of positive X axis) - */ - static CalibratedCamera Level(const Pose2& pose2, double height); - /// @} /// @name Manifold /// @{ @@ -107,105 +307,91 @@ public: /// Return canonical coordinate Vector localCoordinates(const CalibratedCamera& T2) const; - /// Lie group dimensionality + /// @deprecated inline size_t dim() const { return 6; } - /// Lie group dimensionality + /// @deprecated inline static size_t Dim() { return 6; } - /* ************************************************************************* */ - // measurement functions and derivatives - /* ************************************************************************* */ - /// @} /// @name Transformations and mesaurement functions /// @{ - /** - * This function receives the camera pose and the landmark location and - * returns the location the point is supposed to appear in the image - * @param point a 3D point to be projected - * @param Dpose the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the 3D point - * @return the intrinsic coordinates of the projected point - */ - Point2 project(const Point3& point, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const; /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * With optional 2by3 derivative + * @deprecated + * Use project2, which is more consistently named across Pinhole cameras */ - static Point2 project_to_camera(const Point3& cameraPoint, - OptionalJacobian<2, 3> H1 = boost::none); + Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; - /** - * backproject a 2-dimensional point to a 3-dimension point - */ - static Point3 backproject_from_camera(const Point2& p, const double scale); + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + Point3 backproject(const Point2& pn, double depth) const { + return pose().transform_from(backproject_from_camera(pn, depth)); + } /** * Calculate range to a landmark * @param point 3D location of landmark - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) const { - return pose_.range(point, H1, H2); + double range(const Point3& point, + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return pose().range(point, Dcamera, Dpoint); } /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 6> H2 = boost::none) const { - return pose_.range(pose, H1, H2); + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return this->pose().range(pose, Dcamera, Dpose); } /** * Calculate range to another camera * @param camera Other camera - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 = - boost::none, OptionalJacobian<1, 6> H2 = boost::none) const { - return pose_.range(camera.pose_, H1, H2); + double range(const CalibratedCamera& camera, // + OptionalJacobian<1, 6> H1 = boost::none, // + OptionalJacobian<1, 6> H2 = boost::none) const { + return pose().range(camera.pose(), H1, H2); } + /// @} + private: - /// @} /// @name Advanced Interface /// @{ /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(pose_); + void serialize(Archive & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("PinholeBase", + boost::serialization::base_object(*this)); } /// @} }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold< + CalibratedCamera> { +}; } diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h new file mode 100644 index 000000000..a60528909 --- /dev/null +++ b/gtsam/geometry/CameraSet.h @@ -0,0 +1,341 @@ +/* ---------------------------------------------------------------------------- + + * 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 CameraSet.h + * @brief Base class to create smart factors on poses or cameras + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#pragma once + +#include +#include // for Cheirality exception +#include +#include +#include +#include + +namespace gtsam { + +/** + * @brief A set of cameras, all with their own calibration + */ +template +class CameraSet: public std::vector { + +protected: + + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + + static const int D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + /// Make a vector of re-projection errors + static Vector ErrorVector(const std::vector& predicted, + const std::vector& measured) { + + // Check size + size_t m = predicted.size(); + if (measured.size() != m) + throw std::runtime_error("CameraSet::errors: size mismatch"); + + // Project and fill error vector + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + Z e = predicted[i] - measured[i]; + b.segment(row) = e.vector(); + } + return b; + } + +public: + + /// Definitions for blocks of F + typedef Eigen::Matrix MatrixZD; + typedef std::vector FBlocks; + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + virtual void print(const std::string& s = "") const { + std::cout << s << "CameraSet, cameras = \n"; + for (size_t k = 0; k < this->size(); ++k) + this->at(k).print(s); + } + + /// equals + bool equals(const CameraSet& p, double tol = 1e-9) const { + if (this->size() != p.size()) + return false; + bool camerasAreEqual = true; + for (size_t i = 0; i < this->size(); i++) { + if (this->at(i).equals(p.at(i), tol) == false) + camerasAreEqual = false; + break; + } + return camerasAreEqual; + } + + /** + * Project a point (possibly Unit3 at infinity), with derivatives + * Note that F is a sparse block-diagonal matrix, so instead of a large dense + * matrix this function returns the diagonal blocks. + * throws CheiralityException + */ + template + std::vector project2(const POINT& point, // + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { + + static const int N = FixedDimension::value; + + // Allocate result + size_t m = this->size(); + std::vector z(m); + + // Allocate derivatives + if (E) + E->resize(ZDim * m, N); + if (Fs) + Fs->resize(m); + + // Project and fill derivatives + for (size_t i = 0; i < m; i++) { + MatrixZD Fi; + Eigen::Matrix Ei; + z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0); + if (Fs) + (*Fs)[i] = Fi; + if (E) + E->block(ZDim * i, 0) = Ei; + } + + return z; + } + + /// Calculate vector [project2(point)-z] of re-projection errors + template + Vector reprojectionError(const POINT& point, const std::vector& measured, + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { + return ErrorVector(project2(point, Fs, E), measured); + } + + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * G = F' * F - F' * E * P * E' * F + * g = F' * (b - E * P * E' * b) + * Fixed size version + */ + template // N = 2 or 3 + static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + + // a single point is observed in m cameras + size_t m = Fs.size(); + + // Create a SymmetricBlockMatrix + size_t M1 = D * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, D); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera + + const MatrixZD& Fi = Fs[i]; + const Eigen::Matrix Ei_P = // + E.block(ZDim * i, 0, ZDim, N) * P; + + // D = (Dx2) * ZDim + augmentedHessian(i, m) = Fi.transpose() * b.segment(ZDim * i) // F' * b + - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian(i, i) = Fi.transpose() + * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const MatrixZD& Fj = Fs[j]; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian(i, j) = -Fi.transpose() + * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj); + } + } // end of for over cameras + + augmentedHessian(m, m)(0, 0) += b.squaredNorm(); + return augmentedHessian; + } + + /// Computes Point Covariance P, with lambda parameter + template // N = 2 or 3 + static void ComputePointCovariance(Eigen::Matrix& P, + const Matrix& E, double lambda, bool diagonalDamping = false) { + + Matrix EtE = E.transpose() * E; + + if (diagonalDamping) { // diagonal of the hessian + EtE.diagonal() += lambda * EtE.diagonal(); + } else { + DenseIndex n = E.cols(); + EtE += lambda * Eigen::MatrixXd::Identity(n, n); + } + + P = (EtE).inverse(); + } + + /// Computes Point Covariance P, with lambda parameter, dynamic version + static Matrix PointCov(const Matrix& E, const double lambda = 0.0, + bool diagonalDamping = false) { + if (E.cols() == 2) { + Matrix2 P2; + ComputePointCovariance(P2, E, lambda, diagonalDamping); + return P2; + } else { + Matrix3 P3; + ComputePointCovariance(P3, E, lambda, diagonalDamping); + return P3; + } + } + + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * Dynamic version + */ + static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks, + const Matrix& E, const Vector& b, const double lambda = 0.0, + bool diagonalDamping = false) { + SymmetricBlockMatrix augmentedHessian; + if (E.cols() == 2) { + Matrix2 P; + ComputePointCovariance(P, E, lambda, diagonalDamping); + augmentedHessian = SchurComplement(Fblocks, E, P, b); + } else { + Matrix3 P; + ComputePointCovariance(P, E, lambda, diagonalDamping); + augmentedHessian = SchurComplement(Fblocks, E, P, b); + } + return augmentedHessian; + } + + /** + * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, + * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. + */ + template // N = 2 or 3 + static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, + const Eigen::Matrix& P, const Vector& b, + const FastVector& allKeys, const FastVector& keys, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) { + + assert(keys.size()==Fs.size()); + assert(keys.size()<=allKeys.size()); + + FastMap KeySlotMap; + for (size_t slot = 0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); + + // Schur complement trick + // G = F' * F - F' * E * P * E' * F + // g = F' * (b - E * P * E' * b) + + Eigen::Matrix matrixBlock; + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix + + // a single point is observed in m cameras + size_t m = Fs.size(); // cameras observing current point + size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group + assert(allKeys.size()==M); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera in the current factor + + const MatrixZD& Fi = Fs[i]; + const Eigen::Matrix Ei_P = E.template block( + ZDim * i, 0) * P; + + // D = (DxZDim) * (ZDim) + // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) + // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) + // Key cameraKey_i = this->keys_[i]; + DenseIndex aug_i = KeySlotMap.at(keys[i]); + + // information vector - store previous vector + // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i, M) = augmentedHessian(aug_i, M).knownOffDiagonal() + + Fi.transpose() * b.segment(ZDim * i) // F' * b + - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + // main block diagonal - store previous block + matrixBlock = augmentedHessian(aug_i, aug_i); + // add contribution of current factor + augmentedHessian(aug_i, aug_i) = matrixBlock + + (Fi.transpose() + * (Fi - Ei_P * E.template block(ZDim * i, 0).transpose() * Fi)); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const MatrixZD& Fj = Fs[j]; + + DenseIndex aug_j = KeySlotMap.at(keys[j]); + + // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) + // off diagonal block - store previous block + // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i, aug_j) = + augmentedHessian(aug_i, aug_j).knownOffDiagonal() + - Fi.transpose() + * (Ei_P * E.template block(ZDim * j, 0).transpose() * Fj); + } + } // end of for over cameras + + augmentedHessian(M, M)(0, 0) += b.squaredNorm(); + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & (*this); + } +}; + +template +const int CameraSet::D; + +template +const int CameraSet::ZDim; + +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 2c883707f..15d8154b8 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -16,10 +16,8 @@ **/ #include -#include -#include -#include -#include +#include +#include // for cout :-( namespace gtsam { @@ -33,10 +31,11 @@ public: i_(i) { assert(i < N); } - /// Idenity element - static Cyclic Identity() { - return Cyclic(0); + /// Default constructor yields identity + Cyclic():i_(0) { } + static Cyclic identity() { return Cyclic();} + /// Cast to size_t operator size_t() const { return i_; @@ -63,20 +62,10 @@ public: } }; -/// Define cyclic group traits to be a model of the Group concept +/// Define cyclic group to be a model of the Additive Group concept template -struct traits > { - typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic) - static Cyclic Identity() { - return Cyclic::Identity(); - } - static bool Equals(const Cyclic& a, const Cyclic& b, - double tol = 1e-9) { - return a.equals(b, tol); - } - static void Print(const Cyclic& c, const std::string &s = "") { - c.print(s); - } +struct traits > : internal::AdditiveGroupTraits >, // + Testable > { }; } // \namespace gtsam diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 062178fea..7b90edc39 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -13,64 +13,46 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb, OptionalJacobian<5, 6> H) { - const Rot3& _1R2_ = _1P2_.rotation(); - const Point3& _1T2_ = _1P2_.translation(); + const Rot3& aRb = aPb.rotation(); + const Point3& aTb = aPb.translation(); if (!H) { // just make a direction out of translation and create E - Unit3 direction(_1T2_); - return EssentialMatrix(_1R2_, direction); + Unit3 direction(aTb); + return EssentialMatrix(aRb, direction); } else { // Calculate the 5*6 Jacobian H = D_E_1P2 // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation // First get 2*3 derivative from Unit3::FromPoint3 Matrix23 D_direction_1T2; - Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); + Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2); *H << I_3x3, Z_3x3, // - Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix(); - return EssentialMatrix(_1R2_, direction); + Matrix23::Zero(), D_direction_1T2 * aRb.matrix(); + return EssentialMatrix(aRb, direction); } } /* ************************************************************************* */ void EssentialMatrix::print(const string& s) const { cout << s; - aRb_.print("R:\n"); - aTb_.print("d: "); -} - -/* ************************************************************************* */ -EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { - assert(xi.size() == 5); - Vector3 omega(sub(xi, 0, 3)); - Vector2 z(sub(xi, 3, 5)); - Rot3 R = aRb_.retract(omega); - Unit3 t = aTb_.retract(z); - return EssentialMatrix(R, t); -} - -/* ************************************************************************* */ -Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - Vector5 v; - v << aRb_.localCoordinates(other.aRb_), - aTb_.localCoordinates(other.aTb_); - return v; + rotation().print("R:\n"); + direction().print("d: "); } /* ************************************************************************* */ Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, OptionalJacobian<3, 3> Dpoint) const { - Pose3 pose(aRb_, aTb_.point3()); + Pose3 pose(rotation(), direction().point3()); Matrix36 DE_; Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); if (DE) { // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 // The last 3 columns are derivative with respect to change in translation - // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() + // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis() // Duy made an educated guess that this needs to be rotated to the local frame Matrix35 H; - H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); + H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis(); *DE = H; } return q; @@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const { // The rotation must be conjugated to act in the camera frame - Rot3 c1Rc2 = aRb_.conjugate(cRb); + Rot3 c1Rc2 = rotation().conjugate(cRb); if (!HE && !HR) { // Rotate translation direction and return - Unit3 c1Tc2 = cRb * aTb_; + Unit3 c1Tc2 = cRb * direction(); return EssentialMatrix(c1Rc2, c1Tc2); } else { // Calculate derivatives Matrix23 D_c1Tc2_cRb; // 2*3 Matrix2 D_c1Tc2_aTb; // 2*2 - Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb); if (HE) *HE << cRb.matrix(), Matrix32::Zero(), // Matrix23::Zero(), D_c1Tc2_aTb; @@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // if (H) { // See math.lyx Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) - * aTb_.basis(); + Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) + * direction().basis(); *H << HR, HD; } return dot(vA, E_ * vB); diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 606f62f87..697bd462d 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,7 +10,8 @@ #include #include #include -#include +#include +#include namespace gtsam { @@ -20,17 +21,18 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix { +class GTSAM_EXPORT EssentialMatrix : private ProductManifold { private: - - Rot3 aRb_; ///< Rotation between a and b - Unit3 aTb_; ///< translation direction from a to b + typedef ProductManifold Base; Matrix3 E_; ///< Essential matrix -public: + /// Construct from Base + EssentialMatrix(const Base& base) : + Base(base), E_(direction().skew() * rotation().matrix()) { + } - enum { dimension = 5 }; +public: /// Static function to convert Point2 to homogeneous coordinates static Vector3 Homogeneous(const Point2& p) { @@ -42,12 +44,12 @@ public: /// Default constructor EssentialMatrix() : - aTb_(1, 0, 0), E_(aTb_.skew()) { + Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) { } /// Construct from rotation and translation EssentialMatrix(const Rot3& aRb, const Unit3& aTb) : - aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { + Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) { } /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) @@ -72,7 +74,8 @@ public: /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { - return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol); + return rotation().equals(other.rotation(), tol) + && direction().equals(other.direction(), tol); } /// @} @@ -80,22 +83,19 @@ public: /// @name Manifold /// @{ - /// Dimensionality of tangent space = 5 DOF - inline static size_t Dim() { - return 5; - } - - /// Return the dimensionality of the tangent space - size_t dim() const { - return 5; - } + using Base::dimension; + using Base::dim; + using Base::Dim; /// Retract delta to manifold - EssentialMatrix retract(const Vector& xi) const; + EssentialMatrix retract(const TangentVector& v) const { + return Base::retract(v); + } /// Compute the coordinates in the tangent space - Vector5 localCoordinates(const EssentialMatrix& other) const; - + TangentVector localCoordinates(const EssentialMatrix& other) const { + return Base::localCoordinates(other); + } /// @} /// @name Essential matrix methods @@ -103,12 +103,12 @@ public: /// Rotation inline const Rot3& rotation() const { - return aRb_; + return this->first; } /// Direction inline const Unit3& direction() const { - return aTb_; + return this->second; } /// Return 3*3 matrix representation @@ -118,12 +118,12 @@ public: /// Return epipole in image_a , as Unit3 to allow for infinity inline const Unit3& epipole_a() const { - return aTb_; // == direction() + return direction(); } /// Return epipole in image_b, as Unit3 to allow for infinity inline Unit3 epipole_b() const { - return aRb_.unrotate(aTb_); // == rotation.unrotate(direction()) + return rotation().unrotate(direction()); } /** @@ -179,9 +179,9 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(aRb_); - ar & BOOST_SERIALIZATION_NVP(aTb_); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(first); + ar & BOOST_SERIALIZATION_NVP(second); ar & boost::serialization::make_nvp("E11", E_(0,0)); ar & boost::serialization::make_nvp("E12", E_(0,1)); @@ -195,7 +195,6 @@ private: } /// @} - }; template<> diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 088a84243..95290497d 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -13,6 +13,7 @@ * @file OrientedPlane3.cpp * @date Dec 19, 2013 * @author Alex Trevor + * @author Zhaoyang Lv * @brief A plane, represented by a normal direction and perpendicular distance */ @@ -25,79 +26,54 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -/// The print fuction -void OrientedPlane3::print(const std::string& s) const { - Vector coeffs = planeCoefficients(); +void OrientedPlane3::print(const string& s) const { + Vector4 coeffs = planeCoefficients(); cout << s << " : " << coeffs << endl; } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane, - const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr, - OptionalJacobian<3, 3> Hp) { - Matrix n_hr; - Matrix n_hp; - Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp); +OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp, + OptionalJacobian<3, 6> Hr) const { + Matrix23 D_rotated_plane; + Matrix22 D_rotated_pose; + Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); - Vector n_unit = plane.n_.unitVector(); - Vector unit_vec = n_rotated.unitVector(); - double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_; - OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2), - pred_d); + Vector3 unit_vec = n_rotated.unitVector(); + double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_; if (Hr) { - *Hr = gtsam::zeros(3, 6); - (*Hr).block<2, 3>(0, 0) = n_hr; - (*Hr).block<1, 3>(2, 3) = unit_vec; + *Hr = zeros(3, 6); + Hr->block<2, 3>(0, 0) = D_rotated_plane; + Hr->block<1, 3>(2, 3) = unit_vec; } if (Hp) { - Vector xrp = xr.translation().vector(); - Matrix n_basis = plane.n_.basis(); - Vector hpp = n_basis.transpose() * xrp; - *Hp = gtsam::zeros(3, 3); - (*Hp).block<2, 2>(0, 0) = n_hp; - (*Hp).block<1, 2>(2, 0) = hpp; + Vector2 hpp = n_.basis().transpose() * xr.translation().vector(); + *Hp = Z_3x3; + Hp->block<2, 2>(0, 0) = D_rotated_pose; + Hp->block<1, 2>(2, 0) = hpp; (*Hp)(2, 2) = 1; } - return (transformed_plane); + return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d); } + /* ************************************************************************* */ -Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const { +Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { Vector2 n_error = -n_.localCoordinates(plane.n_); - double d_error = d_ - plane.d_; - Vector3 e; - e << n_error(0), n_error(1), d_error; - return (e); + return Vector3(n_error(0), n_error(1), d_ - plane.d_); } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { - // Retract the Unit3 - Vector2 n_v(v(0), v(1)); - Unit3 n_retracted = n_.retract(n_v); - double d_retracted = d_ + v(2); - return OrientedPlane3(n_retracted, d_retracted); +OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { + return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2)); } /* ************************************************************************* */ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { - Vector n_local = n_.localCoordinates(y.n_); + Vector2 n_local = n_.localCoordinates(y.n_); double d_local = d_ - y.d_; - Vector3 e; - e << n_local(0), n_local(1), -d_local; - return e; + return Vector3(n_local(0), n_local(1), -d_local); } -/* ************************************************************************* */ -Vector3 OrientedPlane3::planeCoefficients() const { - Vector unit_vec = n_.unitVector(); - Vector3 a; - a << unit_vec[0], unit_vec[1], unit_vec[2], d_; - return a; -} - -/* ************************************************************************* */ - } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 28b67cb4e..5c9a5cdef 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -14,6 +14,7 @@ * @date Dec 19, 2013 * @author Alex Trevor * @author Frank Dellaert + * @author Zhaoyang Lv * @brief An infinite plane, represented by a normal direction and perpendicular distance */ @@ -22,47 +23,57 @@ #include #include #include -#include namespace gtsam { -/// Represents an infinite plane in 3D. -class OrientedPlane3: public DerivedValue { +/** + * @brief Represents an infinite plane in 3D, which is composed of a planar normal and its + * perpendicular distance to the origin. + * Currently it provides a transform of the plane, and a norm 1 differencing of two planes. + * Refer to Trevor12iros for more math details. + */ +class GTSAM_EXPORT OrientedPlane3 { private: - Unit3 n_; /// The direction of the planar normal - double d_; /// The perpendicular distance to this plane + Unit3 n_; ///< The direction of the planar normal + double d_; ///< The perpendicular distance to this plane public: enum { dimension = 3 }; + /// @name Constructors /// @{ /// Default constructor OrientedPlane3() : - n_(), d_(0.0) { + n_(), d_(0.0) { } /// Construct from a Unit3 and a distance OrientedPlane3(const Unit3& s, double d) : - n_(s), d_(d) { + n_(s), d_(d) { } - OrientedPlane3(Vector vec) : - n_(vec(0), vec(1), vec(2)), d_(vec(3)) { + /// Construct from a vector of plane coefficients + OrientedPlane3(const Vector4& vec) : + n_(vec(0), vec(1), vec(2)), d_(vec(3)) { } - /// Construct from a, b, c, d + /// Construct from four numbers of plane coeffcients (a, b, c, d) OrientedPlane3(double a, double b, double c, double d) { Point3 p(a, b, c); n_ = Unit3(p); d_ = d; } - /// The print fuction + /// @} + /// @name Testable + /// @{ + + /// The print function void print(const std::string& s = std::string()) const; /// The equals function with tolerance @@ -70,13 +81,38 @@ public: return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol)); } - /// Transforms a plane to the specified pose - static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane, - const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, - OptionalJacobian<3, 3> Hp = boost::none); + /// @} - /// Computes the error between two poses - Vector3 error(const gtsam::OrientedPlane3& plane) const; + /** Transforms a plane to the specified pose + * @param xr a transformation in current coordiante + * @param Hp optional Jacobian wrpt the destination plane + * @param Hr optional jacobian wrpt the pose transformation + * @return the transformed plane + */ + OrientedPlane3 transform(const Pose3& xr, + OptionalJacobian<3, 3> Hp = boost::none, + OptionalJacobian<3, 6> Hr = boost::none) const; + + /** + * @deprecated the static method has wrong Jacobian order, + * please use the member method transform() + * @param The raw plane + * @param xr a transformation in current coordiante + * @param Hr optional jacobian wrpt the pose transformation + * @param Hp optional Jacobian wrpt the destination plane + * @return the transformed plane + */ + static OrientedPlane3 Transform(const OrientedPlane3& plane, + const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, + OptionalJacobian<3, 3> Hp = boost::none) { + return plane.transform(xr, Hp, Hr); + } + + /** Computes the error between two planes. + * The error is a norm 1 difference in tangent space. + * @param the other plane + */ + Vector3 error(const OrientedPlane3& plane) const; /// Dimensionality of tangent space = 3 DOF inline static size_t Dim() { @@ -89,27 +125,36 @@ public: } /// The retract function - OrientedPlane3 retract(const Vector& v) const; + OrientedPlane3 retract(const Vector3& v) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; - /// Returns the plane coefficients (a, b, c, d) - Vector3 planeCoefficients() const; + /// Returns the plane coefficients + inline Vector4 planeCoefficients() const { + Vector3 unit_vec = n_.unitVector(); + return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_); + } + /// Return the normal inline Unit3 normal() const { return n_; } + /// Return the perpendicular distance to the origin + inline double distance() const { + return d_; + } + /// @} }; template<> struct traits : public internal::Manifold< - OrientedPlane3> { +OrientedPlane3> { }; template<> struct traits : public internal::Manifold< - OrientedPlane3> { +OrientedPlane3> { }; } // namespace gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 46df47ecb..12e9f023b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -18,32 +18,32 @@ #pragma once -#include -#include -#include +#include namespace gtsam { /** * A pinhole camera class that has a Pose3 and a Calibration. + * Use PinholePose if you will not be optimizing for Calibration * @addtogroup geometry * \nosubgrouping */ template -class PinholeCamera { +class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { private: - Pose3 pose_; - Calibration K_; - GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + typedef PinholeBaseK Base; ///< base class has 3D pose as private member + Calibration K_; ///< Calibration, part of class now // Get dimensions of calibration type at compile time static const int DimK = FixedDimension::value; public: - enum { dimension = 6 + DimK }; + enum { + dimension = 6 + DimK + }; ///< Dimension depends on calibration /// @name Standard Constructors /// @{ @@ -54,12 +54,12 @@ public: /** constructor with pose */ explicit PinholeCamera(const Pose3& pose) : - pose_(pose) { + Base(pose) { } /** constructor with pose and calibration */ PinholeCamera(const Pose3& pose, const Calibration& K) : - pose_(pose), K_(K) { + Base(pose), K_(K) { } /// @} @@ -75,12 +75,7 @@ public: */ static PinholeCamera Level(const Calibration &K, const Pose2& pose2, double height) { - const double st = sin(pose2.theta()), ct = cos(pose2.theta()); - const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - const Rot3 wRc(x, y, z); - const Point3 t(pose2.x(), pose2.y(), height); - const Pose3 pose3(wRc, t); - return PinholeCamera(pose3, K); + return PinholeCamera(Base::LevelPose(pose2, height), K); } /// PinholeCamera::level with default calibration @@ -99,28 +94,23 @@ public: */ static PinholeCamera Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) { - Point3 zc = target - eye; - zc = zc / zc.norm(); - Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down - xc = xc / xc.norm(); - Point3 yc = zc.cross(xc); - Pose3 pose3(Rot3(xc, yc, zc), eye); - return PinholeCamera(pose3, K); + return PinholeCamera(Base::LookatPose(eye, target, upVector), K); } /// @} /// @name Advanced Constructors /// @{ - explicit PinholeCamera(const Vector &v) { - pose_ = Pose3::Expmap(v.head(6)); - if (v.size() > 6) { - K_ = Calibration(v.tail(DimK)); - } + /// Init from vector, can be 6D (default calibration) or dim + explicit PinholeCamera(const Vector &v) : + Base(v.head<6>()) { + if (v.size() > 6) + K_ = Calibration(v.tail()); } + /// Init from Vector and calibration PinholeCamera(const Vector &v, const Vector &K) : - pose_(Pose3::Expmap(v)), K_(K) { + Base(v), K_(K) { } /// @} @@ -128,14 +118,14 @@ public: /// @{ /// assert equality up to a tolerance - bool equals(const PinholeCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol) - && K_.equals(camera.calibration(), tol); + bool equals(const Base &camera, double tol = 1e-9) const { + const PinholeCamera* e = dynamic_cast(&camera); + return Base::equals(camera, tol) && K_.equals(e->calibration(), tol); } /// print void print(const std::string& s = "PinholeCamera") const { - pose_.print(s + ".pose"); + Base::print(s); K_.print(s + ".calibration"); } @@ -147,31 +137,21 @@ public: } /// return pose - inline Pose3& pose() { - return pose_; - } - - /// return pose, constant version - inline const Pose3& pose() const { - return pose_; + const Pose3& pose() const { + return Base::pose(); } /// return pose, with derivative - inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const { + const Pose3& getPose(OptionalJacobian<6, dimension> H) const { if (H) { H->setZero(); - H->block(0, 0, 6, 6) = I_6x6; + H->template block<6, 6>(0, 0) = I_6x6; } - return pose_; + return Base::pose(); } /// return calibration - inline Calibration& calibration() { - return K_; - } - - /// return calibration - inline const Calibration& calibration() const { + const Calibration& calibration() const { return K_; } @@ -179,13 +159,13 @@ public: /// @name Manifold /// @{ - /// Manifold dimension - inline size_t dim() const { + /// @deprecated + size_t dim() const { return dimension; } - /// Manifold dimension - inline static size_t Dim() { + /// @deprecated + static size_t Dim() { return dimension; } @@ -194,18 +174,17 @@ public: /// move a cameras according to d PinholeCamera retract(const Vector& d) const { if ((size_t) d.size() == 6) - return PinholeCamera(pose().retract(d), calibration()); + return PinholeCamera(this->pose().retract(d), calibration()); else - return PinholeCamera(pose().retract(d.head(6)), + return PinholeCamera(this->pose().retract(d.head<6>()), calibration().retract(d.tail(calibration().dim()))); } /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { VectorK6 d; - // TODO: why does d.head<6>() not compile?? - d.head(6) = pose().localCoordinates(T2.pose()); - d.tail(DimK) = calibration().localCoordinates(T2.calibration()); + d.template head<6>() = this->pose().localCoordinates(T2.pose()); + d.template tail() = calibration().localCoordinates(T2.calibration()); return d; } @@ -218,170 +197,45 @@ public: /// @name Transformations and measurement functions /// @{ - /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * @param P A point in camera coordinates - * @param Dpoint is the 2*3 Jacobian w.r.t. P - */ - static Point2 project_to_camera(const Point3& P, // - OptionalJacobian<2, 3> Dpoint = boost::none) { -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (P.z() <= 0) - throw CheiralityException(); -#endif - double d = 1.0 / P.z(); - const double u = P.x() * d, v = P.y() * d; - if (Dpoint) - *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Point2(u, v); - } - - /// Project a point into the image and check depth - inline std::pair projectSafe(const Point3& pw) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); - } - typedef Eigen::Matrix Matrix2K; - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration + /** Templated projection of a 3D point or a point at infinity into the image + * @param pw either a Point3 or a Unit3, in world coordinates */ - inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - if (Dpose || Dpoint) { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - if (Dpose) - calculateDpose(pn, d, Dpi_pn, *Dpose); - if (Dpoint) - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - return pi; - } else - return K_.uncalibrate(pn, Dcal); - } - - /** project a point at infinity from world coordinate to the image - * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration - */ - inline Point2 projectPointAtInfinity(const Point3& pw, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { - - if (!Dpose && !Dpoint && !Dcal) { - const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) - const Point2 pn = project_to_camera(pc); // project the point to the camera - return K_.uncalibrate(pn); - } - - // world to camera coordinate - Matrix3 Dpc_rot, Dpc_point; - const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); - - Matrix36 Dpc_pose; - Dpc_pose.setZero(); - Dpc_pose.leftCols<3>() = Dpc_rot; - - // camera to normalized image coordinate - Matrix23 Dpn_pc; // 2*3 - const Point2 pn = project_to_camera(pc, Dpn_pc); - - // uncalibration - Matrix2 Dpi_pn; // 2*2 - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; - if (Dpose) - *Dpose = Dpi_pc * Dpc_pose; - if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) + template + Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera, + OptionalJacobian<2, FixedDimension::value> Dpoint) const { + // We just call 3-derivative version in Base + Matrix26 Dpose; + Eigen::Matrix Dcal; + Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint, + Dcamera ? &Dcal : 0); + if (Dcamera) + *Dcamera << Dpose, Dcal; return pi; } - /** project a point from world coordinate to the image, fixed Jacobians - * @param pw is a point in the world coordinate - * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] - * @param Dpoint is the Jacobian w.r.t. point3 - */ - Point2 project2( - const Point3& pw, // - OptionalJacobian<2, dimension> Dcamera = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const { - - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - - if (!Dcamera && !Dpoint) { - return K_.uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - if (Dcamera) { // TODO why does leftCols<6>() not compile ?? - calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); - (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib - } - if (Dpoint) { - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } + /// project a 3D point from world coordinates into the image + Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { + return _project2(pw, Dcamera, Dpoint); } - /// backproject a 2-dimensional point to a 3-dimensional point at given depth - inline Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = K_.calibrate(p); - const Point3 pc(pn.x() * depth, pn.y() * depth, depth); - return pose_.transform_from(pc); - } - - /// backproject a 2-dimensional point to a 3-dimensional point at infinity - inline Point3 backprojectPointAtInfinity(const Point2& p) const { - const Point2 pn = K_.calibrate(p); - const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 - return pose_.rotation().rotate(pc); + /// project a point at infinity from world coordinates into the image + Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const { + return _project2(pw, Dcamera, Dpoint); } /** * Calculate range to a landmark * @param point 3D location of landmark - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the landmark * @return range (double) */ - double range( - const Point3& point, // - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { + double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { Matrix16 Dpose_; - double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); + double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint); if (Dcamera) *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; @@ -390,16 +244,12 @@ public: /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpose2 the optionally computed Jacobian with respect to the other pose * @return range (double) */ - double range( - const Pose3& pose, // - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { Matrix16 Dpose_; - double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); + double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose); if (Dcamera) *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; @@ -408,28 +258,23 @@ public: /** * Calculate range to another camera * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ template - double range( - const PinholeCamera& camera, // + double range(const PinholeCamera& camera, OptionalJacobian<1, dimension> Dcamera = boost::none, -// OptionalJacobian<1, 6 + traits::dimension::value> Dother = - boost::optional Dother = - boost::none) const { + boost::optional Dother = boost::none) const { Matrix16 Dcamera_, Dother_; - double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0, + double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0, Dother ? &Dother_ : 0); if (Dcamera) { - Dcamera->resize(1, 6 + DimK); + Dcamera->resize(1, 6 + DimK); *Dcamera << Dcamera_, Eigen::Matrix::Zero(); } if (Dother) { - Dother->resize(1, 6+CalibrationB::dimension); + Dother->resize(1, 6 + CalibrationB::dimension); Dother->setZero(); - Dother->block(0, 0, 1, 6) = Dother_; + Dother->block<1, 6>(0, 0) = Dother_; } return result; } @@ -437,12 +282,9 @@ public: /** * Calculate range to another camera * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ - double range( - const CalibratedCamera& camera, // + double range(const CalibratedCamera& camera, OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { return range(camera.pose(), Dcamera, Dother); @@ -450,65 +292,26 @@ public: private: - /** - * Calculate Jacobian with respect to pose - * @param pn projection in normalized coordinates - * @param d disparity (inverse depth) - * @param Dpi_pn derivative of uncalibrate with respect to pn - * @param Dpose Output argument, can be matrix or block, assumed right size ! - * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html - */ - template - static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn, - Eigen::MatrixBase const & Dpose) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - double uv = u * v, uu = u * u, vv = v * v; - Matrix26 Dpn_pose; - Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; - assert(Dpose.rows()==2 && Dpose.cols()==6); - const_cast&>(Dpose) = // - Dpi_pn * Dpn_pose; - } - - /** - * Calculate Jacobian with respect to point - * @param pn projection in normalized coordinates - * @param d disparity (inverse depth) - * @param Dpi_pn derivative of uncalibrate with respect to pn - * @param Dpoint Output argument, can be matrix or block, assumed right size ! - * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html - */ - template - static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, - const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpoint) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - Matrix23 Dpn_point; - Dpn_point << // - R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // - /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); - Dpn_point *= d; - assert(Dpoint.rows()==2 && Dpoint.cols()==3); - const_cast&>(Dpoint) = // - Dpi_pn * Dpn_point; - } - /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(pose_); + void serialize(Archive & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("PinholeBaseK", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(K_); } }; +template +struct traits > : public internal::Manifold< + PinholeCamera > { +}; template -struct traits< PinholeCamera > : public internal::Manifold > {}; - -template -struct traits< const PinholeCamera > : public internal::Manifold > {}; +struct traits > : public internal::Manifold< + PinholeCamera > { +}; } // \ gtsam diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h new file mode 100644 index 000000000..ac453e048 --- /dev/null +++ b/gtsam/geometry/PinholePose.h @@ -0,0 +1,408 @@ +/* ---------------------------------------------------------------------------- + + * 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 PinholePose.h + * @brief Pinhole camera with known calibration + * @author Yong-Dian Jian + * @author Frank Dellaert + * @date Feb 20, 2015 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * A pinhole camera class that has a Pose3 and a *fixed* Calibration. + * @addtogroup geometry + * \nosubgrouping + */ +template +class GTSAM_EXPORT PinholeBaseK: public PinholeBase { + +private: + + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + + // Get dimensions of calibration type at compile time + static const int DimK = FixedDimension::value; + +public: + + typedef CALIBRATION CalibrationType; + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholeBaseK() { + } + + /** constructor with pose */ + explicit PinholeBaseK(const Pose3& pose) : + PinholeBase(pose) { + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholeBaseK(const Vector &v) : + PinholeBase(v) { + } + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholeBaseK() { + } + + /// return calibration + virtual const CALIBRATION& calibration() const = 0; + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) const { + std::pair pn = PinholeBase::projectSafe(pw); + pn.first = calibration().uncalibrate(pn.first); + return pn; + } + + /** project a point from world coordinate to the image + * @param pw is a point in the world coordinates + */ + Point2 project(const Point3& pw) const { + const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates + return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates + } + + /** project a point from world coordinate to the image + * @param pw is a point at infinity in the world coordinates + */ + Point2 project(const Unit3& pw) const { + const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame + const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates + return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates + } + + /** Templated projection of a point (possibly at infinity) from world coordinate to the image + * @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates + * @param Dpose is the Jacobian w.r.t. pose3 + * @param Dpoint is the Jacobian w.r.t. point3 + * @param Dcal is the Jacobian w.r.t. calibration + */ + template + Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, FixedDimension::value> Dpoint, + OptionalJacobian<2, DimK> Dcal) const { + + // project to normalized coordinates + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); + + // uncalibrate to pixel coordinates + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, Dcal, + Dpose || Dpoint ? &Dpi_pn : 0); + + // If needed, apply chain rule + if (Dpose) + *Dpose = Dpi_pn * *Dpose; + if (Dpoint) + *Dpoint = Dpi_pn * *Dpoint; + + return pi; + } + + /// project a 3D point from world coordinates into the image + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + return _project(pw, Dpose, Dpoint, Dcal); + } + + /// project a point at infinity from world coordinates into the image + Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 2> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + return _project(pw, Dpose, Dpoint, Dcal); + } + + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + Point3 backproject(const Point2& p, double depth) const { + const Point2 pn = calibration().calibrate(p); + return pose().transform_from(backproject_from_camera(pn, depth)); + } + + /// backproject a 2-dimensional point to a 3-dimensional point at infinity + Unit3 backprojectPointAtInfinity(const Point2& p) const { + const Point2 pn = calibration().calibrate(p); + const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 + return pose().rotation().rotate(pc); + } + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @return range (double) + */ + double range(const Point3& point, + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return pose().range(point, Dcamera, Dpoint); + } + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @return range (double) + */ + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return this->pose().range(pose, Dcamera, Dpose); + } + + /** + * Calculate range to a CalibratedCamera + * @param camera Other camera + * @return range (double) + */ + double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera = + boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { + return pose().range(camera.pose(), Dcamera, Dother); + } + + /** + * Calculate range to a PinholePoseK derived class + * @param camera Other camera + * @return range (double) + */ + template + double range(const PinholeBaseK& camera, + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return pose().range(camera.pose(), Dcamera, Dother); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("PinholeBase", + boost::serialization::base_object(*this)); + } + +}; +// end of class PinholeBaseK + +/** + * A pinhole camera class that has a Pose3 and a *fixed* Calibration. + * Instead of using this class, one might consider calibrating the measurements + * and using CalibratedCamera, which would then be faster. + * @addtogroup geometry + * \nosubgrouping + */ +template +class GTSAM_EXPORT PinholePose: public PinholeBaseK { + +private: + + typedef PinholeBaseK Base; ///< base class has 3D pose as private member + boost::shared_ptr K_; ///< shared pointer to fixed calibration + +public: + + enum { + dimension = 6 + }; ///< There are 6 DOF to optimize for + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholePose() { + } + + /** constructor with pose, uses default calibration */ + explicit PinholePose(const Pose3& pose) : + Base(pose), K_(new CALIBRATION()) { + } + + /** constructor with pose and calibration */ + PinholePose(const Pose3& pose, const boost::shared_ptr& K) : + Base(pose), K_(K) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static PinholePose Level(const boost::shared_ptr& K, + const Pose2& pose2, double height) { + return PinholePose(Base::LevelPose(pose2, height), K); + } + + /// PinholePose::level with default calibration + static PinholePose Level(const Pose2& pose2, double height) { + return PinholePose::Level(boost::make_shared(), pose2, height); + } + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + * @param K optional calibration parameter + */ + static PinholePose Lookat(const Point3& eye, const Point3& target, + const Point3& upVector, const boost::shared_ptr& K = + boost::make_shared()) { + return PinholePose(Base::LookatPose(eye, target, upVector), K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + /// Init from 6D vector + explicit PinholePose(const Vector &v) : + Base(v), K_(new CALIBRATION()) { + } + + /// Init from Vector and calibration + PinholePose(const Vector &v, const Vector &K) : + Base(v), K_(new CALIBRATION(K)) { + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const Base &camera, double tol = 1e-9) const { + const PinholePose* e = dynamic_cast(&camera); + return Base::equals(camera, tol) && K_->equals(e->calibration(), tol); + } + + /// print + void print(const std::string& s = "PinholePose") const { + Base::print(s); + if (!K_) + std::cout << "s No calibration given" << std::endl; + else + K_->print(s + ".calibration"); + } + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholePose() { + } + + /// return calibration + virtual const CALIBRATION& calibration() const { + return *K_; + } + + /** project a point from world coordinate to the image, 2 derivatives only + * @param pw is a point in world coordinates + * @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose) + * @param Dpoint is the Jacobian w.r.t. point3 + */ + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { + return Base::project(pw, Dpose, Dpoint); + } + + /// project2 version for point at infinity + Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const { + return Base::project(pw, Dpose, Dpoint); + } + + /// @} + /// @name Manifold + /// @{ + + /// @deprecated + size_t dim() const { + return 6; + } + + /// @deprecated + static size_t Dim() { + return 6; + } + + /// move a cameras according to d + PinholePose retract(const Vector6& d) const { + return PinholePose(Base::pose().retract(d), K_); + } + + /// return canonical coordinate + Vector6 localCoordinates(const PinholePose& p) const { + return Base::pose().localCoordinates(p.Base::pose()); + } + + /// for Canonical + static PinholePose identity() { + return PinholePose(); // assumes that the default constructor is valid + } + + /// @} + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("PinholeBaseK", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(K_); + } + +}; +// end of class PinholePose + +template +struct traits > : public internal::Manifold< + PinholePose > { +}; + +template +struct traits > : public internal::Manifold< + PinholePose > { +}; + +} // \ gtsam diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h new file mode 100644 index 000000000..5101e9fc8 --- /dev/null +++ b/gtsam/geometry/PinholeSet.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * 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 PinholeSet.h + * @brief A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * PinholeSet: triangulates point and keeps an estimate of it around. + */ +template +class PinholeSet: public CameraSet { + +private: + typedef CameraSet Base; + typedef PinholeSet This; + +protected: + +public: + + /** Virtual destructor */ + virtual ~PinholeSet() { + } + + /// @name Testable + /// @{ + + /// print + virtual void print(const std::string& s = "") const { + Base::print(s); + } + + /// equals + bool equals(const PinholeSet& p, double tol = 1e-9) const { + return Base::equals(p, tol); // TODO all flags + } + + /// @} + + /// triangulateSafe + TriangulationResult triangulateSafe( + const std::vector& measured, + const TriangulationParameters& params) const { + return gtsam::triangulateSafe(*this, measured, params); + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 6f4f93cf9..56809ae53 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -148,7 +148,7 @@ public: /// @{ /// equality - inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} + inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;} /// get x double x() const {return x_;} @@ -185,16 +185,18 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); } /// @} - }; +// For MATLAB wrapper +typedef std::vector Point2Vector; + /// multiply with scalar inline Point2 operator*(double s, const Point2& p) {return p*s;} diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 95f728e39..883e5fb62 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -181,7 +181,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); @@ -189,15 +189,14 @@ namespace gtsam { } /// @} - }; - /// Syntactic sugar for multiplying coordinates by a scalar s*p - inline Point3 operator*(double s, const Point3& p) { return p*s;} +/// Syntactic sugar for multiplying coordinates by a scalar s*p +inline Point3 operator*(double s, const Point3& p) { return p*s;} - template<> - struct traits : public internal::VectorSpace {}; +template<> +struct traits : public internal::VectorSpace {}; - template<> - struct traits : public internal::VectorSpace {}; +template<> +struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 0b0172857..9e87407f4 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -59,7 +59,7 @@ bool Pose2::equals(const Pose2& q, double tol) const { } /* ************************************************************************* */ -Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) { +Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) { if (H) *H = Pose2::ExpmapDerivative(xi); assert(xi.size() == 3); Point2 v(xi(0),xi(1)); @@ -130,7 +130,7 @@ Matrix3 Pose2::AdjointMap() const { } /* ************************************************************************* */ -Matrix3 Pose2::adjointMap(const Vector& v) { +Matrix3 Pose2::adjointMap(const Vector3& v) { // See Chirikjian12book2, vol.2, pg. 36 Matrix3 ad = zeros(3,3); ad(0,1) = -v[2]; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d4b647949..f3cb9e2a1 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -118,7 +118,7 @@ public: /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none); + static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); @@ -128,15 +128,14 @@ public: * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ Matrix3 AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const { - assert(xi.size() == 3); + inline Vector3 Adjoint(const Vector3& xi) const { return AdjointMap()*xi; } /** * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 */ - static Matrix3 adjointMap(const Vector& v); + static Matrix3 adjointMap(const Vector3& v); /** * wedge for SE(2): @@ -271,7 +270,7 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); } @@ -291,10 +290,10 @@ typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index be8e1bfed..32fd75eb8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -111,8 +111,10 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { - if (H) *H = ExpmapDerivative(xi); +Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { + if (H) { + *H = ExpmapDerivative(xi); + } // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -254,6 +256,14 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { return J; } +/* ************************************************************************* */ +const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { + if (H) { + *H << Z_3x3, rotation().matrix(); + } + return t_; +} + /* ************************************************************************* */ Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); @@ -280,8 +290,9 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); (*Dpose) << DR, R; } - if (Dpoint) + if (Dpoint) { *Dpoint = R_.matrix(); + } return R_ * p + t_; } @@ -299,17 +310,18 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, +wz, 0.0, -wx, 0.0,-1.0, 0.0, -wy, +wx, 0.0, 0.0, 0.0,-1.0; } - if (Dpoint) + if (Dpoint) { *Dpoint = Rt; + } return q; } /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) const { - if (!H1 && !H2) + if (!H1 && !H2) { return transform_to(point).norm(); - else { + } else { Matrix36 D1; Matrix3 D2; Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); @@ -342,25 +354,25 @@ boost::optional align(const vector& pairs) { return boost::none; // we need at least three pairs // calculate centroids - Vector cp = zero(3), cq = zero(3); - BOOST_FOREACH(const Point3Pair& pair, pairs){ - cp += pair.first.vector(); - cq += pair.second.vector(); -} + Vector3 cp = Vector3::Zero(), cq = Vector3::Zero(); + BOOST_FOREACH(const Point3Pair& pair, pairs) { + cp += pair.first.vector(); + cq += pair.second.vector(); + } double f = 1.0 / n; cp *= f; cq *= f; // Add to form H matrix - Matrix3 H = Eigen::Matrix3d::Zero(); - BOOST_FOREACH(const Point3Pair& pair, pairs){ - Vector dp = pair.first.vector() - cp; - Vector dq = pair.second.vector() - cq; - H += dp * dq.transpose(); -} + Matrix3 H = Z_3x3; + BOOST_FOREACH(const Point3Pair& pair, pairs) { + Vector3 dp = pair.first.vector() - cp; + Vector3 dq = pair.second.vector() - cq; + H += dp * dq.transpose(); + } // Compute SVD - Matrix U,V; + Matrix U, V; Vector S; svd(H, U, S, V); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4130a252e..8a0f23404 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -110,7 +110,7 @@ public: /// @{ /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ - static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none); + static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none); /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); @@ -125,7 +125,7 @@ public: * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ */ - Vector Adjoint(const Vector& xi_b) const { + Vector6 Adjoint(const Vector6& xi_b) const { return AdjointMap() * xi_b; } /// FIXME Not tested - marked as incorrect @@ -223,9 +223,7 @@ public: } /// get translation - const Point3& translation() const { - return t_; - } + const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; /// get x double x() const { @@ -294,7 +292,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(t_); } @@ -322,11 +320,14 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -template<> -struct traits : public internal::LieGroupTraits {}; +// For MATLAB wrapper +typedef std::vector Pose3Vector; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; + +template<> +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index deae1f3a0..198cd5862 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -118,12 +118,12 @@ namespace gtsam { Matrix1 AdjointMap() const { return I_1x1; } /// Left-trivialized derivative of the exponential map - static Matrix ExpmapDerivative(const Vector& v) { + static Matrix ExpmapDerivative(const Vector& /*v*/) { return ones(1); } /// Left-trivialized derivative inverse of the exponential map - static Matrix LogmapDerivative(const Vector& v) { + static Matrix LogmapDerivative(const Vector& /*v*/) { return ones(1); } @@ -200,7 +200,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(s_); } @@ -208,9 +208,9 @@ namespace gtsam { }; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; } // gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d35fa52f4..881c58579 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -428,7 +428,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { #ifndef GTSAM_USE_QUATERNIONS ar & boost::serialization::make_nvp("rot11", rot_(0,0)); @@ -463,9 +463,9 @@ namespace gtsam { GTSAM_EXPORT std::pair RQ(const Matrix3& A); template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; template<> - struct traits : public internal::LieGroupTraits {}; + struct traits : public internal::LieGroup {}; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index a07c3601d..a08168ed8 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -129,11 +129,11 @@ public: }; template<> -struct traits : public internal::LieGroupTraits { +struct traits : public internal::LieGroup { }; template<> -struct traits : public internal::LieGroupTraits { +struct traits : public internal::LieGroup { }; } // end namespace gtsam diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index c6d33bcb3..a119096d4 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -24,7 +24,114 @@ namespace gtsam { /// A simple camera class with a Cal3_S2 calibration - typedef PinholeCamera SimpleCamera; +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; + +/** + * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x + * Use PinholeCameraCal3_S2 instead + */ +class SimpleCamera : public PinholeCameraCal3_S2 { + + typedef PinholeCamera Base; + typedef boost::shared_ptr shared_ptr; + +public: + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + SimpleCamera() : + Base() { + } + + /** constructor with pose */ + explicit SimpleCamera(const Pose3& pose) : + Base(pose) { + } + + /** constructor with pose and calibration */ + SimpleCamera(const Pose3& pose, const Cal3_S2& K) : + Base(pose, K) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static SimpleCamera Level(const Cal3_S2 &K, const Pose2& pose2, + double height) { + return SimpleCamera(Base::LevelPose(pose2, height), K); + } + + /// PinholeCamera::level with default calibration + static SimpleCamera Level(const Pose2& pose2, double height) { + return SimpleCamera::Level(Cal3_S2(), pose2, height); + } + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + * @param K optional calibration parameter + */ + static SimpleCamera Lookat(const Point3& eye, const Point3& target, + const Point3& upVector, const Cal3_S2& K = Cal3_S2()) { + return SimpleCamera(Base::LookatPose(eye, target, upVector), K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + /// Init from vector, can be 6D (default calibration) or dim + explicit SimpleCamera(const Vector &v) : + Base(v) { + } + + /// Init from Vector and calibration + SimpleCamera(const Vector &v, const Vector &K) : + Base(v, K) { + } + + /// Copy this object as its actual derived type. + SimpleCamera::shared_ptr clone() const { return boost::make_shared(*this); } + + + /// @} + /// @name Manifold + /// @{ + + /// move a cameras according to d + SimpleCamera retract(const Vector& d) const { + if ((size_t) d.size() == 6) + return SimpleCamera(this->pose().retract(d), calibration()); + else + return SimpleCamera(this->pose().retract(d.head(6)), + calibration().retract(d.tail(calibration().dim()))); + } + + /// @} + +}; + +template<> +struct traits : public internal::Manifold { +}; + +template<> +struct traits : public internal::Manifold { +}; /// Recover camera from 3*4 camera matrix GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 9170f8282..5c6646454 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -29,16 +29,15 @@ namespace gtsam { } /* ************************************************************************* */ - StereoPoint2 StereoCamera::project(const Point3& point, - OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, - OptionalJacobian<3,6> H3) const { + StereoPoint2 StereoCamera::project(const Point3& point) const { + return project2(point); + } + + /* ************************************************************************* */ + StereoPoint2 StereoCamera::project2(const Point3& point, + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { -#ifdef STEREOCAMERA_CHAIN_RULE - const Point3 q = leftCamPose_.transform_to(point, H1, H2); -#else - // omit derivatives const Point3 q = leftCamPose_.transform_to(point); -#endif if ( q.z() <= 0 ) throw StereoCheiralityException(); @@ -56,12 +55,6 @@ namespace gtsam { // check if derivatives need to be computed if (H1 || H2) { -#ifdef STEREOCAMERA_CHAIN_RULE - // just implement chain rule - Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian - if (H1) *H1 = D_project_point*(*H1); - if (H2) *H2 = D_project_point*(*H2); -#else // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; @@ -76,10 +69,6 @@ namespace gtsam { fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v; *H2 << d * (*H2); } -#endif - if (H3) - // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet - H3->setZero(); } // finally translate @@ -87,15 +76,57 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { - double d = 1.0 / P.z(), d2 = d*d; + StereoPoint2 StereoCamera::project(const Point3& point, + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, + OptionalJacobian<3,0> H3) const { + if (H3) + throw std::runtime_error( + "StereoCamera::project does not support third derivative - BTW use project2"); + return project2(point,H1,H2); + } + + /* ************************************************************************* */ + Point3 StereoCamera::backproject(const StereoPoint2& z) const { + Vector measured = z.vector(); + double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); + double X = Z * (measured[0] - K_->px()) / K_->fx(); + double Y = Z * (measured[2] - K_->py()) / K_->fy(); + Point3 point = leftCamPose_.transform_from(Point3(X, Y, Z)); + return point; + } + + /* ************************************************************************* */ + Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1, + OptionalJacobian<3, 3> H2) const { const Cal3_S2Stereo& K = *K_; - double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); - Matrix3 m; - m << f_x*d, 0.0, -d2*f_x* P.x(), - f_x*d, 0.0, -d2*f_x*(P.x() - b), - 0.0, f_y*d, -d2*f_y* P.y(); - return m; + const double fx = K.fx(), fy = K.fy(), cx = K.px(), cy = K.py(), b = K.baseline(); + + double uL = z.uL(), uR = z.uR(), v = z.v(); + double disparity = uL - uR; + + double local_z = b * fx / disparity; + const Point3 local(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z); + + if(H1 || H2) { + double z_partial_uR = local_z/disparity; + double x_partial_uR = local.x()/disparity; + double y_partial_uR = local.y()/disparity; + Matrix3 D_local_z; + D_local_z << -x_partial_uR + local.z()/fx, x_partial_uR, 0, + -y_partial_uR, y_partial_uR, local.z() / fy, + -z_partial_uR, z_partial_uR, 0; + + Matrix3 D_point_local; + const Point3 point = leftCamPose_.transform_from(local, H1, D_point_local); + + if(H2) { + *H2 = D_point_local * D_local_z; + } + + return point; + } + + return leftCamPose_.transform_from(local); } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 913b1eab3..f09626c9d 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -17,9 +17,6 @@ #pragma once -#include - -#include #include #include #include @@ -28,32 +25,47 @@ namespace gtsam { class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error { public: - StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {} + StereoCheiralityException() : + std::runtime_error("Stereo Cheirality Exception") { + } }; - /** * A stereo camera class, parameterize by left camera pose and stereo calibration * @addtogroup geometry */ class GTSAM_EXPORT StereoCamera { +public: + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef StereoPoint2 Measurement; + private: Pose3 leftCamPose_; Cal3_S2Stereo::shared_ptr K_; public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ - StereoCamera() { + /// Default constructor allocates a calibration! + StereoCamera() : + K_(new Cal3_S2Stereo()) { } + /// Construct from pose and shared calibration StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); + /// Return shared pointer to calibration const Cal3_S2Stereo::shared_ptr calibration() const { return K_; } @@ -62,26 +74,28 @@ public: /// @name Testable /// @{ + /// print void print(const std::string& s = "") const { leftCamPose_.print(s + ".camera."); K_->print(s + ".calibration."); } + /// equals bool equals(const StereoCamera &camera, double tol = 1e-9) const { - return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( - *camera.K_, tol); + return leftCamPose_.equals(camera.leftCamPose_, tol) + && K_->equals(*camera.K_, tol); } /// @} /// @name Manifold /// @{ - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space inline size_t dim() const { return 6; } - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space static inline size_t Dim() { return 6; } @@ -100,46 +114,58 @@ public: /// @name Standard Interface /// @{ + /// pose const Pose3& pose() const { return leftCamPose_; } + /// baseline double baseline() const { return K_->baseline(); } - /* - * project 3D point and compute optional derivatives + /// Project 3D point to StereoPoint2 (uL,uR,v) + StereoPoint2 project(const Point3& point) const; + + /** Project 3D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + */ + StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 = + boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; + + /// back-project a measurement + Point3 backproject(const StereoPoint2& z) const; + + /** Back-project the 2D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + */ + Point3 backproject2(const StereoPoint2& z, + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + + /// @} + /// @name Deprecated + /// @{ + + /** Project 3D point and compute optional derivatives + * @deprecated, use project2 - this class has fixed calibration * @param H1 derivative with respect to pose * @param H2 derivative with respect to point * @param H3 IGNORED (for calibration) */ - StereoPoint2 project(const Point3& point, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none, - OptionalJacobian<3, 6> H3 = boost::none) const; - - /** - * - */ - Point3 backproject(const StereoPoint2& z) const { - Vector measured = z.vector(); - double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); - double X = Z *(measured[0]- K_->px()) / K_->fx(); - double Y = Z *(measured[2]- K_->py()) / K_->fy(); - Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); - return world_point; - } + StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1, + OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = + boost::none) const; /// @} private: - /** utility functions */ - Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(K_); } @@ -147,8 +173,10 @@ private: }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 803c59a4b..1b9559e67 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -18,165 +18,145 @@ #pragma once -#include #include +#include +#include namespace gtsam { - /** - * A 2D stereo point, v will be same for rectified images - * @addtogroup geometry - * \nosubgrouping - */ - class GTSAM_EXPORT StereoPoint2 { - public: - static const size_t dimension = 3; - private: - double uL_, uR_, v_; +/** + * A 2D stereo point, v will be same for rectified images + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT StereoPoint2 { +private: - public: + double uL_, uR_, v_; - /// @name Standard Constructors - /// @{ +public: + enum { dimension = 3 }; + /// @name Standard Constructors + /// @{ - /** default constructor */ - StereoPoint2() : + /** default constructor */ + StereoPoint2() : uL_(0), uR_(0), v_(0) { - } + } - /** constructor */ - StereoPoint2(double uL, double uR, double v) : + /** constructor */ + StereoPoint2(double uL, double uR, double v) : uL_(uL), uR_(uR), v_(v) { - } + } - /// @} - /// @name Testable - /// @{ + /// construct from 3D vector + StereoPoint2(const Vector3& v) : + uL_(v(0)), uR_(v(1)), v_(v(2)) {} - /** print */ - void print(const std::string& s="") const; + /// @} + /// @name Testable + /// @{ - /** equals */ - bool equals(const StereoPoint2& q, double tol=1e-9) const { - return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_ - - q.v_) < tol); - } + /** print */ + void print(const std::string& s = "") const; - /// @} - /// @name Group - /// @{ + /** equals */ + bool equals(const StereoPoint2& q, double tol = 1e-9) const { + return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol + && fabs(v_ - q.v_) < tol); + } - /// identity - inline static StereoPoint2 identity() { return StereoPoint2(); } + /// @} + /// @name Group + /// @{ - /// inverse - inline StereoPoint2 inverse() const { - return StereoPoint2()- (*this); - } + /// identity + inline static StereoPoint2 identity() { + return StereoPoint2(); + } - /// "Compose", just adds the coordinates of two points. - inline StereoPoint2 compose(const StereoPoint2& p1) const { - return *this + p1; - } + /// inverse + StereoPoint2 operator-() const { + return StereoPoint2(-uL_, -uR_, -v_); + } - /// add two stereo points - StereoPoint2 operator+(const StereoPoint2& b) const { - return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); - } + /// add + inline StereoPoint2 operator +(const StereoPoint2& b) const { + return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); + } - /// subtract two stereo points - StereoPoint2 operator-(const StereoPoint2& b) const { - return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); - } - - /// @} - /// @name Manifold - /// @{ + /// subtract + inline StereoPoint2 operator -(const StereoPoint2& b) const { + return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); + } - /// dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Standard Interface + /// @{ - /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return dimension; } + /// equality + inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;} - /// Updates a with tangent space delta - inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + /// get uL + inline double uL() const {return uL_;} - /// Returns inverse retraction - inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + /// get uR + inline double uR() const {return uR_;} - /// @} - /// @name Lie Group - /// @{ + /// get v + inline double v() const {return v_;} - /** Exponential map around identity - just create a Point2 from a vector */ - static inline StereoPoint2 Expmap(const Vector& d) { - return StereoPoint2(d(0), d(1), d(2)); - } + /** convert to vector */ + Vector3 vector() const { + return Vector3(uL_, uR_, v_); + } - /** Log map around identity - just return the Point2 as a vector */ - static inline Vector Logmap(const StereoPoint2& p) { - return p.vector(); - } + /** convenient function to get a Point2 from the left image */ + Point2 point2() const { + return Point2(uL_, v_); + } - /** The difference between another point and this point */ - inline StereoPoint2 between(const StereoPoint2& p2) const { - return p2 - *this; - } + /** convenient function to get a Point2 from the right image */ + Point2 right() const { + return Point2(uR_, v_); + } - /// @} - /// @name Standard Interface - /// @{ + /// @name Deprecated + /// @{ + inline StereoPoint2 inverse() const { return StereoPoint2()- (*this);} + inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1;} + inline StereoPoint2 between(const StereoPoint2& p2) const { return p2 - *this; } + inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); } + static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); } + /// @} - /// get uL - inline double uL() const {return uL_;} + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); - /// get uR - inline double uR() const {return uR_;} +private: - /// get v - inline double v() const {return v_;} + /// @} + /// @name Advanced Interface + /// @{ - /** convert to vector */ - Vector3 vector() const { - return Vector3(uL_, uR_, v_); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(uL_); + ar & BOOST_SERIALIZATION_NVP(uR_); + ar & BOOST_SERIALIZATION_NVP(v_); + } - /** convenient function to get a Point2 from the left image */ - Point2 point2() const { - return Point2(uL_, v_); - } + /// @} - /** convenient function to get a Point2 from the right image */ - Point2 right() const { - return Point2(uR_, v_); - } +}; - /// Streaming - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); +template<> +struct traits : public internal::VectorSpace {}; - private: - - /// @} - /// @name Advanced Interface - /// @{ - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(uL_); - ar & BOOST_SERIALIZATION_NVP(uR_); - ar & BOOST_SERIALIZATION_NVP(v_); - } - - /// @} - - }; - - template<> - struct traits : public internal::Manifold {}; - - template<> - struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 415410aa4..a74e39f47 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -21,6 +21,7 @@ #include #include #include +#include // for GTSAM_USE_TBB #ifdef __clang__ # pragma clang diagnostic push @@ -31,6 +32,10 @@ # pragma clang diagnostic pop #endif +#ifdef GTSAM_USE_TBB +#include +#endif + #include #include @@ -65,8 +70,15 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { return result; } +#ifdef GTSAM_USE_TBB +tbb::mutex unit3BasisMutex; +#endif + /* ************************************************************************* */ const Matrix32& Unit3::basis() const { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(unit3BasisMutex); +#endif // Return cached version if exists if (B_) @@ -108,7 +120,7 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 Matrix23 Bt = basis().transpose(); Vector2 xi = Bt * q.p_.vector(); diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 7d145c213..f8cea092e 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -22,7 +22,7 @@ #include #include -#include + #include #include @@ -108,7 +108,7 @@ public: } /// Return unit-norm Vector - Vector unitVector(boost::optional H = boost::none) const { + Vector3 unitVector(boost::optional H = boost::none) const { if (H) *H = basis(); return (p_.vector()); @@ -120,7 +120,7 @@ public: } /// Signed, vector-valued error between two directions - Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; + Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; /// Distance between two directions double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; @@ -160,7 +160,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(p_); // homebrew serialize Eigen Matrix ar & boost::serialization::make_nvp("B11", (*B_)(0, 0)); diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index aa06a2e29..3e93dedc1 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -26,6 +26,8 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2) static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); static Point2 p(1, -2); +static Point2 p_uv(1320.3, 1740); +static Point2 p_xy(2, 3); /* ************************************************************************* */ TEST( Cal3_S2, easy_constructor) @@ -73,6 +75,27 @@ TEST( Cal3_S2, Duncalibrate2) CHECK(assert_equal(numerical,computed,1e-9)); } +Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); } +/* ************************************************************************* */ +TEST(Cal3_S2, Dcalibrate1) +{ + Matrix computed; + Point2 expected = K.calibrate(p_uv, computed, boost::none); + Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); + CHECK(assert_equal(expected, p_xy, 1e-8)); + CHECK(assert_equal(numerical, computed, 1e-8)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2, Dcalibrate2) +{ + Matrix computed; + Point2 expected = K.calibrate(p_uv, boost::none, computed); + Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); + CHECK(assert_equal(expected, p_xy, 1e-8)); + CHECK(assert_equal(numerical, computed, 1e-8)); +} + /* ************************************************************************* */ TEST( Cal3_S2, assert_equal) { diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 6a990e08e..199ae30ce 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -29,6 +29,7 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) +// Camera situated at 0.5 meters high, looking down static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., @@ -87,34 +88,94 @@ TEST( CalibratedCamera, project) } /* ************************************************************************* */ -TEST( CalibratedCamera, Dproject_to_camera1) { - Point3 pp(155,233,131); +static Point2 Project1(const Point3& point) { + return PinholeBase::Project(point); +} + +TEST( CalibratedCamera, DProject1) { + Point3 pp(155, 233, 131); Matrix test1; - CalibratedCamera::project_to_camera(pp, test1); - Matrix test2 = numericalDerivative11( - boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp); + CalibratedCamera::Project(pp, test1); + Matrix test2 = numericalDerivative11(Project1, pp); CHECK(assert_equal(test1, test2)); } /* ************************************************************************* */ -static Point2 project2(const Pose3& pose, const Point3& point) { - return CalibratedCamera(pose).project(point); +static Point2 Project2(const Unit3& point) { + return PinholeBase::Project(point); +} + +Unit3 pointAtInfinity(0, 0, 1000); +TEST( CalibratedCamera, DProjectInfinity) { + Matrix test1; + CalibratedCamera::Project(pointAtInfinity, test1); + Matrix test2 = numericalDerivative11(Project2, + pointAtInfinity); + CHECK(assert_equal(test1, test2)); +} + +/* ************************************************************************* */ +static Point2 project2(const CalibratedCamera& camera, const Point3& point) { + return camera.project(point); } TEST( CalibratedCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; Point2 result = camera.project(point1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); - Matrix numerical_point = numericalDerivative22(project2, pose1, point1); + Matrix numerical_pose = numericalDerivative21(project2, camera, point1); + Matrix numerical_point = numericalDerivative22(project2, camera, point1); CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1))); CHECK(assert_equal(Point2(-.16, .16), result)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( CalibratedCamera, Dproject_point_pose2) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose1); + Matrix Dpose, Dpoint; + camera.project(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(project2, camera, point1); + Matrix numerical_point = numericalDerivative22(project2, camera, point1); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static Point2 projectAtInfinity(const CalibratedCamera& camera, const Unit3& point) { + return camera.project2(point); +} + +TEST( CalibratedCamera, Dproject_point_pose_infinity) +{ + Matrix Dpose, Dpoint; + Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); + Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity); + CHECK(assert_equal(Point2(), result)); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( CalibratedCamera, Dproject_point_pose2_infinity) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose1); + Matrix Dpose, Dpoint; + camera.project2(pointAtInfinity, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); + Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp new file mode 100644 index 000000000..0afa04411 --- /dev/null +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -0,0 +1,177 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCameraSet.cpp + * @brief Unit tests for testCameraSet Class + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Cal3Bundler test +#include +#include +TEST(CameraSet, Pinhole) { + typedef PinholeCamera Camera; + typedef CameraSet Set; + typedef vector ZZ; + Set set; + Camera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT(assert_equal(set, set)); + Set set2 = set; + set2.push_back(camera); + EXPECT(!set.equals(set2)); + + // Check measurements + Point2 expected; + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix actualE; + Matrix29 F1; + { + Matrix23 E1; + camera.project2(p, F1, E1); + actualE.resize(4,3); + actualE << E1, E1; + } + + // Check computed derivatives + Set::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); + + // Check errors + ZZ measured; + measured.push_back(Point2(1, 2)); + measured.push_back(Point2(3, 4)); + Vector4 expectedV; + + // reprojectionError + expectedV << -1, -2, -3, -4; + Vector actualV = set.reprojectionError(p, measured); + EXPECT(assert_equal(expectedV, actualV)); + + // Check Schur complement + Matrix F(4, 18); + F << F1, Matrix29::Zero(), Matrix29::Zero(), F1; + Matrix Ft = F.transpose(); + Matrix34 Et = E.transpose(); + Matrix3 P = Et * E; + Matrix schur(19, 19); + Vector4 b = actualV; + Vector v = Ft * (b - E * P * Et * b); + schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30; + SymmetricBlockMatrix actualReduced = Set::SchurComplement(Fs, E, P, b); + EXPECT(assert_equal(schur, actualReduced.matrix())); + + // Check Schur complement update, same order, should just double + FastVector allKeys, keys; + allKeys.push_back(1); + allKeys.push_back(2); + keys.push_back(1); + keys.push_back(2); + Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced); + EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.matrix())); + + // Check Schur complement update, keys reversed + FastVector keys2; + keys2.push_back(2); + keys2.push_back(1); + Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced); + Vector4 reverse_b; + reverse_b << b.tail<2>(), b.head<2>(); + Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b); + Matrix A(19, 19); + A << Ft * F - Ft * E * P * Et * F, reverse_v, reverse_v.transpose(), 30; + EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.matrix())); + + // reprojectionErrorAtInfinity + Unit3 pointAtInfinity(0, 0, 1000); + EXPECT( + assert_equal(pointAtInfinity, + camera.backprojectPointAtInfinity(Point2()))); + actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E); + EXPECT(assert_equal(expectedV, actualV)); + LONGS_EQUAL(2, Fs.size()); + { + Matrix22 E1; + camera.project2(pointAtInfinity, F1, E1); + actualE.resize(4,2); + actualE << E1, E1; + } + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); +} + +/* ************************************************************************* */ +#include +TEST(CameraSet, Stereo) { + typedef vector ZZ; + CameraSet set; + StereoCamera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT_LONGS_EQUAL(6, traits::dimension); + + // Check measurements + StereoPoint2 expected(0, -1, 0); + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix63 actualE; + Matrix F1; + { + Matrix33 E1; + camera.project2(p, F1, E1); + actualE << E1, E1; + } + + // Check computed derivatives + CameraSet::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index a15d7e2c2..7becfc75f 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -22,52 +22,114 @@ using namespace std; using namespace gtsam; -typedef Cyclic<3> G; // Let's use the cyclic group of order 3 +typedef Cyclic<3> Z3; // Let's use the cyclic group of order 3 +typedef Cyclic<2> Z2; //****************************************************************************** TEST(Cyclic, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - EXPECT_LONGS_EQUAL(0, traits::Identity()); + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT_LONGS_EQUAL(0, traits::Identity()); } //****************************************************************************** TEST(Cyclic, Constructor) { - G g(0); + Z3 g(0); } //****************************************************************************** TEST(Cyclic, Compose) { - EXPECT_LONGS_EQUAL(0, traits::Compose(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits::Compose(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits::Compose(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits::Compose(Z3(0),Z3(0))); + EXPECT_LONGS_EQUAL(1, traits::Compose(Z3(0),Z3(1))); + EXPECT_LONGS_EQUAL(2, traits::Compose(Z3(0),Z3(2))); - EXPECT_LONGS_EQUAL(2, traits::Compose(G(2),G(0))); - EXPECT_LONGS_EQUAL(0, traits::Compose(G(2),G(1))); - EXPECT_LONGS_EQUAL(1, traits::Compose(G(2),G(2))); + EXPECT_LONGS_EQUAL(2, traits::Compose(Z3(2),Z3(0))); + EXPECT_LONGS_EQUAL(0, traits::Compose(Z3(2),Z3(1))); + EXPECT_LONGS_EQUAL(1, traits::Compose(Z3(2),Z3(2))); } //****************************************************************************** TEST(Cyclic, Between) { - EXPECT_LONGS_EQUAL(0, traits::Between(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, traits::Between(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, traits::Between(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits::Between(Z3(0),Z3(0))); + EXPECT_LONGS_EQUAL(1, traits::Between(Z3(0),Z3(1))); + EXPECT_LONGS_EQUAL(2, traits::Between(Z3(0),Z3(2))); - EXPECT_LONGS_EQUAL(1, traits::Between(G(2),G(0))); - EXPECT_LONGS_EQUAL(2, traits::Between(G(2),G(1))); - EXPECT_LONGS_EQUAL(0, traits::Between(G(2),G(2))); + EXPECT_LONGS_EQUAL(1, traits::Between(Z3(2),Z3(0))); + EXPECT_LONGS_EQUAL(2, traits::Between(Z3(2),Z3(1))); + EXPECT_LONGS_EQUAL(0, traits::Between(Z3(2),Z3(2))); } //****************************************************************************** -TEST(Cyclic, Ivnverse) { - EXPECT_LONGS_EQUAL(0, traits::Inverse(G(0))); - EXPECT_LONGS_EQUAL(2, traits::Inverse(G(1))); - EXPECT_LONGS_EQUAL(1, traits::Inverse(G(2))); +TEST(Cyclic, Inverse) { + EXPECT_LONGS_EQUAL(0, traits::Inverse(Z3(0))); + EXPECT_LONGS_EQUAL(2, traits::Inverse(Z3(1))); + EXPECT_LONGS_EQUAL(1, traits::Inverse(Z3(2))); +} + +//****************************************************************************** +TEST(Cyclic, Negation) { + EXPECT_LONGS_EQUAL(0, -Z3(0)); + EXPECT_LONGS_EQUAL(2, -Z3(1)); + EXPECT_LONGS_EQUAL(1, -Z3(2)); +} + +//****************************************************************************** +TEST(Cyclic, Negation2) { + EXPECT_LONGS_EQUAL(0, -Z2(0)); + EXPECT_LONGS_EQUAL(1, -Z2(1)); } //****************************************************************************** TEST(Cyclic , Invariants) { - G g(2), h(1); - check_group_invariants(g,h); + Z3 g(2), h(1); + EXPECT(check_group_invariants(g,h)); +} + +//****************************************************************************** +// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the +// smallest non-cyclic group called the Klein four-group: +typedef DirectSum K4; + +namespace gtsam { + +/// Define K4 to be a model of the Additive Group concept, and provide Testable +template<> +struct traits : internal::AdditiveGroupTraits { + static void Print(const K4& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second << ")" << endl; + } + static bool Equals(const K4& m1, const K4& m2, double tol = 1e-8) { + return m1 == m2; + } +}; + +} // namespace gtsam + +TEST(Cyclic , DirectSum) { + // The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the + // smallest non-cyclic group called the Klein four-group: + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsTestable)); + + // Refer to http://en.wikipedia.org/wiki/Klein_four-group + K4 e(0,0), a(0, 1), b(1, 0), c(1, 1); + EXPECT(assert_equal(a, - a)); + EXPECT(assert_equal(b, - b)); + EXPECT(assert_equal(c, - c)); + EXPECT(assert_equal(a, a + e)); + EXPECT(assert_equal(b, b + e)); + EXPECT(assert_equal(c, c + e)); + EXPECT(assert_equal(e, a + a)); + EXPECT(assert_equal(e, b + b)); + EXPECT(assert_equal(e, c + c)); + EXPECT(assert_equal(c, a + b)); + EXPECT(assert_equal(b, a + c)); + EXPECT(assert_equal(a, b + c)); + EXPECT(assert_equal(c, a - b)); + EXPECT(assert_equal(a, b - c)); + EXPECT(assert_equal(b, c - a)); + EXPECT(check_group_invariants(a, b)); + EXPECT(check_group_invariants(b, c)); + EXPECT(check_group_invariants(c, a)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index d2990a747..fe27b2911 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -30,6 +30,14 @@ TEST (EssentialMatrix, equality) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, FromPose3) { + EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)); + Pose3 pose(c1Rc2, c1Tc2); + EssentialMatrix actual = EssentialMatrix::FromPose3(pose); + EXPECT(assert_equal(expected, actual)); +} + //******************************************************************************* TEST(EssentialMatrix, localCoordinates0) { EssentialMatrix E; @@ -47,11 +55,11 @@ TEST (EssentialMatrix, localCoordinates) { Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); EXPECT(assert_equal(zero(5), actual, 1e-8)); - Vector d = zero(6); - d(4) += 1e-5; + Vector6 d; + d << 0.1, 0.2, 0.3, 0, 0, 0; Vector actual2 = hx.localCoordinates( EssentialMatrix::FromPose3(pose.retract(d))); - EXPECT(assert_equal(zero(5), actual2, 1e-8)); + EXPECT(assert_equal(d.head(5), actual2, 1e-8)); } //************************************************************************* @@ -75,6 +83,15 @@ TEST (EssentialMatrix, retract2) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, RoundTrip) { + Vector5 d; + d << 0.1, 0.2, 0.3, 0.4, 0.5; + EssentialMatrix e, hx = e.retract(d); + Vector actual = e.localCoordinates(hx); + EXPECT(assert_equal(d, actual, 1e-8)); +} + //************************************************************************* Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 14a387102..11931449f 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -13,62 +13,85 @@ * @file testOrientedPlane3.cpp * @date Dec 19, 2012 * @author Alex Trevor + * @author Zhaoyang Lv * @brief Tests the OrientedPlane3 class */ -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include -#include -#include #include using namespace boost::assign; using namespace gtsam; using namespace std; +using boost::none; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) //******************************************************************************* -TEST (OrientedPlane3, transform) -{ - // Test transforming a plane to a pose - gtsam::Pose3 pose(gtsam::Rot3::ypr (-M_PI/4.0, 0.0, 0.0), gtsam::Point3(2.0, 3.0, 4.0)); - OrientedPlane3 plane (-1 , 0, 0, 5); - OrientedPlane3 expected_meas (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3); - OrientedPlane3 transformed_plane = OrientedPlane3::Transform (plane, pose, boost::none, boost::none); - EXPECT (assert_equal (expected_meas, transformed_plane, 1e-9)); +TEST (OrientedPlane3, getMethods) { + Vector4 c; + c << -1, 0, 0, 5; + OrientedPlane3 plane1(c); + OrientedPlane3 plane2(c[0], c[1], c[2], c[3]); + Vector4 coefficient1 = plane1.planeCoefficients(); + double distance1 = plane1.distance(); + EXPECT(assert_equal(coefficient1, c, 1e-8)); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane1.normal().unitVector())); + EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8); + Vector4 coefficient2 = plane2.planeCoefficients(); + double distance2 = plane2.distance(); + EXPECT(assert_equal(coefficient2, c, 1e-8)); + EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane2.normal().unitVector())); +} + + +//******************************************************************************* +OrientedPlane3 Transform_(const OrientedPlane3& plane, const Pose3& xr) { + return OrientedPlane3::Transform(plane, xr); +} + +OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) { + return plane.transform(xr); +} + +TEST (OrientedPlane3, transform) { + gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0), + gtsam::Point3(2.0, 3.0, 4.0)); + OrientedPlane3 plane(-1, 0, 0, 5); + OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); + OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose, + none, none); + OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none); + EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-9)); + EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-9)); // Test the jacobians of transform Matrix actualH1, expectedH1, actualH2, expectedH2; { - expectedH1 = numericalDerivative11(boost::bind (&OrientedPlane3::Transform, plane, _1, boost::none, boost::none), pose); - - OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none); - EXPECT (assert_equal (expectedH1, actualH1, 1e-9)); + // because the Transform class uses a wrong order of Jacobians in interface + OrientedPlane3::Transform(plane, pose, actualH1, none); + expectedH1 = numericalDerivative22(Transform_, plane, pose); + EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + OrientedPlane3::Transform(plane, pose, none, actualH2); + expectedH2 = numericalDerivative21(Transform_, plane, pose); + EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } { - expectedH2 = numericalDerivative11 (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane); - - OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2); - EXPECT (assert_equal (expectedH2, actualH2, 1e-9)); + plane.transform(pose, actualH1, none); + expectedH1 = numericalDerivative21(transform_, plane, pose); + EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + plane.transform(pose, none, actualH2); + expectedH2 = numericalDerivative22(Transform_, plane, pose); + EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } - } //******************************************************************************* -// Returns a random vector -- copied from testUnit3.cpp +// Returns a any size random vector inline static Vector randomVector(const Vector& minLimits, const Vector& maxLimits) { @@ -78,23 +101,23 @@ inline static Vector randomVector(const Vector& minLimits, // Create the random vector for (size_t i = 0; i < numDims; i++) { - double range = maxLimits(i) - minLimits(i); - vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); - } + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } return vector; } //******************************************************************************* TEST(OrientedPlane3, localCoordinates_retract) { - + size_t numIterations = 10000; - gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4); + Vector4 minPlaneLimit, maxPlaneLimit; minPlaneLimit << -1.0, -1.0, -1.0, 0.01; maxPlaneLimit << 1.0, 1.0, 1.0, 10.0; - Vector minXiLimit(3),maxXiLimit(3); - minXiLimit << -M_PI, -M_PI, -10.0; - maxXiLimit << M_PI, M_PI, 10.0; + Vector3 minXiLimit, maxXiLimit; + minXiLimit << -M_PI, -M_PI, -10.0; + maxXiLimit << M_PI, M_PI, 10.0; for (size_t i = 0; i < numIterations; i++) { sleep(0); @@ -104,15 +127,15 @@ TEST(OrientedPlane3, localCoordinates_retract) { Vector v12 = randomVector(minXiLimit, maxXiLimit); // Magnitude of the rotation can be at most pi - if (v12.segment<3>(0).norm () > M_PI) - v12.segment<3>(0) = v12.segment<3>(0) / M_PI; + if (v12.head<3>().norm() > M_PI) + v12.head<3>() = v12.head<3>() / M_PI; OrientedPlane3 p2 = p1.retract(v12); // Check if the local coordinates and retract return the same results. Vector actual_v12 = p1.localCoordinates(p2); - EXPECT(assert_equal(v12, actual_v12, 1e-3)); + EXPECT(assert_equal(v12, actual_v12, 1e-6)); OrientedPlane3 actual_p2 = p1.retract(actual_v12); - EXPECT(assert_equal(p2, actual_p2, 1e-3)); + EXPECT(assert_equal(p2, actual_p2, 1e-6)); } } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 20f7a3231..74bc4ca2a 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -15,12 +15,14 @@ * @brief test PinholeCamera class */ -#include -#include -#include #include #include #include +#include +#include +#include + +#include #include #include @@ -43,10 +45,10 @@ static const Point3 point2(-0.08, 0.08, 0.0); static const Point3 point3( 0.08, 0.08, 0.0); static const Point3 point4( 0.08,-0.08, 0.0); -static const Point3 point1_inf(-0.16,-0.16, -1.0); -static const Point3 point2_inf(-0.16, 0.16, -1.0); -static const Point3 point3_inf( 0.16, 0.16, -1.0); -static const Point3 point4_inf( 0.16,-0.16, -1.0); +static const Unit3 point1_inf(-0.16,-0.16, -1.0); +static const Unit3 point2_inf(-0.16, 0.16, -1.0); +static const Unit3 point3_inf( 0.16, 0.16, -1.0); +static const Unit3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) @@ -152,9 +154,9 @@ TEST( PinholeCamera, backprojectInfinity2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backprojectPointAtInfinity(Point2()); - Point3 expected(0., 1., 0.); - Point2 x = camera.projectPointAtInfinity(expected); + Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 expected(0., 1., 0.); + Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(Point2(), x)); @@ -167,9 +169,9 @@ TEST( PinholeCamera, backprojectInfinity3) Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backprojectPointAtInfinity(Point2()); - Point3 expected(0., 0., 1.); - Point2 x = camera.projectPointAtInfinity(expected); + Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 expected(0., 0., 1.); + Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(Point2(), x)); @@ -195,17 +197,17 @@ TEST( PinholeCamera, Dproject) } /* ************************************************************************* */ -static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) { - return Camera(pose,cal).projectPointAtInfinity(point3D); +static Point2 projectInfinity3(const Pose3& pose, const Unit3& point3D, const Cal3_S2& cal) { + return Camera(pose,cal).project(point3D); } TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; - Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 + Unit3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 // test Projection - Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); + Point2 actual = camera.project(point3D, Dpose, Dpoint, Dcal); Point2 expected(-5.0, 5.0); EXPECT(assert_equal(actual, expected, 1e-7)); @@ -236,6 +238,20 @@ TEST( PinholeCamera, Dproject2) EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); } +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( PinholeCamera, Dproject3) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Camera camera(pose1); + Matrix Dpose, Dpoint; + camera.project2(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + /* ************************************************************************* */ static double range0(const Camera& camera, const Point3& point) { return camera.range(point); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp new file mode 100644 index 000000000..dc294899f --- /dev/null +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -0,0 +1,278 @@ +/* ---------------------------------------------------------------------------- + + * 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 testPinholePose.cpp + * @author Frank Dellaert + * @brief test PinholePose class + * @date Feb 20, 2015 + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +typedef PinholePose Camera; + +static const Cal3_S2::shared_ptr K = boost::make_shared(625, 625, 0, 0, 0); + +static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Camera camera(pose, K); + +static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); +static const Camera camera1(pose1, K); + +static const Point3 point1(-0.08,-0.08, 0.0); +static const Point3 point2(-0.08, 0.08, 0.0); +static const Point3 point3( 0.08, 0.08, 0.0); +static const Point3 point4( 0.08,-0.08, 0.0); + +static const Unit3 point1_inf(-0.16,-0.16, -1.0); +static const Unit3 point2_inf(-0.16, 0.16, -1.0); +static const Unit3 point3_inf( 0.16, 0.16, -1.0); +static const Unit3 point4_inf( 0.16,-0.16, -1.0); + +/* ************************************************************************* */ +TEST( PinholePose, constructor) +{ + EXPECT(assert_equal( pose, camera.pose())); +} + +//****************************************************************************** +TEST(PinholeCamera, Pose) { + + Matrix actualH; + EXPECT(assert_equal(pose, camera.getPose(actualH))); + + // Check derivative + boost::function f = // + boost::bind(&Camera::getPose,_1,boost::none); + Matrix numericalH = numericalDerivative11(f,camera); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +/* ************************************************************************* */ +TEST( PinholePose, lookat) +{ + // Create a level camera, looking in Y-direction + Point3 C(10.0,0.0,0.0); + Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + + // expected + Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); + Pose3 expected(Rot3(xc,yc,zc),C); + EXPECT(assert_equal( camera.pose(), expected)); + + Point3 C2(30.0,0.0,10.0); + Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + + Matrix R = camera2.pose().rotation().matrix(); + Matrix I = trans(R)*R; + EXPECT(assert_equal(I, eye(3))); +} + +/* ************************************************************************* */ +TEST( PinholePose, project) +{ + EXPECT(assert_equal( camera.project2(point1), Point2(-100, 100) )); + EXPECT(assert_equal( camera.project2(point2), Point2(-100, -100) )); + EXPECT(assert_equal( camera.project2(point3), Point2( 100, -100) )); + EXPECT(assert_equal( camera.project2(point4), Point2( 100, 100) )); +} + +/* ************************************************************************* */ +TEST( PinholePose, backproject) +{ + EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); + EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); + EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); + EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); +} + +/* ************************************************************************* */ +TEST( PinholePose, backprojectInfinity) +{ + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); +} + +/* ************************************************************************* */ +TEST( PinholePose, backproject2) +{ + Point3 origin; + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down + Camera camera(Pose3(rot, origin), K); + + Point3 actual = camera.backproject(Point2(), 1.); + Point3 expected(0., 1., 0.); + pair x = camera.projectSafe(expected); + + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x.first)); + EXPECT(x.second); +} + +/* ************************************************************************* */ +static Point2 project3(const Pose3& pose, const Point3& point, + const Cal3_S2::shared_ptr& cal) { + return Camera(pose, cal).project2(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, Dproject) +{ + Matrix Dpose, Dpoint; + Point2 result = camera.project2(point1, Dpose, Dpoint); + Matrix expectedDcamera = numericalDerivative31(project3, pose, point1, K); + Matrix expectedDpoint = numericalDerivative32(project3, pose, point1, K); + EXPECT(assert_equal(Point2(-100, 100), result)); + EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static Point2 project4(const Camera& camera, const Point3& point) { + return camera.project2(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, Dproject2) +{ + Matrix Dcamera, Dpoint; + Point2 result = camera.project2(point1, Dcamera, Dpoint); + Matrix expectedDcamera = numericalDerivative21(project4, camera, point1); + Matrix expectedDpoint = numericalDerivative22(project4, camera, point1); + EXPECT(assert_equal(result, Point2(-100, 100) )); + EXPECT(assert_equal(expectedDcamera, Dcamera, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( CalibratedCamera, Dproject3) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Camera camera(pose1); + Matrix Dpose, Dpoint; + camera.project2(point1, Dpose, Dpoint); + Matrix expectedDcamera = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(expectedDcamera, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity, + const Cal3_S2::shared_ptr& cal) { + return Camera(pose, cal).project(pointAtInfinity); +} + +/* ************************************************************************* */ +TEST( PinholePose, DprojectAtInfinity2) +{ + Unit3 pointAtInfinity(0,0,1000); + Matrix Dpose, Dpoint; + Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); + Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K); + Matrix expectedDpoint = numericalDerivative32(project, pose, pointAtInfinity, K); + EXPECT(assert_equal(Point2(0,0), result)); + EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static double range0(const Camera& camera, const Point3& point) { + return camera.range(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, range0) { + Matrix D1; Matrix D2; + double result = camera.range(point1, D1, D2); + Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); + Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); + EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + 1e-9); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); +} + +/* ************************************************************************* */ +static double range1(const Camera& camera, const Pose3& pose) { + return camera.range(pose); +} + +/* ************************************************************************* */ +TEST( PinholePose, range1) { + Matrix D1; Matrix D2; + double result = camera.range(pose1, D1, D2); + Matrix expectedDcamera = numericalDerivative21(range1, camera, pose1); + Matrix expectedDpoint = numericalDerivative22(range1, camera, pose1); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); +} + +/* ************************************************************************* */ +typedef PinholePose Camera2; +static const boost::shared_ptr K2 = + boost::make_shared(625, 1e-3, 1e-3); +static const Camera2 camera2(pose1, K2); +static double range2(const Camera& camera, const Camera2& camera2) { + return camera.range(camera2); +} + +/* ************************************************************************* */ +TEST( PinholePose, range2) { + Matrix D1; Matrix D2; + double result = camera.range(camera2, D1, D2); + Matrix expectedDcamera = numericalDerivative21(range2, camera, camera2); + Matrix expectedDpoint = numericalDerivative22(range2, camera, camera2); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); +} + +/* ************************************************************************* */ +static const CalibratedCamera camera3(pose1); +static double range3(const Camera& camera, const CalibratedCamera& camera3) { + return camera.range(camera3); +} + +/* ************************************************************************* */ +TEST( PinholePose, range3) { + Matrix D1; Matrix D2; + double result = camera.range(camera3, D1, D2); + Matrix expectedDcamera = numericalDerivative21(range3, camera, camera3); + Matrix expectedDpoint = numericalDerivative22(range3, camera, camera3); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp new file mode 100644 index 000000000..b8f001f1c --- /dev/null +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCameraSet.cpp + * @brief Unit tests for testCameraSet Class + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +TEST(PinholeSet, Stereo) { + typedef vector ZZ; + PinholeSet set; + CalibratedCamera camera; + set.push_back(camera); + set.push_back(camera); + // set.print("set: "); + Point3 p(0, 0, 1); + EXPECT_LONGS_EQUAL(6, traits::dimension); + + // Check measurements + Point2 expected(0, 0); + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix43 actualE; + Matrix F1; + { + Matrix23 E1; + camera.project2(p, F1, E1); + actualE << E1, E1; + } + + // Check computed derivatives + PinholeSet::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); + + // Instantiate triangulateSafe + // TODO triangulation does not work yet for CalibratedCamera + // PinholeSet::Result actual = set.triangulateSafe(z); +} + +/* ************************************************************************* */ +// Cal3Bundler test +#include +#include +TEST(PinholeSet, Pinhole) { + typedef PinholeCamera Camera; + typedef vector ZZ; + PinholeSet set; + Camera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT(assert_equal(set, set)); + PinholeSet set2 = set; + set2.push_back(camera); + EXPECT(!set.equals(set2)); + + // Check measurements + Point2 expected; + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix actualE; + Matrix F1; + { + Matrix23 E1; + camera.project2(p, F1, E1); + actualE.resize(4, 3); + actualE << E1, E1; + } + + // Check computed derivatives + { + PinholeSet::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + EXPECT(assert_equal(actualE, E)); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + } + + // Check errors + ZZ measured; + measured.push_back(Point2(1, 2)); + measured.push_back(Point2(3, 4)); + Vector4 expectedV; + + // reprojectionError + expectedV << -1, -2, -3, -4; + Vector actualV = set.reprojectionError(p, measured); + EXPECT(assert_equal(expectedV, actualV)); + + // reprojectionErrorAtInfinity + Unit3 pointAtInfinity(0, 0, 1000); + { + Matrix22 E1; + camera.project2(pointAtInfinity, F1, E1); + actualE.resize(4, 2); + actualE << E1, E1; + } + EXPECT( + assert_equal(pointAtInfinity, + camera.backprojectPointAtInfinity(Point2()))); + { + PinholeSet::FBlocks Fs; + Matrix E; + actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E); + EXPECT(assert_equal(actualE, E)); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + } + EXPECT(assert_equal(expectedV, actualV)); + + // Instantiate triangulateSafe + TriangulationParameters params; + TriangulationResult actual = set.triangulateSafe(z, params); + CHECK(actual.degenerate()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index fce7955e9..6f8d45b3e 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -37,8 +37,8 @@ TEST(Double , Concept) { //****************************************************************************** TEST(Double , Invariants) { double p1(2), p2(5); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } //****************************************************************************** @@ -51,8 +51,8 @@ TEST(Point2 , Concept) { //****************************************************************************** TEST(Point2 , Invariants) { Point2 p1(1, 2), p2(4, 5); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } /* ************************************************************************* */ @@ -61,6 +61,12 @@ TEST(Point2, constructor) { EXPECT(assert_equal(p1, p2)); } +/* ************************************************************************* */ +TEST(Point2, equality) { + Point2 p1(1, 2), p2(1,3); + EXPECT(!(p1 == p2)); +} + /* ************************************************************************* */ TEST(Point2, Lie) { Point2 p1(1, 2), p2(4, 5); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 5c6b32dca..6811ed122 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -36,8 +36,8 @@ TEST(Point3 , Concept) { //****************************************************************************** TEST(Point3 , Invariants) { Point3 p1(1, 2, 3), p2(4, 5, 6); - check_group_invariants(p1, p2); - check_manifold_invariants(p1, p2); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 803bb7c3f..5b835200a 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -775,15 +775,15 @@ Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0)); TEST(Pose2 , Invariants) { Pose2 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index a716406a4..98c80e356 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -186,6 +186,17 @@ TEST(Pose3, expmaps_galore_full) EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); } +/* ************************************************************************* */ +// Check translation and its pushforward +TEST(Pose3, translation) { + Matrix actualH; + EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8)); + + Matrix numericalH = numericalDerivative11( + boost::bind(&Pose3::translation, _1, boost::none), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + /* ************************************************************************* */ TEST(Pose3, Adjoint_compose_full) { @@ -749,15 +760,15 @@ TEST( Pose3, stream) TEST(Pose3 , Invariants) { Pose3 id; - check_group_invariants(id,id); - check_group_invariants(id,T3); - check_group_invariants(T2,id); - check_group_invariants(T2,T3); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T3)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T3)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T3); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T3); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T3)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T3)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 82d3283bd..0421e1e44 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -107,15 +107,15 @@ TEST(Quaternion , Inverse) { //****************************************************************************** TEST(Quaternion , Invariants) { - check_group_invariants(id, id); - check_group_invariants(id, R1); - check_group_invariants(R2, id); - check_group_invariants(R2, R1); + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, R1)); + EXPECT(check_group_invariants(R2, id)); + EXPECT(check_group_invariants(R2, R1)); - check_manifold_invariants(id, id); - check_manifold_invariants(id, R1); - check_manifold_invariants(R2, id); - check_manifold_invariants(R2, R1); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, R1)); + EXPECT(check_manifold_invariants(R2, id)); + EXPECT(check_manifold_invariants(R2, R1)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 37054fd83..6ead22860 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -163,15 +163,15 @@ Rot2 T2(0.2); TEST(Rot2 , Invariants) { Rot2 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 7e0dcc43f..349f87029 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -659,17 +659,17 @@ Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); TEST(Rot3 , Invariants) { Rot3 id; - check_group_invariants(id,id); - check_group_invariants(id,T1); - check_group_invariants(T2,id); - check_group_invariants(T2,T1); - check_group_invariants(T1,T2); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T1)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T1)); + EXPECT(check_group_invariants(T1,T2)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,T1); - check_manifold_invariants(T2,id); - check_manifold_invariants(T2,T1); - check_manifold_invariants(T1,T2); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T1)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T1)); + EXPECT(check_manifold_invariants(T1,T2)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index acbf3bcf5..bc32e0df0 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -57,15 +57,15 @@ TEST(SO3 , Retract) { //****************************************************************************** TEST(SO3 , Invariants) { - check_group_invariants(id,id); - check_group_invariants(id,R1); - check_group_invariants(R2,id); - check_group_invariants(R2,R1); + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,R1)); + EXPECT(check_group_invariants(R2,id)); + EXPECT(check_group_invariants(R2,R1)); - check_manifold_invariants(id,id); - check_manifold_invariants(id,R1); - check_manifold_invariants(R2,id); - check_manifold_invariants(R2,R1); + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,R1)); + EXPECT(check_manifold_invariants(R2,id)); + EXPECT(check_manifold_invariants(R2,R1)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index 84fe5980e..dfef0a9c5 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -16,8 +16,6 @@ * @date Feb 7, 2012 */ -#include -#include #include #include #include diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 002cbe51b..71979481c 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -15,13 +15,13 @@ * @brief test SimpleCamera class */ -#include -#include - -#include +#include +#include #include #include -#include +#include +#include +#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index c77509b91..3adf2257d 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -96,20 +96,16 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_ TEST( StereoCamera, Dproject) { Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); - Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none); + Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); CHECK(assert_equal(expected1,actual1,1e-7)); - Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K); - Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none); + Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K); + Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); CHECK(assert_equal(expected2,actual2,1e-7)); - - Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K); - Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3); -// CHECK(assert_equal(expected3,actual3,1e-8)); } /* ************************************************************************* */ -TEST( StereoCamera, backproject) +TEST( StereoCamera, backproject_case1) { Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); StereoCamera stereoCam2(Pose3(), K2); @@ -121,7 +117,7 @@ TEST( StereoCamera, backproject) } /* ************************************************************************* */ -TEST( StereoCamera, backproject2) +TEST( StereoCamera, backproject_case2) { Rot3 R(0.589511291, -0.804859792, 0.0683931805, -0.804435942, -0.592650676, -0.0405925523, @@ -136,6 +132,53 @@ TEST( StereoCamera, backproject2) CHECK(assert_equal(z, actual, 1e-3)); } +static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) { + return StereoCamera(pose, boost::make_shared(K)).backproject(point); +} + +/* ************************************************************************* */ +TEST( StereoCamera, backproject2_case1) +{ + Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam2(Pose3(), K2); + + Point3 expected_point(1.2, 2.3, 4.5); + StereoPoint2 stereo_point = stereoCam2.project(expected_point); + + Matrix actual_jacobian_1, actual_jacobian_2; + Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2); + CHECK(assert_equal(expected_point, actual_point, 1e-8)); + + Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6)); + + Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6)); +} + +TEST( StereoCamera, backproject2_case2) +{ + Rot3 R(0.589511291, -0.804859792, 0.0683931805, + -0.804435942, -0.592650676, -0.0405925523, + 0.0732045588, -0.0310882277, -0.996832359); + Point3 t(53.5239823, 23.7866016, -4.42379876); + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + StereoCamera camera(Pose3(R,t), K); + StereoPoint2 z(184.812, 129.068, 714.768); + + Matrix actual_jacobian_1, actual_jacobian_2; + Point3 l = camera.backproject2(z, actual_jacobian_1, actual_jacobian_2); + + StereoPoint2 actual = camera.project(l); + CHECK(assert_equal(z, actual, 1e-3)); + + Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(R,t), z, *K); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6)); + + Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(R,t), z, *K); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 5496b8aac..ddcc9238a 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -31,7 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) //****************************************************************************** TEST(StereoPoint2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f986cca1d..352493683 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include @@ -49,6 +50,7 @@ Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); //****************************************************************************** +// Simple test with a well-behaved two camera situation TEST( triangulation, twoPoses) { vector poses; @@ -57,24 +59,37 @@ TEST( triangulation, twoPoses) { poses += pose1, pose2; measurements += z1, z2; - bool optimize = true; double rank_tol = 1e-9; - boost::optional triangulated_landmark = triangulatePoint3(poses, - sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + // 1. Test simple DLT, perfect in no noise situation + bool optimize = false; + boost::optional actual1 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual1, 1e-7)); - // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + // 2. test with optimization on, same answer + optimize = true; + boost::optional actual2 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual2, 1e-7)); + + // 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); + optimize = false; + boost::optional actual3 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + // 4. Now with optimization on + optimize = true; + boost::optional actual4 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } //****************************************************************************** - +// Similar, but now with Bundler calibration TEST( triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // @@ -95,17 +110,17 @@ TEST( triangulation, twoPosesBundler) { bool optimize = true; double rank_tol = 1e-9; - boost::optional triangulated_landmark = triangulatePoint3(poses, - bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + boost::optional actual = // + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual, 1e-7)); - // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + // Add some noise and try again measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + boost::optional actual2 = // + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4)); } //****************************************************************************** @@ -116,17 +131,17 @@ TEST( triangulation, fourPoses) { poses += pose1, pose2; measurements += z1, z2; - boost::optional triangulated_landmark = triangulatePoint3(poses, - sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + boost::optional actual = triangulatePoint3(poses, sharedCal, + measurements); + EXPECT(assert_equal(landmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = // + boost::optional actual2 = // triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); @@ -150,7 +165,7 @@ TEST( triangulation, fourPoses) { SimpleCamera camera4(pose4, *sharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); poses += pose4; measurements += Point2(400, 400); @@ -180,17 +195,17 @@ TEST( triangulation, fourPoses_distinct_Ks) { cameras += camera1, camera2; measurements += z1, z2; - boost::optional triangulated_landmark = // + boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + EXPECT(assert_equal(landmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = // + boost::optional actual2 = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); @@ -216,7 +231,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { SimpleCamera camera4(pose4, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); cameras += camera4; measurements += Point2(400, 400); @@ -244,23 +259,19 @@ TEST( triangulation, twoIdenticalPoses) { } //****************************************************************************** -/* - TEST( triangulation, onePose) { - // we expect this test to fail with a TriangulationUnderconstrainedException - // because there's only one camera observation +TEST( triangulation, onePose) { + // we expect this test to fail with a TriangulationUnderconstrainedException + // because there's only one camera observation - Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480); + vector poses; + vector measurements; - vector poses; - vector measurements; + poses += Pose3(); + measurements += Point2(); - poses += Pose3(); - measurements += Point2(); - - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal), - TriangulationUnderconstrainedException); - } - */ + CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + TriangulationUnderconstrainedException); +} //****************************************************************************** int main() { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 474689525..c92aa8237 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -23,14 +23,8 @@ namespace gtsam { -/** - * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 - * @param projection_matrices Projection matrices (K*P^-1) - * @param measurements 2D measurements - * @param rank_tol SVD rank tolerance - * @return Triangulated Point3 - */ -Point3 triangulateDLT(const std::vector& projection_matrices, +Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { // number of cameras @@ -52,12 +46,20 @@ Point3 triangulateDLT(const std::vector& projection_matrices, double error; Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - // std::cout << "s " << s.transpose() << std:endl; if (rank < 3) throw(TriangulationUnderconstrainedException()); - // Create 3D point from eigenvector + return v; +} + +Point3 triangulateDLT(const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol) { + + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, + rank_tol); + + // Create 3D point from homogeneous coordinates return Point3(sub((v / v(3)), 0, 3)); } @@ -89,6 +91,5 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } - } // \namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index ce83f780b..4ac634f03 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,7 +18,7 @@ #pragma once - +#include #include #include #include @@ -43,6 +43,17 @@ public: } }; +/** + * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 + * @param projection_matrices Projection matrices (K*P^-1) + * @param measurements 2D measurements + * @param rank_tol SVD rank tolerance + * @return Triangulated point, in homogeneous coordinates + */ +GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol = 1e-9); + /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) @@ -52,9 +63,8 @@ public: */ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol); + const std::vector& measurements, double rank_tol = 1e-9); -/// /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses @@ -76,8 +86,9 @@ std::pair triangulationGraph( static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; - PinholeCamera camera_i(pose_i, *sharedCal); - graph.push_back(TriangulationFactor // + typedef PinholePose Camera; + Camera camera_i(pose_i, sharedCal); + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); @@ -92,9 +103,9 @@ std::pair triangulationGraph( * @param initialEstimate * @return graph and initial values */ -template +template std::pair triangulationGraph( - const std::vector >& cameras, + const std::vector& cameras, const std::vector& measurements, Key landmarkKey, const Point3& initialEstimate) { Values values; @@ -103,14 +114,23 @@ std::pair triangulationGraph( static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { - const PinholeCamera& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // + const CAMERA& camera_i = cameras[i]; + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); } -/// +/// PinholeCamera specific version +template +std::pair triangulationGraph( + const std::vector >& cameras, + const std::vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + return triangulationGraph > // + (cameras, measurements, landmarkKey, initialEstimate); +} + /** * Optimize for triangulation * @param graph nonlinear factors for projection @@ -137,8 +157,8 @@ Point3 triangulateNonlinear(const std::vector& poses, // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements, - Symbol('p', 0), initialEstimate); + boost::tie(graph, values) = triangulationGraph // + (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); return optimize(graph, values, Symbol('p', 0)); } @@ -150,20 +170,48 @@ Point3 triangulateNonlinear(const std::vector& poses, * @param initialEstimate * @return refined Point3 */ -template +template Point3 triangulateNonlinear( - const std::vector >& cameras, + const std::vector& cameras, const std::vector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph(cameras, measurements, - Symbol('p', 0), initialEstimate); + boost::tie(graph, values) = triangulationGraph // + (cameras, measurements, Symbol('p', 0), initialEstimate); return optimize(graph, values, Symbol('p', 0)); } +/// PinholeCamera specific version +template +Point3 triangulateNonlinear( + const std::vector >& cameras, + const std::vector& measurements, const Point3& initialEstimate) { + return triangulateNonlinear > // + (cameras, measurements, initialEstimate); +} + +/** + * Create a 3*4 camera projection matrix from calibration and pose. + * Functor for partial application on calibration + * @param pose The camera pose + * @param cal The calibration + * @return Returns a Matrix34 + */ +template +struct CameraProjectionMatrix { + CameraProjectionMatrix(const CALIBRATION& calibration) : + K_(calibration.K()) { + } + Matrix34 operator()(const Pose3& pose) const { + return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); + } +private: + const Matrix3 K_; +}; + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -188,23 +236,24 @@ Point3 triangulatePoint3(const std::vector& poses, // construct projection matrices from poses & calibration std::vector projection_matrices; - BOOST_FOREACH(const Pose3& pose, poses) { - projection_matrices.push_back( - sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0)); - } + CameraProjectionMatrix createP(*sharedCal); // partially apply + BOOST_FOREACH(const Pose3& pose, poses) + projection_matrices.push_back(createP(pose)); + // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - // The n refine using non-linear optimization + // Then refine using non-linear optimization if (optimize) - point = triangulateNonlinear(poses, sharedCal, measurements, point); + point = triangulateNonlinear // + (poses, sharedCal, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies infront of all cameras + // verify that the triangulated point lies in front of all cameras BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -223,42 +272,196 @@ Point3 triangulatePoint3(const std::vector& poses, * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ -template +template Point3 triangulatePoint3( - const std::vector >& cameras, + const std::vector& cameras, const std::vector& measurements, double rank_tol = 1e-9, bool optimize = false) { size_t m = cameras.size(); - assert(measurements.size()==m); + assert(measurements.size() == m); if (m < 2) throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - typedef PinholeCamera Camera; std::vector projection_matrices; - BOOST_FOREACH(const Camera& camera, cameras) { - Matrix P_ = (camera.pose().inverse().matrix()); - projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) ); - } + BOOST_FOREACH(const CAMERA& camera, cameras) + projection_matrices.push_back( + CameraProjectionMatrix(camera.calibration())( + camera.pose())); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization if (optimize) - point = triangulateNonlinear(cameras, measurements, point); + point = triangulateNonlinear(cameras, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies infront of all cameras - BOOST_FOREACH(const Camera& camera, cameras) { + // verify that the triangulated point lies in front of all cameras + BOOST_FOREACH(const CAMERA& camera, cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif return point; } +/// Pinhole-specific version +template +Point3 triangulatePoint3( + const std::vector >& cameras, + const std::vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { + return triangulatePoint3 > // + (cameras, measurements, rank_tol, optimize); +} + +struct TriangulationParameters { + + double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + bool enableEPI; ///< if set to true, will refine triangulation using LM + + /** + * if the landmark is triangulated at distance larger than this, + * result is flagged as degenerate. + */ + double landmarkDistanceThreshold; // + + /** + * If this is nonnegative the we will check if the average reprojection error + * is smaller than this threshold after triangulation, otherwise result is + * flagged as degenerate. + */ + double dynamicOutlierRejectionThreshold; + + /** + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param enableEPI if true refine triangulation with embedded LM iterations + * @param landmarkDistanceThreshold flag as degenerate if point further than this + * @param dynamicOutlierRejectionThreshold or if average error larger than this + * + */ + TriangulationParameters(const double _rankTolerance = 1.0, + const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, + double _dynamicOutlierRejectionThreshold = -1) : + rankTolerance(_rankTolerance), enableEPI(_enableEPI), // + landmarkDistanceThreshold(_landmarkDistanceThreshold), // + dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) { + } + + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationParameters& p) { + os << "rankTolerance = " << p.rankTolerance << std::endl; + os << "enableEPI = " << p.enableEPI << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold + << std::endl; + os << "dynamicOutlierRejectionThreshold = " + << p.dynamicOutlierRejectionThreshold << std::endl; + return os; + } +}; + +/** + * TriangulationResult is an optional point, along with the reasons why it is invalid. + */ +class TriangulationResult: public boost::optional { + enum Status { + VALID, DEGENERATE, BEHIND_CAMERA + }; + Status status_; + TriangulationResult(Status s) : + status_(s) { + } +public: + TriangulationResult(const Point3& p) : + status_(VALID) { + reset(p); + } + static TriangulationResult Degenerate() { + return TriangulationResult(DEGENERATE); + } + static TriangulationResult BehindCamera() { + return TriangulationResult(BEHIND_CAMERA); + } + bool degenerate() const { + return status_ == DEGENERATE; + } + bool behindCamera() const { + return status_ == BEHIND_CAMERA; + } + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationResult& result) { + if (result) + os << "point = " << *result << std::endl; + else + os << "no point, status = " << result.status_ << std::endl; + return os; + } +}; + +/// triangulateSafe: extensive checking of the outcome +template +TriangulationResult triangulateSafe(const std::vector& cameras, + const std::vector& measured, + const TriangulationParameters& params) { + + size_t m = cameras.size(); + + // if we have a single pose the corresponding factor is uninformative + if (m < 2) + return TriangulationResult::Degenerate(); + else + // We triangulate the 3D position of the landmark + try { + Point3 point = triangulatePoint3(cameras, measured, + params.rankTolerance, params.enableEPI); + + // Check landmark distance and re-projection errors to avoid outliers + size_t i = 0; + double totalReprojError = 0.0; + BOOST_FOREACH(const CAMERA& camera, cameras) { + const Pose3& pose = camera.pose(); + if (params.landmarkDistanceThreshold > 0 + && pose.translation().distance(point) + > params.landmarkDistanceThreshold) + return TriangulationResult::Degenerate(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies in front of all cameras + // Only needed if this was not yet handled by exception + const Point3& p_local = pose.transform_to(point); + if (p_local.z() <= 0) + return TriangulationResult::BehindCamera(); +#endif + // Check reprojection error + if (params.dynamicOutlierRejectionThreshold > 0) { + const Point2& zi = measured.at(i); + Point2 reprojectionError(camera.project(point) - zi); + totalReprojError += reprojectionError.vector().norm(); + } + i += 1; + } + // Flag as degenerate if average reprojection error is too large + if (params.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Degenerate(); + + // all good! + return TriangulationResult(point); + } catch (TriangulationUnderconstrainedException&) { + // This exception is thrown if + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + return TriangulationResult::Degenerate(); + } catch (TriangulationCheiralityException&) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + return TriangulationResult::BehindCamera(); + } +} + } // \namespace gtsam diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 281648409..3a3e1317c 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -344,7 +344,7 @@ namespace gtsam { gttic(Full_root_factoring); boost::shared_ptr p_C1_B; { FastVector C1_minus_B; { - FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); + KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); BOOST_FOREACH(const Key j, *B->conditional()) { C1_minus_B_set.erase(j); } C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); @@ -356,7 +356,7 @@ namespace gtsam { } boost::shared_ptr p_C2_B; { FastVector C2_minus_B; { - FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); + KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); BOOST_FOREACH(const Key j, *B->conditional()) { C2_minus_B_set.erase(j); } C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 830ddd3ec..4d68acb5b 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -19,13 +19,13 @@ #pragma once -#include - -#include +#include #include #include #include +#include + namespace gtsam { // Forward declarations @@ -253,7 +253,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(nodes_); ar & BOOST_SERIALIZATION_NVP(roots_); } diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 274886c21..256ff983d 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -44,8 +44,8 @@ namespace gtsam { FastVector BayesTreeCliqueBase::separator_setminus_B(const derived_ptr& B) const { - FastSet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); - FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); + KeySet indicesB(B->conditional()->begin(), B->conditional()->end()); FastVector S_setminus_B; std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); @@ -58,8 +58,8 @@ namespace gtsam { const derived_ptr& B, const FactorGraphType& p_Cp_B) const { gttic(shortcut_indices); - FastSet allKeys = p_Cp_B.keys(); - FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + KeySet allKeys = p_Cp_B.keys(); + KeySet indicesB(B->conditional()->begin(), B->conditional()->end()); FastVector S_setminus_B = separator_setminus_B(B); FastVector keep; // keep = S\B intersect allKeys (S_setminus_B is already sorted) diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index beb222178..eae886785 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -160,7 +160,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(conditional_); ar & BOOST_SERIALIZATION_NVP(parent_); ar & BOOST_SERIALIZATION_NVP(children); diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 13130bf2e..6b28f2dbf 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -7,180 +7,233 @@ * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree */ -#include -#include #include #include #include +#include +#include #include #include -namespace gtsam -{ - namespace - { - /* ************************************************************************* */ - // Elimination traversal data - stores a pointer to the parent data and collects the factors - // resulting from elimination of the children. Also sets up BayesTree cliques with parent and - // child pointers. - template - struct EliminationData { - EliminationData* const parentData; - size_t myIndexInParent; - FastVector childFactors; - boost::shared_ptr bayesTreeNode; - EliminationData(EliminationData* _parentData, size_t nChildren) : - parentData(_parentData), - bayesTreeNode(boost::make_shared()) - { - if(parentData) { - myIndexInParent = parentData->childFactors.size(); - parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor()); - } else { - myIndexInParent = 0; - } - // Set up BayesTree parent and child pointers - if(parentData) { - if(parentData->parentData) // If our parent is not the dummy node - bayesTreeNode->parent_ = parentData->bayesTreeNode; - parentData->bayesTreeNode->children.push_back(bayesTreeNode); - } - } - }; +namespace gtsam { - /* ************************************************************************* */ - // Elimination pre-order visitor - just creates the EliminationData structure for the visited - // node. - template - EliminationData eliminationPreOrderVisitor( - const typename CLUSTERTREE::sharedNode& node, EliminationData& parentData) - { - EliminationData myData(&parentData, node->children.size()); - myData.bayesTreeNode->problemSize_ = node->problemSize(); - return myData; +/* ************************************************************************* */ +// Elimination traversal data - stores a pointer to the parent data and collects +// the factors resulting from elimination of the children. Also sets up BayesTree +// cliques with parent and child pointers. +template +struct EliminationData { + // Typedefs + typedef typename CLUSTERTREE::sharedFactor sharedFactor; + typedef typename CLUSTERTREE::FactorType FactorType; + typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; + typedef typename CLUSTERTREE::ConditionalType ConditionalType; + typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; + + EliminationData* const parentData; + size_t myIndexInParent; + FastVector childFactors; + boost::shared_ptr bayesTreeNode; + EliminationData(EliminationData* _parentData, size_t nChildren) : + parentData(_parentData), bayesTreeNode(boost::make_shared()) { + if (parentData) { + myIndexInParent = parentData->childFactors.size(); + parentData->childFactors.push_back(sharedFactor()); + } else { + myIndexInParent = 0; } + // Set up BayesTree parent and child pointers + if (parentData) { + if (parentData->parentData) // If our parent is not the dummy node + bayesTreeNode->parent_ = parentData->bayesTreeNode; + parentData->bayesTreeNode->children.push_back(bayesTreeNode); + } + } - /* ************************************************************************* */ - // Elimination post-order visitor - combine the child factors with our own factors, add the - // resulting conditional to the BayesTree, and add the remaining factor to the parent. - template - struct EliminationPostOrderVisitor - { - const typename CLUSTERTREE::Eliminate& eliminationFunction; - typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; - EliminationPostOrderVisitor(const typename CLUSTERTREE::Eliminate& eliminationFunction, + // Elimination pre-order visitor - creates the EliminationData structure for the visited node. + static EliminationData EliminationPreOrderVisitor( + const typename CLUSTERTREE::sharedNode& node, + EliminationData& parentData) { + assert(node); + EliminationData myData(&parentData, node->children.size()); + myData.bayesTreeNode->problemSize_ = node->problemSize(); + return myData; + } + + // Elimination post-order visitor - combine the child factors with our own factors, add the + // resulting conditional to the BayesTree, and add the remaining factor to the parent. + class EliminationPostOrderVisitor { + const typename CLUSTERTREE::Eliminate& eliminationFunction_; + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_; + + public: + // Construct functor + EliminationPostOrderVisitor( + const typename CLUSTERTREE::Eliminate& eliminationFunction, typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : - eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {} - void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) - { - // Typedefs - typedef typename CLUSTERTREE::sharedFactor sharedFactor; - typedef typename CLUSTERTREE::FactorType FactorType; - typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; - typedef typename CLUSTERTREE::ConditionalType ConditionalType; - typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; - - // Gather factors - FactorGraphType gatheredFactors; - gatheredFactors.reserve(node->factors.size() + node->children.size()); - gatheredFactors += node->factors; - gatheredFactors += myData.childFactors; - - // Check for Bayes tree orphan subtrees, and add them to our children - BOOST_FOREACH(const sharedFactor& f, node->factors) - { - if(const BayesTreeOrphanWrapper* asSubtree = dynamic_cast*>(f.get())) - { - myData.bayesTreeNode->children.push_back(asSubtree->clique); - asSubtree->clique->parent_ = myData.bayesTreeNode; - } - } - - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, Ordering(node->keys)); - - // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor - myData.bayesTreeNode->setEliminationResult(eliminationResult); - - // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid - // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 - // object they're added to. - BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) - nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); - - // Store remaining factor in parent's gathered factors - if(!eliminationResult.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; - } - }; - } - - /* ************************************************************************* */ - template - void ClusterTree::Cluster::print( - const std::string& s, const KeyFormatter& keyFormatter) const - { - std::cout << s; - BOOST_FOREACH(Key j, keys) - std::cout << j << " "; - std::cout << "problemSize = " << problemSize_ << std::endl; - } - - /* ************************************************************************* */ - template - void ClusterTree::print( - const std::string& s, const KeyFormatter& keyFormatter) const - { - treeTraversal::PrintForest(*this, s, keyFormatter); - } - - /* ************************************************************************* */ - template - ClusterTree& ClusterTree::operator=(const This& other) - { - // Start by duplicating the tree. - roots_ = treeTraversal::CloneForest(other); - - // Assign the remaining factors - these are pointers to factors in the original factor graph and - // we do not clone them. - remainingFactors_ = other.remainingFactors_; - - return *this; - } - - /* ************************************************************************* */ - template - std::pair, boost::shared_ptr > - ClusterTree::eliminate(const Eliminate& function) const - { - gttic(ClusterTree_eliminate); - // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node - // that contains all of the roots as its children. rootsContainer also stores the remaining - // uneliminated factors passed up from the roots. - boost::shared_ptr result = boost::make_shared(); - EliminationData rootsContainer(0, roots_.size()); - EliminationPostOrderVisitor visitorPost(function, result->nodes_); - { - TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP - treeTraversal::DepthFirstForestParallel(*this, rootsContainer, - eliminationPreOrderVisitor, visitorPost, 10); + eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) { } - // Create BayesTree from roots stored in the dummy BayesTree node. - result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end()); + // Function that does the HEAVY lifting + void operator()(const typename CLUSTERTREE::sharedNode& node, + EliminationData& myData) { + assert(node); - // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr allRemainingFactors = boost::make_shared(); - allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size()); - allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); - BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) - if(factor) - allRemainingFactors->push_back(factor); + // Gather factors + FactorGraphType gatheredFactors; + gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors += node->factors; + gatheredFactors += myData.childFactors; - // Return result - return std::make_pair(result, allRemainingFactors); + // Check for Bayes tree orphan subtrees, and add them to our children + BOOST_FOREACH(const sharedFactor& f, node->factors) { + if (const BayesTreeOrphanWrapper* asSubtree = + dynamic_cast*>(f.get())) { + myData.bayesTreeNode->children.push_back(asSubtree->clique); + asSubtree->clique->parent_ = myData.bayesTreeNode; + } + } + + // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>> + std::pair, + boost::shared_ptr > eliminationResult = + eliminationFunction_(gatheredFactors, node->orderedFrontalKeys); + // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< + + // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor + myData.bayesTreeNode->setEliminationResult(eliminationResult); + + // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid + // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 + // object they're added to. + BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) + nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode)); + + // Store remaining factor in parent's gathered factors + if (!eliminationResult.second->empty()) + myData.parentData->childFactors[myData.myIndexInParent] = + eliminationResult.second; + } + }; +}; + +/* ************************************************************************* */ +template +void ClusterTree::Cluster::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << " (" << problemSize_ << ")"; + PrintKeyVector(orderedFrontalKeys); +} + +/* ************************************************************************* */ +template +void ClusterTree::Cluster::mergeChildren( + const std::vector& merge) { + gttic(Cluster_mergeChildren); + + // Count how many keys, factors and children we'll end up with + size_t nrKeys = orderedFrontalKeys.size(); + size_t nrFactors = factors.size(); + size_t nrNewChildren = 0; + // Loop over children + size_t i = 0; + BOOST_FOREACH(const sharedNode& child, children) { + if (merge[i]) { + nrKeys += child->orderedFrontalKeys.size(); + nrFactors += child->factors.size(); + nrNewChildren += child->children.size(); + } else { + nrNewChildren += 1; // we keep the child + } + ++i; } + // now reserve space, and really merge + orderedFrontalKeys.reserve(nrKeys); + factors.reserve(nrFactors); + typename Node::Children newChildren; + newChildren.reserve(nrNewChildren); + i = 0; + BOOST_FOREACH(const sharedNode& child, children) { + if (merge[i]) { + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + orderedFrontalKeys.insert(orderedFrontalKeys.end(), + child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend()); + // Merge keys, factors, and children. + factors.insert(factors.end(), child->factors.begin(), + child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), + child->children.end()); + // Increment problem size + problemSize_ = std::max(problemSize_, child->problemSize_); + // Increment number of frontal variables + } else { + newChildren.push_back(child); // we keep the child + } + ++i; + } + children = newChildren; + std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); +} + +/* ************************************************************************* */ +template +void ClusterTree::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + treeTraversal::PrintForest(*this, s, keyFormatter); +} + +/* ************************************************************************* */ +template +ClusterTree& ClusterTree::operator=( + const This& other) { + // Start by duplicating the tree. + roots_ = treeTraversal::CloneForest(other); + + // Assign the remaining factors - these are pointers to factors in the original factor graph and + // we do not clone them. + remainingFactors_ = other.remainingFactors_; + + return *this; +} + +/* ************************************************************************* */ +template +std::pair, boost::shared_ptr > ClusterTree< + BAYESTREE, GRAPH>::eliminate(const Eliminate& function) const { + gttic(ClusterTree_eliminate); + // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node + // that contains all of the roots as its children. rootsContainer also stores the remaining + // uneliminated factors passed up from the roots. + boost::shared_ptr result = boost::make_shared(); + typedef EliminationData Data; + Data rootsContainer(0, roots_.size()); + typename Data::EliminationPostOrderVisitor visitorPost(function, + result->nodes_); + { + TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + treeTraversal::DepthFirstForestParallel(*this, rootsContainer, + Data::EliminationPreOrderVisitor, visitorPost, 10); + } + + // Create BayesTree from roots stored in the dummy BayesTree node. + result->roots_.insert(result->roots_.end(), + rootsContainer.bayesTreeNode->children.begin(), + rootsContainer.bayesTreeNode->children.end()); + + // Add remaining factors that were not involved with eliminated variables + boost::shared_ptr remaining = boost::make_shared< + FactorGraphType>(); + remaining->reserve( + remainingFactors_.size() + rootsContainer.childFactors.size()); + remaining->push_back(remainingFactors_.begin(), remainingFactors_.end()); + BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) { + if (factor) + remaining->push_back(factor); + } + // Return result + return std::make_pair(result, remaining); +} + } diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 5a412a79e..b00532d22 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -11,115 +11,124 @@ #include #include -#include +#include -namespace gtsam -{ +namespace gtsam { - /** - * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: - * each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that - * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. - * \nosubgrouping - */ - template - class ClusterTree - { - public: - typedef GRAPH FactorGraphType; ///< The factor graph type - typedef typename GRAPH::FactorType FactorType; ///< The type of factors - typedef ClusterTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination - typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals - typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional - typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine +/** + * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: + * each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that + * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. + * \nosubgrouping + */ +template +class ClusterTree { +public: + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef ClusterTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination + typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals + typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine - struct Cluster { - typedef FastVector Keys; - typedef FastVector Factors; - typedef FastVector > Children; + struct Cluster { + typedef Ordering Keys; + typedef FastVector Factors; + typedef FastVector > Children; - Keys keys; ///< Frontal keys of this node - Factors factors; ///< Factors associated with this node - Children children; ///< sub-trees - int problemSize_; + Cluster() { + } + Cluster(Key key, const Factors& factors) : + factors(factors) { + orderedFrontalKeys.push_back(key); + } - int problemSize() const { return problemSize_; } + Keys orderedFrontalKeys; ///< Frontal keys of this node + Factors factors; ///< Factors associated with this node + Children children; ///< sub-trees + int problemSize_; - /** print this node */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - }; + int problemSize() const { + return problemSize_; + } - typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster - typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions - typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions - - /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - - protected: - FastVector roots_; - FastVector remainingFactors_; - - /// @name Standard Constructors - /// @{ - - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - ClusterTree(const This& other) { *this = other; } - - /// @} - - public: - /// @name Testable - /// @{ - - /** Print the cluster tree */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// @} - - /// @name Standard Interface - /// @{ - - /** Eliminate the factors to a Bayes tree and remaining factor graph - * @param function The function to use to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The Bayes tree and factor graph resulting from elimination - */ - std::pair, boost::shared_ptr > - eliminate(const Eliminate& function) const; - - /// @} - - /// @name Advanced Interface - /// @{ - - /** Return the set of roots (one for a tree, multiple for a forest) */ - const FastVector& roots() const { return roots_; } - - /** Return the remaining factors that are not pulled into elimination */ - const FastVector& remainingFactors() const { return remainingFactors_; } - - /// @} - - protected: - /// @name Details - - /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors - /// are copied, factors are not cloned. - This& operator=(const This& other); - - /// Default constructor to be used in derived classes - ClusterTree() {} - - /// @} + /// print this node + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + /// Merge all children for which bit is set into this node + void mergeChildren(const std::vector& merge); }; + typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster + typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions + typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions + + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + +protected: + FastVector roots_; + FastVector remainingFactors_; + + /// @name Standard Constructors + /// @{ + + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + ClusterTree(const This& other) {*this = other;} + + /// @} + +public: + /// @name Testable + /// @{ + + /** Print the cluster tree */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + + /// @name Standard Interface + /// @{ + + /** Eliminate the factors to a Bayes tree and remaining factor graph + * @param function The function to use to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The Bayes tree and factor graph resulting from elimination + */ + std::pair, boost::shared_ptr > + eliminate(const Eliminate& function) const; + + /// @} + + /// @name Advanced Interface + /// @{ + + /** Return the set of roots (one for a tree, multiple for a forest) */ + const FastVector& roots() const {return roots_;} + + /** Return the remaining factors that are not pulled into elimination */ + const FastVector& remainingFactors() const {return remainingFactors_;} + + /// @} + +protected: + /// @name Details + + /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors + /// are copied, factors are not cloned. + This& operator=(const This& other); + + /// Default constructor to be used in derived classes + ClusterTree() {} + + /// @} + +}; } - diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index bdfec9cd5..39c738a19 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -141,7 +141,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(nrFrontals_); } diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 70b0dd393..68e623b68 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -101,10 +101,12 @@ namespace gtsam { { // Retrieve the factors involving this variable and create the current node const VariableIndex::Factors& factors = structure[order[j]]; - nodes[j] = boost::make_shared(); - nodes[j]->key = order[j]; + const sharedNode node = boost::make_shared(); + node->key = order[j]; // for row i \in Struct[A*j] do + node->children.reserve(factors.size()); + node->factors.reserve(factors.size()); BOOST_FOREACH(const size_t i, factors) { // If we already hit a variable in this factor, make the subtree containing the previous // variable in this factor a child of the current node. This means that the variables @@ -123,16 +125,16 @@ namespace gtsam { if (r != j) { // Now that we found the root, hook up parent and child pointers in the nodes. parents[r] = j; - nodes[j]->children.push_back(nodes[r]); + node->children.push_back(nodes[r]); } } else { - // Add the current factor to the current node since we are at the first variable in this - // factor. - nodes[j]->factors.push_back(graph[i]); + // Add the factor to the current node since we are at the first variable in this factor. + node->factors.push_back(graph[i]); factorUsed[i] = true; } prevCol[i] = j; } + nodes[j] = node; } } catch(std::invalid_argument& e) { // If this is thrown from structure[order[j]] above, it means that it was requested to diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index d66f6b8ac..3e41d7692 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -160,7 +160,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(keys_); } diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 8c19d4ff4..c629d336a 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -28,6 +28,7 @@ #include #include +#include // for cout :-( namespace gtsam { @@ -72,8 +73,8 @@ namespace gtsam { /* ************************************************************************* */ template - FastSet FactorGraph::keys() const { - FastSet allKeys; + KeySet FactorGraph::keys() const { + KeySet allKeys; BOOST_FOREACH(const sharedFactor& factor, factors_) if (factor) allKeys.insert(factor->begin(), factor->end()); diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index adb1c0aad..e97860eaa 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -332,7 +332,7 @@ namespace gtsam { size_t nrFactors() const; /** Potentially very slow function to return all keys involved */ - FastSet keys() const; + KeySet keys() 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); } @@ -342,7 +342,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(factors_); } diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index aebd3325a..99f668591 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -29,7 +29,7 @@ namespace gtsam { // Remove the contaminated part of the Bayes tree BayesNetType bn; if (!this->empty()) { - const FastSet newFactorKeys = newFactors.keys(); + const KeySet newFactorKeys = newFactors.keys(); this->removeTop(std::vector(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans); } @@ -44,7 +44,7 @@ namespace gtsam { // eliminate into a Bayes net const VariableIndex varIndex(factors); - const FastSet newFactorKeys = newFactors.keys(); + const KeySet newFactorKeys = newFactors.keys(); const Ordering constrainedOrdering = Ordering::ColamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 9d96c5b9c..352a8dded 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-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) @@ -26,123 +26,132 @@ #include namespace gtsam { - - namespace { - /* ************************************************************************* */ - template - struct ConstructorTraversalData { - ConstructorTraversalData* const parentData; - typename JunctionTree::sharedNode myJTNode; - FastVector childSymbolicConditionals; - FastVector childSymbolicFactors; - ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {} - }; - /* ************************************************************************* */ - // Pre-order visitor function - template - ConstructorTraversalData ConstructorTraversalVisitorPre( +template +struct ConstructorTraversalData { + typedef typename JunctionTree::Node Node; + typedef typename JunctionTree::sharedNode sharedNode; + + ConstructorTraversalData* const parentData; + sharedNode myJTNode; + FastVector childSymbolicConditionals; + FastVector childSymbolicFactors; + + // Small inner class to store symbolic factors + class SymbolicFactors: public FactorGraph { + }; + + ConstructorTraversalData(ConstructorTraversalData* _parentData) : + parentData(_parentData) { + } + + // Pre-order visitor function + static ConstructorTraversalData ConstructorTraversalVisitorPre( const boost::shared_ptr& node, - ConstructorTraversalData& parentData) - { - // On the pre-order pass, before children have been visited, we just set up a traversal data - // structure with its own JT node, and create a child pointer in its parent. - ConstructorTraversalData myData = ConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared::Node>(); - myData.myJTNode->keys.push_back(node->key); - myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end()); - parentData.myJTNode->children.push_back(myData.myJTNode); - return myData; - } + ConstructorTraversalData& parentData) { + // On the pre-order pass, before children have been visited, we just set up + // a traversal data structure with its own JT node, and create a child + // pointer in its parent. + ConstructorTraversalData myData = ConstructorTraversalData(&parentData); + myData.myJTNode = boost::make_shared(node->key, node->factors); + parentData.myJTNode->children.push_back(myData.myJTNode); + return myData; + } - /* ************************************************************************* */ - // Post-order visitor function - template - void ConstructorTraversalVisitorPostAlg2( + // Post-order visitor function + static void ConstructorTraversalVisitorPostAlg2( const boost::shared_ptr& ETreeNode, - const ConstructorTraversalData& myData) - { - // In this post-order visitor, we combine the symbolic elimination results from the - // elimination tree children and symbolically eliminate the current elimination tree node. We - // then check whether each of our elimination tree child nodes should be merged with us. The - // check for this is that our number of symbolic elimination parents is exactly 1 less than - // our child's symbolic elimination parents - this condition indicates that eliminating the - // current node did not introduce any parents beyond those already in the child. + const ConstructorTraversalData& myData) { + // In this post-order visitor, we combine the symbolic elimination results + // from the elimination tree children and symbolically eliminate the current + // elimination tree node. We then check whether each of our elimination + // tree child nodes should be merged with us. The check for this is that + // our number of symbolic elimination parents is exactly 1 less than + // our child's symbolic elimination parents - this condition indicates that + // eliminating the current node did not introduce any parents beyond those + // already in the child-> - // Do symbolic elimination for this node - class : public FactorGraph {} symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); - // Add ETree node factors - symbolicFactors += ETreeNode->factors; - // Add symbolic factors passed up from children - symbolicFactors += myData.childSymbolicFactors; + // Do symbolic elimination for this node + SymbolicFactors symbolicFactors; + symbolicFactors.reserve( + ETreeNode->factors.size() + myData.childSymbolicFactors.size()); + // Add ETree node factors + symbolicFactors += ETreeNode->factors; + // Add symbolic factors passed up from children + symbolicFactors += myData.childSymbolicFactors; - Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); - std::pair symbolicElimResult = - internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); + Ordering keyAsOrdering; + keyAsOrdering.push_back(ETreeNode->key); + SymbolicConditional::shared_ptr myConditional; + SymbolicFactor::shared_ptr mySeparatorFactor; + boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic( + symbolicFactors, keyAsOrdering); - // Store symbolic elimination results in the parent - myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first); - myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second); + // Store symbolic elimination results in the parent + myData.parentData->childSymbolicConditionals.push_back(myConditional); + myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); - // Merge our children if they are in our clique - if our conditional has exactly one fewer - // parent than our child's conditional. - size_t myNrFrontals = 1; - const size_t myNrParents = symbolicElimResult.first->nrParents(); - size_t nrMergedChildren = 0; - assert(myData.myJTNode->children.size() == myData.childSymbolicConditionals.size()); - // Loop over children - int combinedProblemSize = (int) (symbolicElimResult.first->size() * symbolicFactors.size()); - for(size_t child = 0; child < myData.childSymbolicConditionals.size(); ++child) { - // Check if we should merge the child - if(myNrParents + myNrFrontals == myData.childSymbolicConditionals[child]->nrParents()) { - // Get a reference to the child, adjusting the index to account for children previously - // merged and removed from the child list. - const typename JunctionTree::Node& childToMerge = - *myData.myJTNode->children[child - nrMergedChildren]; - // Merge keys, factors, and children. - myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end()); - myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end()); - myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end()); - // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_); - // Increment number of frontal variables - myNrFrontals += childToMerge.keys.size(); - // Remove child from list. - myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren)); - // Increment number of merged children - ++ nrMergedChildren; - } + sharedNode node = myData.myJTNode; + const FastVector& childConditionals = + myData.childSymbolicConditionals; + node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size()); + + // Merge our children if they are in our clique - if our conditional has + // exactly one fewer parent than our child's conditional. + const size_t myNrParents = myConditional->nrParents(); + const size_t nrChildren = node->children.size(); + assert(childConditionals.size() == nrChildren); + + // decide which children to merge, as index into children + std::vector merge(nrChildren, false); + size_t myNrFrontals = 1, i = 0; + BOOST_FOREACH(const sharedNode& child, node->children) { + // Check if we should merge the i^th child + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + // Increment number of frontal variables + myNrFrontals += child->orderedFrontalKeys.size(); + merge[i] = true; } - myData.myJTNode->problemSize_ = combinedProblemSize; + ++i; } + + // now really merge + node->mergeChildren(merge); } +}; - /* ************************************************************************* */ - template - template - JunctionTree::JunctionTree(const EliminationTree& eliminationTree) - { - gttic(JunctionTree_FromEliminationTree); - // Here we rely on the BayesNet having been produced by this elimination tree, such that the - // conditionals are arranged in DFS post-order. We traverse the elimination tree, and inspect - // the symbolic conditional corresponding to each node. The elimination tree node is added to - // the same clique with its parent if it has exactly one more Bayes net conditional parent than - // does its elimination tree parent. +/* ************************************************************************* */ +template +template +JunctionTree::JunctionTree( + const EliminationTree& eliminationTree) { + gttic(JunctionTree_FromEliminationTree); + // Here we rely on the BayesNet having been produced by this elimination tree, + // such that the conditionals are arranged in DFS post-order. We traverse the + // elimination tree, and inspect the symbolic conditional corresponding to + // each node. The elimination tree node is added to the same clique with its + // parent if it has exactly one more Bayes net conditional parent than + // does its elimination tree parent. - // Traverse the elimination tree, doing symbolic elimination and merging nodes as we go. Gather - // the created junction tree roots in a dummy Node. - typedef typename EliminationTree::Node ETreeNode; - ConstructorTraversalData rootData(0); - rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather the junction tree roots - treeTraversal::DepthFirstForest(eliminationTree, rootData, - ConstructorTraversalVisitorPre, ConstructorTraversalVisitorPostAlg2); + // Traverse the elimination tree, doing symbolic elimination and merging nodes + // as we go. Gather the created junction tree roots in a dummy Node. + typedef typename EliminationTree::Node ETreeNode; + typedef ConstructorTraversalData Data; + Data rootData(0); + rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather + // the junction tree roots + treeTraversal::DepthFirstForest(eliminationTree, rootData, + Data::ConstructorTraversalVisitorPre, + Data::ConstructorTraversalVisitorPostAlg2); - // Assign roots from the dummy node - Base::roots_ = rootData.myJTNode->children; + // Assign roots from the dummy node + typedef typename JunctionTree::Node Node; + const typename Node::Children& children = rootData.myJTNode->children; + Base::roots_.reserve(children.size()); + Base::roots_.insert(Base::roots_.begin(), children.begin(), children.end()); - // Transfer remaining factors from elimination tree - Base::remainingFactors_ = eliminationTree.remainingFactors(); - } + // Transfer remaining factors from elimination tree + Base::remainingFactors_ = eliminationTree.remainingFactors(); +} -} //namespace gtsam +} // namespace gtsam diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index 0b9be2f1c..a2a6906d1 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -17,57 +17,72 @@ * @date Feb 20, 2012 */ -#include - -#include -#include - #include #include +#include +#include +#include + +using namespace std; + namespace gtsam { /* ************************************************************************* */ -std::string _multirobotKeyFormatter(Key key) { +string _defaultKeyFormatter(Key key) { + const Symbol asSymbol(key); + if (asSymbol.chr() > 0) + return (string) asSymbol; + else + return boost::lexical_cast(key); +} + +/* ************************************************************************* */ +void PrintKey(Key key, const string& s, const KeyFormatter& keyFormatter) { + cout << s << keyFormatter(key); +} + +/* ************************************************************************* */ +string _multirobotKeyFormatter(Key key) { const LabeledSymbol asLabeledSymbol(key); if (asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) - return (std::string) asLabeledSymbol; + return (string) asLabeledSymbol; const Symbol asSymbol(key); if (asLabeledSymbol.chr() > 0) - return (std::string) asSymbol; + return (string) asSymbol; else - return boost::lexical_cast(key); + return boost::lexical_cast(key); } /* ************************************************************************* */ template -static void print(const CONTAINER& keys, const std::string& s, +static void Print(const CONTAINER& keys, const string& s, const KeyFormatter& keyFormatter) { - std::cout << s << " "; + cout << s << " "; if (keys.empty()) - std::cout << "(none)" << std::endl; + cout << "(none)" << endl; else { BOOST_FOREACH(const Key& key, keys) - std::cout << keyFormatter(key) << " "; - std::cout << std::endl; + cout << keyFormatter(key) << " "; + cout << endl; } } /* ************************************************************************* */ -void printKeyList(const KeyList& keys, const std::string& s, +void PrintKeyList(const KeyList& keys, const string& s, const KeyFormatter& keyFormatter) { - print(keys, s, keyFormatter); + Print(keys, s, keyFormatter); } /* ************************************************************************* */ -void printKeyVector(const KeyVector& keys, const std::string& s, +void PrintKeyVector(const KeyVector& keys, const string& s, const KeyFormatter& keyFormatter) { - print(keys, s, keyFormatter); + Print(keys, s, keyFormatter); } /* ************************************************************************* */ -void printKeySet(const KeySet& keys, const std::string& s, +void PrintKeySet(const KeySet& keys, const string& s, const KeyFormatter& keyFormatter) { - print(keys, s, keyFormatter); + Print(keys, s, keyFormatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index e2be79dc7..d2b342575 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -17,45 +17,75 @@ */ #pragma once -#include -#include - -#include -#include #include -#include #include +#include +#include +#include +#include +#include + +#include namespace gtsam { +/// Typedef for a function to format a key, i.e. to convert it to a string +typedef boost::function KeyFormatter; - // Helper function for Multi-robot Key Formatter - GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); +// Helper function for DefaultKeyFormatter +GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - /// - /// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain - /// integer keys. This keyformatter will need to be passed in to override the default - /// formatter in print functions. - /// - /// Checks for LabeledSymbol, Symbol and then plain keys, in order. - static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter; +/// The default KeyFormatter, which is used if no KeyFormatter is passed to +/// a nonlinear 'print' function. Automatically detects plain integer keys +/// and Symbol keys. +static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; - /// Useful typedefs for operations with Values - allow for matlab interfaces - typedef FastList KeyList; - typedef FastVector KeyVector; - typedef FastSet KeySet; - typedef FastMap KeyGroupMap; +// Helper function for Multi-robot Key Formatter +GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); - /// Utility function to print sets of keys with optional prefix - GTSAM_EXPORT void printKeyList(const KeyList& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +/// +/// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain +/// integer keys. This keyformatter will need to be passed in to override the default +/// formatter in print functions. +/// +/// Checks for LabeledSymbol, Symbol and then plain keys, in order. +static const gtsam::KeyFormatter MultiRobotKeyFormatter = + &_multirobotKeyFormatter; - /// Utility function to print sets of keys with optional prefix - GTSAM_EXPORT void printKeyVector(const KeyVector& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +/// Useful typedef for operations with Values - allows for matlab interface +typedef FastVector KeyVector; - /// Utility function to print sets of keys with optional prefix - GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); -} +// TODO(frank): Nothing fast about these :-( +typedef FastList KeyList; +typedef FastSet KeySet; +typedef FastMap KeyGroupMap; + +/// Utility function to print one key with optional prefix +GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Utility function to print sets of keys with optional prefix +GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Utility function to print sets of keys with optional prefix +GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s = + "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Utility function to print sets of keys with optional prefix +GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +// 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); + } + static bool Equals(const Key& key1, const Key& key2, double tol = 1e-8) { + return key1 == key2; + } +}; + +} // namespace gtsam diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 23936afed..452c98434 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -109,7 +109,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(label_); ar & BOOST_SERIALIZATION_NVP(j_); diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 7299d7c8a..06e5ddeec 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -26,8 +26,8 @@ namespace gtsam { /* ************************************************************************* */ template void MetisIndex::augment(const FactorGraph& factors) { - std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first - std::map >::iterator iAdjMapIt; + std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first + std::map >::iterator iAdjMapIt; std::set keySet; /* ********** Convert to CSR format ********** */ diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 0ec5774e5..8af2649c5 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -18,15 +18,15 @@ #pragma once -#include -#include -#include - -#include #include #include #include #include +#include + +#include +#include +#include namespace gtsam { @@ -115,9 +115,9 @@ public: /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains /// the variables in \c constrainLast to the end of the ordering, and orders all other variables /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c - /// constrainLast will be ordered in the same order specified in the vector \c - /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be - /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + /// constrainFirst will be ordered in the same order specified in the vector \c + /// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be + /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template static Ordering ColamdConstrainedFirst(const FactorGraph& graph, const std::vector& constrainFirst, bool forceOrder = false) { @@ -130,7 +130,7 @@ public: /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the /// variables in \c constrainFirst will be ordered in the same order specified in the /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c - /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to + /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to /// reduce fill-in as well. static GTSAM_EXPORT Ordering ColamdConstrainedFirst( const VariableIndex& variableIndex, @@ -164,7 +164,7 @@ public: /// Return a natural Ordering. Typically used by iterative solvers template static Ordering Natural(const FactorGraph &fg) { - FastSet src = fg.keys(); + KeySet src = fg.keys(); std::vector keys(src.begin(), src.end()); std::stable_sort(keys.begin(), keys.end()); return Ordering(keys); diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 4f9734639..68d927baf 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -117,7 +117,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(j_); } diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index 82eb85c76..c00a3633e 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -18,6 +18,8 @@ #pragma once #include +#include +#include namespace gtsam { diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index bcaec6ee4..3b80f0d01 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -18,16 +18,14 @@ #pragma once #include -#include #include -#include -#include -#include +#include +#include -#include +#include +#include -#include -#include +#include #include namespace gtsam { @@ -45,7 +43,7 @@ class GTSAM_EXPORT VariableIndex { public: typedef boost::shared_ptr shared_ptr; - typedef FastList Factors; + typedef FastVector Factors; typedef Factors::iterator Factor_iterator; typedef Factors::const_iterator Factor_const_iterator; diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 1033c0cc9..b0708e660 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -14,6 +14,7 @@ * @author Alex Cunningham */ +#include #include #include #include diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h new file mode 100644 index 000000000..23d11964c --- /dev/null +++ b/gtsam/linear/BinaryJacobianFactor.h @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file BinaryJacobianFactor.h + * + * @brief A binary JacobianFactor specialization that uses fixed matrix math for speed + * + * @date June 2015 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * A binary JacobianFactor specialization that uses fixed matrix math for speed + */ +template +struct BinaryJacobianFactor: JacobianFactor { + + /// Constructor + BinaryJacobianFactor(Key key1, const Eigen::Matrix& A1, + Key key2, const Eigen::Matrix& A2, + const Eigen::Matrix& b, // + const SharedDiagonal& model = SharedDiagonal()) : + JacobianFactor(key1, A1, key2, A2, b, model) { + } + + inline Key key1() const { + return keys_[0]; + } + inline Key key2() const { + return keys_[1]; + } + + // Fixed-size matrix update + void updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_BinaryJacobianFactor); + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw std::invalid_argument( + "BinaryJacobianFactor::updateHessian: cannot update information with " + "constrained noise model"); + BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())), + key2(), model->Whiten(getA(end())), model->whiten(getb())); + whitenedFactor.updateHessian(infoKeys, info); + } else { + // First build an array of slots + DenseIndex slot1 = Slot(infoKeys, key1()); + DenseIndex slot2 = Slot(infoKeys, key2()); + DenseIndex slotB = info->nBlocks() - 1; + + const Matrix& Ab = Ab_.matrix(); + Eigen::Block A1(Ab, 0, 0); + Eigen::Block A2(Ab, 0, N1); + Eigen::Block b(Ab, 0, N1 + N2); + + // We perform I += A'*A to the upper triangle + (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose()); + (*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2; + (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; + (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); + (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; + (*info)(slotB, slotB)(0, 0) += b.transpose() * b; + } + } +}; + +template +struct traits > : Testable< + BinaryJacobianFactor > { +}; + +} //namespace gtsam diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 5e7e08f61..a1b9e2d83 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -1,8 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * ConjugateGradientSolver.cpp - * - * Created on: Jun 4, 2014 - * Author: Yong-Dian Jian + * @file ConjugateGradientSolver.cpp + * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Yong-Dian Jian + * @author Sungtae An + * @date Nov 6, 2014 */ #include @@ -35,7 +47,8 @@ std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) } /*****************************************************************************/ -ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) { +ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator( + const std::string &src) { std::string s = src; boost::algorithm::to_upper(s); if (s == "GTSAM") return ConjugateGradientParameters::GTSAM; @@ -43,6 +56,7 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla return ConjugateGradientParameters::GTSAM; } +/*****************************************************************************/ } diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 20e0c5262..2596a7403 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -20,7 +20,6 @@ #pragma once #include -#include namespace gtsam { @@ -87,9 +86,8 @@ public: static BLASKernel blasTranslator(const std::string &s) ; }; -/*********************************************************************************************/ /* - * A template of linear preconditioned conjugate gradient method. + * A template for the linear preconditioned conjugate gradient method. * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. @@ -98,8 +96,9 @@ public: * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. */ -template -V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { +template +V preconditionedConjugateGradient(const S &system, const V &initial, + const ConjugateGradientParameters ¶meters) { V estimate, residual, direction, q1, q2; estimate = residual = direction = q1 = q2 = initial; diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index a89e7e21c..401583bbf 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -166,7 +166,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index e0a820819..d9b75c69f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -135,7 +135,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index c67636341..7f9c5ea3c 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -28,6 +28,8 @@ namespace gtsam { // Forward declarations class VectorValues; + class Scatter; + class SymmetricBlockMatrix; /** * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a @@ -119,6 +121,14 @@ namespace gtsam { */ virtual GaussianFactor::shared_ptr negate() const = 0; + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + virtual void updateHessian(const FastVector& keys, + SymmetricBlockMatrix* info) const = 0; + /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; @@ -131,16 +141,22 @@ namespace gtsam { /// Gradient wrt a key at any values virtual Vector gradient(Key key, const VectorValues& x) const = 0; + // Determine position of a given key + template + static DenseIndex Slot(const CONTAINER& keys, Key key) { + return std::find(keys.begin(), keys.end(), key) - keys.begin(); + } + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; // GaussianFactor - + /// traits template<> struct traits : public Testable { diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 6953d2969..a39b1d91e 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -47,7 +47,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { - FastSet keys; + KeySet keys; BOOST_FOREACH(const sharedFactor& factor, *this) if (factor) keys.insert(factor->begin(), factor->end()); @@ -123,26 +123,32 @@ namespace gtsam { /* ************************************************************************* */ vector > GaussianFactorGraph::sparseJacobian() const { // First find dimensions of each variable - vector dims; + typedef std::map KeySizeMap; + KeySizeMap dims; BOOST_FOREACH(const sharedFactor& factor, *this) { - for (GaussianFactor::const_iterator pos = factor->begin(); - pos != factor->end(); ++pos) { - if (dims.size() <= *pos) - dims.resize(*pos + 1, 0); - dims[*pos] = factor->getDim(pos); + if (!static_cast(factor)) continue; + + for (GaussianFactor::const_iterator key = factor->begin(); + key != factor->end(); ++key) { + dims[*key] = factor->getDim(key); } } // Compute first scalar column of each variable - vector columnIndices(dims.size() + 1, 0); - for (size_t j = 1; j < dims.size() + 1; ++j) - columnIndices[j] = columnIndices[j - 1] + dims[j - 1]; + size_t currentColIndex = 0; + KeySizeMap columnIndices = dims; + BOOST_FOREACH(const KeySizeMap::value_type& col, dims) { + columnIndices[col.first] = currentColIndex; + currentColIndex += dims[col.first]; + } // Iterate over all factors, adding sparse scalar entries typedef boost::tuple triplet; vector entries; size_t row = 0; BOOST_FOREACH(const sharedFactor& factor, *this) { + if (!static_cast(factor)) continue; + // Convert to JacobianFactor if necessary JacobianFactor::shared_ptr jacobianFactor( boost::dynamic_pointer_cast(factor)); @@ -159,11 +165,11 @@ namespace gtsam { // Whiten the factor and add entries for it // iterate over all variables in the factor const JacobianFactor whitened(jacobianFactor->whiten()); - for (JacobianFactor::const_iterator pos = whitened.begin(); - pos < whitened.end(); ++pos) { - JacobianFactor::constABlock whitenedA = whitened.getA(pos); + for (JacobianFactor::const_iterator key = whitened.begin(); + key < whitened.end(); ++key) { + JacobianFactor::constABlock whitenedA = whitened.getA(key); // find first column index for this key - size_t column_start = columnIndices[*pos]; + size_t column_start = columnIndices[*key]; for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { double s = whitenedA(i, j); @@ -173,7 +179,7 @@ namespace gtsam { } JacobianFactor::constBVector whitenedb(whitened.getb()); - size_t bcolumn = columnIndices.back(); + size_t bcolumn = currentColIndex; for (size_t i = 0; i < (size_t) whitenedb.size(); i++) entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i))); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 811c0f8b0..02bc95511 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -135,7 +135,7 @@ namespace gtsam { * Return the set of variables involved in the factors (computes a set * union). */ - typedef FastSet Keys; + typedef KeySet Keys; Keys keys() const; /* return a map of (Key, dimension) */ @@ -322,7 +322,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 453e59892..2b275b60f 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -15,17 +15,19 @@ * @date Dec 8, 2010 */ -#include -#include -#include -#include -#include -#include +#include + #include #include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -49,232 +51,185 @@ using namespace std; using namespace boost::assign; -namespace br { using namespace boost::range; using namespace boost::adaptors; } +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} namespace gtsam { -/* ************************************************************************* */ -string SlotEntry::toString() const { - ostringstream oss; - oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; - return oss.str(); -} - -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { - gttic(Scatter_Constructor); - static const size_t none = std::numeric_limits::max(); - - // First do the set union. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { - if (factor) { - for (GaussianFactor::const_iterator variable = factor->begin(); - variable != factor->end(); ++variable) { - // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers - const JacobianFactor* asJacobian = - dynamic_cast(factor.get()); - if (!asJacobian || asJacobian->cols() > 1) - this->insert( - make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); - } - } - } - - // If we have an ordering, pre-fill the ordered variables first - size_t slot = 0; - if (ordering) { - BOOST_FOREACH(Key key, *ordering) { - const_iterator entry = find(key); - if (entry == end()) - throw std::invalid_argument( - "The ordering provided to the HessianFactor Scatter constructor\n" - "contained extra variables that did not appear in the factors to combine."); - at(key).slot = (slot++); - } - } - - // Next fill in the slot indices (we can only get these after doing the set - // union. - BOOST_FOREACH(value_type& var_slot, *this) { - if (var_slot.second.slot == none) - var_slot.second.slot = (slot++); - } -} - /* ************************************************************************* */ HessianFactor::HessianFactor() : - info_(cref_list_of<1>(1)) -{ + info_(cref_list_of<1>(1)) { linearTerm().setZero(); constantTerm() = 0.0; } /* ************************************************************************* */ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) : - GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) -{ - if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0,0) = G; - info_(0,1) = g; - info_(1,1)(0,0) = f; + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) { + if (G.rows() != G.cols() || G.rows() != g.size()) + throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0, 0) = G; + info_(0, 1) = g; + info_(1, 1)(0, 0) = f; } /* ************************************************************************* */ // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : - GaussianFactor(cref_list_of<1>(j)), - info_(cref_list_of<2> (Sigma.cols()) (1) ) -{ - if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0,0) = Sigma.inverse(); // G - info_(0,1) = info_(0,0).selfadjointView() * mu; // g - info_(1,1)(0,0) = mu.dot(info_(0,1).knownOffDiagonal().col(0)); // f + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) { + if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) + throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0, 0) = Sigma.inverse(); // G + info_(0, 1) = info_(0, 0).selfadjointView() * mu; // g + info_(1, 1)(0, 0) = mu.dot(info_(0, 1).knownOffDiagonal().col(0)); // f } /* ************************************************************************* */ -HessianFactor::HessianFactor(Key j1, Key j2, - const Matrix& G11, const Matrix& G12, const Vector& g1, - const Matrix& G22, const Vector& g2, double f) : - GaussianFactor(cref_list_of<2>(j1)(j2)), - info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) -{ - info_(0,0) = G11; - info_(0,1) = G12; - info_(0,2) = g1; - info_(1,1) = G22; - info_(1,2) = g2; - info_(2,2)(0,0) = f; +HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11, + const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2, + double f) : + GaussianFactor(cref_list_of<2>(j1)(j2)), info_( + cref_list_of<3>(G11.cols())(G22.cols())(1)) { + info_(0, 0) = G11; + info_(0, 1) = G12; + info_(0, 2) = g1; + info_(1, 1) = G22; + info_(1, 2) = g2; + info_(2, 2)(0, 0) = f; } /* ************************************************************************* */ -HessianFactor::HessianFactor(Key j1, Key j2, Key j3, - const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, - const Matrix& G22, const Matrix& G23, const Vector& g2, - const Matrix& G33, const Vector& g3, double f) : - GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), - info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) -{ - if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || - G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - info_(0,0) = G11; - info_(0,1) = G12; - info_(0,2) = G13; - info_(0,3) = g1; - info_(1,1) = G22; - info_(1,2) = G23; - info_(1,3) = g2; - info_(2,2) = G33; - info_(2,3) = g3; - info_(3,3)(0,0) = f; +HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, + const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22, + const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3, + double f) : + GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_( + cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) { + if (G11.rows() != G11.cols() || G11.rows() != G12.rows() + || G11.rows() != G13.rows() || G11.rows() != g1.size() + || G22.cols() != G12.cols() || G33.cols() != G13.cols() + || G22.cols() != g2.size() || G33.cols() != g3.size()) + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + info_(0, 0) = G11; + info_(0, 1) = G12; + info_(0, 2) = G13; + info_(0, 3) = g1; + info_(1, 1) = G22; + info_(1, 2) = G23; + info_(1, 3) = g2; + info_(2, 2) = G33; + info_(2, 3) = g3; + info_(3, 3)(0, 0) = f; } /* ************************************************************************* */ namespace { -DenseIndex _getSizeHF(const Vector& m) { return m.size(); } +DenseIndex _getSizeHF(const Vector& m) { + return m.size(); +} } /* ************************************************************************* */ -HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, - const std::vector& gs, double f) : - GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) -{ +HessianFactor::HessianFactor(const std::vector& js, + const std::vector& Gs, const std::vector& gs, double f) : + GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) { // Get the number of variables size_t variable_count = js.size(); // Verify the provided number of entries in the vectors are consistent - if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2) - throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ + if (gs.size() != variable_count + || Gs.size() != (variable_count * (variable_count + 1)) / 2) + throw invalid_argument( + "Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2"); // Verify the dimensions of each provided matrix are consistent // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula - for(size_t i = 0; i < variable_count; ++i){ + for (size_t i = 0; i < variable_count; ++i) { DenseIndex block_size = gs[i].size(); // Check rows - for(size_t j = 0; j < variable_count-i; ++j){ - size_t index = i*(2*variable_count - i + 1)/2 + j; - if(Gs[index].rows() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + for (size_t j = 0; j < variable_count - i; ++j) { + size_t index = i * (2 * variable_count - i + 1) / 2 + j; + if (Gs[index].rows() != block_size) { + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); } } // Check cols - for(size_t j = 0; j <= i; ++j){ - size_t index = j*(2*variable_count - j + 1)/2 + (i-j); - if(Gs[index].cols() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + for (size_t j = 0; j <= i; ++j) { + size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j); + if (Gs[index].cols() != block_size) { + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); } } } // Fill in the blocks size_t index = 0; - for(size_t i = 0; i < variable_count; ++i){ - for(size_t j = i; j < variable_count; ++j){ + for (size_t i = 0; i < variable_count; ++i) { + for (size_t j = i; j < variable_count; ++j) { info_(i, j) = Gs[index++]; } info_(i, variable_count) = gs[i]; } - info_(variable_count, variable_count)(0,0) = f; + info_(variable_count, variable_count)(0, 0) = f; } /* ************************************************************************* */ namespace { -void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) -{ +void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) { gttic(HessianFactor_fromJacobian); const SharedDiagonal& jfModel = jf.get_model(); - if(jfModel) - { - if(jf.get_model()->isConstrained()) - throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - info.full().triangularView() = jf.matrixObject().full().transpose() * - (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() * - jf.matrixObject().full(); + if (jfModel) { + if (jf.get_model()->isConstrained()) + throw invalid_argument( + "Cannot construct HessianFactor from JacobianFactor with constrained noise model"); + info.full().triangularView() = + jf.matrixObject().full().transpose() + * (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() + * jf.matrixObject().full(); } else { - info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); + info.full().triangularView() = jf.matrixObject().full().transpose() + * jf.matrixObject().full(); } } } /* ************************************************************************* */ HessianFactor::HessianFactor(const JacobianFactor& jf) : - GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) -{ + GaussianFactor(jf), info_( + SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) { _FromJacobianHelper(jf, info_); } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactor& gf) : - GaussianFactor(gf) -{ + GaussianFactor(gf) { // Copy the matrix data depending on what type of factor we're copying from - if(const JacobianFactor* jf = dynamic_cast(&gf)) - { + if (const JacobianFactor* jf = dynamic_cast(&gf)) { info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject()); _FromJacobianHelper(*jf, info_); - } - else if(const HessianFactor* hf = dynamic_cast(&gf)) - { + } else if (const HessianFactor* hf = dynamic_cast(&gf)) { info_ = hf->info_; - } - else - { - throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); + } else { + throw std::invalid_argument( + "In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); } } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter) -{ + boost::optional scatter) { gttic(HessianFactor_MergeConstructor); boost::optional computedScatter; - if(!scatter) { + if (!scatter) { computedScatter = Scatter(factors); scatter = computedScatter; } @@ -282,11 +237,14 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Allocate and copy keys gttic(allocate); // Allocate with dimensions for each variable plus 1 at the end for the information vector - keys_.resize(scatter->size()); - FastVector dims(scatter->size() + 1); - BOOST_FOREACH(const Scatter::value_type& key_slotentry, *scatter) { - keys_[key_slotentry.second.slot] = key_slotentry.first; - dims[key_slotentry.second.slot] = key_slotentry.second.dimension; + const size_t n = scatter->size(); + keys_.resize(n); + FastVector dims(n + 1); + DenseIndex slot = 0; + BOOST_FOREACH(const SlotEntry& slotentry, *scatter) { + keys_[slot] = slotentry.key; + dims[slot] = slotentry.dimension; + ++slot; } dims.back() = 1; info_ = SymmetricBlockMatrix(dims); @@ -296,64 +254,50 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Form A' * A gttic(update); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) - { - if(factor) { - if(const HessianFactor* hessian = dynamic_cast(factor.get())) - updateATA(*hessian, *scatter); - else if(const JacobianFactor* jacobian = dynamic_cast(factor.get())) - updateATA(*jacobian, *scatter); - else - throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); - } - } + if (factor) + factor->updateHessian(keys_, &info_); gttoc(update); } /* ************************************************************************* */ -void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const { +void HessianFactor::print(const std::string& s, + const KeyFormatter& formatter) const { cout << s << "\n"; cout << " keys: "; - for(const_iterator key=this->begin(); key!=this->end(); ++key) - cout << formatter(*key) << "(" << this->getDim(key) << ") "; + for (const_iterator key = begin(); key != end(); ++key) + cout << formatter(*key) << "(" << getDim(key) << ") "; cout << "\n"; - gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: "); + gtsam::print(Matrix(info_.full().selfadjointView()), + "Augmented information matrix: "); } /* ************************************************************************* */ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { - if(!dynamic_cast(&lf)) + const HessianFactor* rhs = dynamic_cast(&lf); + if (!rhs || !Factor::equals(lf, tol)) return false; - else { - if(!Factor::equals(lf, tol)) - return false; - Matrix thisMatrix = this->info_.full().selfadjointView(); - thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); - rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; - return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); - } + return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(), + tol); } /* ************************************************************************* */ -Matrix HessianFactor::augmentedInformation() const -{ +Matrix HessianFactor::augmentedInformation() const { return info_.full().selfadjointView(); } /* ************************************************************************* */ -Matrix HessianFactor::information() const -{ - return info_.range(0, this->size(), 0, this->size()).selfadjointView(); +Matrix HessianFactor::information() const { + return info_.range(0, size(), 0, size()).selfadjointView(); } /* ************************************************************************* */ VectorValues HessianFactor::hessianDiagonal() const { VectorValues d; // Loop over all variables - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal Matrix B = info_(j, j).selfadjointView(); - d.insert(keys_[j],B.diagonal()); + d.insert(keys_[j], B.diagonal()); } return d; } @@ -366,26 +310,24 @@ void HessianFactor::hessianDiagonal(double* d) const { } /* ************************************************************************* */ -map HessianFactor::hessianBlockDiagonal() const { - map blocks; +map HessianFactor::hessianBlockDiagonal() const { + map blocks; // Loop over all variables - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert it Matrix B = info_(j, j).selfadjointView(); - blocks.insert(make_pair(keys_[j],B)); + blocks.insert(make_pair(keys_[j], B)); } return blocks; } /* ************************************************************************* */ -Matrix HessianFactor::augmentedJacobian() const -{ +Matrix HessianFactor::augmentedJacobian() const { return JacobianFactor(*this).augmentedJacobian(); } /* ************************************************************************* */ -std::pair HessianFactor::jacobian() const -{ +std::pair HessianFactor::jacobian() const { return JacobianFactor(*this).jacobian(); } @@ -396,107 +338,34 @@ double HessianFactor::error(const VectorValues& c) const { double xtg = 0, xGx = 0; // extract the relevant subset of the VectorValues // NOTE may not be as efficient - const Vector x = c.vector(this->keys()); + const Vector x = c.vector(keys()); xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; - return 0.5 * (f - 2.0 * xtg + xGx); + xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x; + return 0.5 * (f - 2.0 * xtg + xGx); } /* ************************************************************************* */ -void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) -{ - gttic(updateATA); - // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in - // the update factor to slots in the combined factor. - - // First build an array of slots - gttic(slots); - FastVector slots(update.size()); - DenseIndex slot = 0; - BOOST_FOREACH(Key j, update) { - slots[slot] = scatter.at(j).slot; - ++ slot; - } - gttoc(slots); - +void HessianFactor::updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_HessianFactor); // Apply updates to the upper triangle - gttic(update); - size_t nrInfoBlocks = this->info_.nBlocks(); - for(DenseIndex j2=0; j2nBlocks() - 1; + vector slots(n + 1); + for (DenseIndex j = 0; j <= n; ++j) { + const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); + slots[j] = J; + for (DenseIndex i = 0; i <= j; ++i) { + const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid. + (*info)(I, J) += info_(i, j); } } - gttoc(update); -} - -/* ************************************************************************* */ -void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) { - - // This function updates 'combined' with the information in 'update'. - // 'scatter' maps variables in the update factor to slots in the combined - // factor. - - gttic(updateATA); - - if(update.rows() > 0) - { - // First build an array of slots - gttic(slots); - FastVector slots(update.size()); - DenseIndex slot = 0; - BOOST_FOREACH(Key j, update) { - slots[slot] = scatter.at(j).slot; - ++ slot; - } - gttoc(slots); - - gttic(whiten); - // Whiten the factor if it has a noise model - boost::optional _whitenedFactor; - const JacobianFactor* whitenedFactor; - if(update.get_model()) - { - if(update.get_model()->isConstrained()) - throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model"); - - _whitenedFactor = update.whiten(); - whitenedFactor = &(*_whitenedFactor); - } - else - { - whitenedFactor = &update; - } - gttoc(whiten); - - const VerticalBlockMatrix& updateBlocks = whitenedFactor->matrixObject(); - - // Apply updates to the upper triangle - gttic(update); - DenseIndex nrInfoBlocks = this->info_.nBlocks(), nrUpdateBlocks = updateBlocks.nBlocks(); - for(DenseIndex j2 = 0; j2 < nrUpdateBlocks; ++j2) { // Horizontal block of Hessian - DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j2]; - assert(slot2 >= 0 && slot2 <= nrInfoBlocks); - for(DenseIndex j1 = 0; j1 <= j2; ++j1) { // Vertical block of Hessian - DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j1]; - assert(slot1 >= 0 && slot1 < nrInfoBlocks); - if(slot1 != slot2) - info_(slot1, slot2).knownOffDiagonal() += updateBlocks(j1).transpose() * updateBlocks(j2); - else - info_(slot1, slot2).selfadjointView().rankUpdate(updateBlocks(j1).transpose()); - } - } - gttoc(update); - } } /* ************************************************************************* */ -GaussianFactor::shared_ptr HessianFactor::negate() const -{ +GaussianFactor::shared_ptr HessianFactor::negate() const { shared_ptr result = boost::make_shared(*this); - result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result + result->info_.full().triangularView() = + -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result return result; } @@ -513,7 +382,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column // And fill the above temporary y values, to be added into yvalues after - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // xj is the input vector Vector xj = x.at(keys_[j]); DenseIndex i = 0; @@ -522,13 +391,13 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // blocks on the diagonal are only half y[i] += info_(j, j).selfadjointView() * xj; // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex)size(); ++i) + for (i = j + 1; i < (DenseIndex) size(); ++i) y[i] += info_(i, j).knownOffDiagonal() * xj; } // copy to yvalues - for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) { - bool didNotExist; + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + bool didNotExist; VectorValues::iterator it; boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector()); if (didNotExist) @@ -543,7 +412,7 @@ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; size_t n = size(); for (size_t j = 0; j < n; ++j) - g.insert(keys_[j], -info_(j,n).knownOffDiagonal()); + g.insert(keys_[j], -info_(j, n).knownOffDiagonal()); return g; } @@ -566,8 +435,7 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { if (i > j) { Matrix Gji = info(j, i); Gij = Gji.transpose(); - } - else { + } else { Gij = info(i, j); } // Accumulate Gij*xj to gradf @@ -579,30 +447,34 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { } /* ************************************************************************* */ -std::pair, boost::shared_ptr > -EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) -{ +std::pair, + boost::shared_ptr > EliminateCholesky( + const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminateCholesky); // Build joint factor HessianFactor::shared_ptr jointFactor; try { - jointFactor = boost::make_shared(factors, Scatter(factors, keys)); - } catch(std::invalid_argument&) { + jointFactor = boost::make_shared(factors, + Scatter(factors, keys)); + } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateCholesky was called with a request to eliminate variables that are not\n" - "involved in the provided factors."); + "involved in the provided factors."); } // Do dense elimination GaussianConditional::shared_ptr conditional; try { size_t numberOfKeysToEliminate = keys.size(); - VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate); - conditional = boost::make_shared(jointFactor->keys(), numberOfKeysToEliminate, Ab); + VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial( + numberOfKeysToEliminate); + conditional = boost::make_shared(jointFactor->keys(), + numberOfKeysToEliminate, Ab); // Erase the eliminated keys in the remaining factor - jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate); - } catch(CholeskyFailed&) { + jointFactor->keys_.erase(jointFactor->begin(), + jointFactor->begin() + numberOfKeysToEliminate); + } catch (const CholeskyFailed& e) { throw IndeterminantLinearSystemException(keys.front()); } @@ -611,9 +483,9 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) } /* ************************************************************************* */ -std::pair, boost::shared_ptr > -EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) -{ +std::pair, + boost::shared_ptr > EliminatePreferCholesky( + const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminatePreferCholesky); // If any JacobianFactors have constrained noise models, we have to convert diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 553978e37..b74d557ea 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -18,10 +18,10 @@ #pragma once +#include +#include #include #include -#include -#include #include @@ -41,30 +41,6 @@ namespace gtsam { GTSAM_EXPORT std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); - /** - * One SlotEntry stores the slot index for a variable, as well its dimension. - */ - struct GTSAM_EXPORT SlotEntry { - size_t slot, dimension; - SlotEntry(size_t _slot, size_t _dimension) - : slot(_slot), dimension(_dimension) {} - std::string toString() const; - }; - - /** - * Scatter is an intermediate data structure used when building a HessianFactor - * incrementally, to get the keys in the right order. The "scatter" is a map from - * global variable indices to slot indices in the union of involved variables. - * We also include the dimensionality of the variable. - */ - class Scatter: public FastMap { - public: - Scatter() { - } - Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering = boost::none); - }; - /** * @brief A Gaussian factor using the canonical parameters (information form) * @@ -363,19 +339,12 @@ namespace gtsam { /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */ const SymmetricBlockMatrix& matrixObject() const { return info_; } - /** Update the factor by adding the information from the JacobianFactor + /** Update an information matrix by adding the information corresponding to this factor * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated */ - void updateATA(const JacobianFactor& update, const Scatter& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactor& update, const Scatter& scatter); + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; @@ -434,7 +403,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); ar & BOOST_SERIALIZATION_NVP(info_); } diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index ab3ccde13..79ade1c8a 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -1,6 +1,17 @@ +/* ---------------------------------------------------------------------------- + + * 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 IterativeSolver.cpp - * @brief + * @brief Some support classes for iterative solvers * @date Sep 3, 2012 * @author Yong-Dian Jian */ @@ -9,18 +20,22 @@ #include #include #include +#include #include -#include using namespace std; namespace gtsam { /*****************************************************************************/ -string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } +string IterativeOptimizationParameters::getVerbosity() const { + return verbosityTranslator(verbosity_); +} /*****************************************************************************/ -void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); } +void IterativeOptimizationParameters::setVerbosity(const string &src) { + verbosity_ = verbosityTranslator(src); +} /*****************************************************************************/ void IterativeOptimizationParameters::print() const { @@ -29,78 +44,82 @@ void IterativeOptimizationParameters::print() const { /*****************************************************************************/ void IterativeOptimizationParameters::print(ostream &os) const { - os << "IterativeOptimizationParameters:" << endl - << "verbosity: " << verbosityTranslator(verbosity_) << endl; + os << "IterativeOptimizationParameters:" << endl << "verbosity: " + << verbosityTranslator(verbosity_) << endl; } /*****************************************************************************/ - ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { +ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { p.print(os); return os; } /*****************************************************************************/ -IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) { - string s = src; boost::algorithm::to_upper(s); - if (s == "SILENT") return IterativeOptimizationParameters::SILENT; - else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; - else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator( + const string &src) { + string s = src; + boost::algorithm::to_upper(s); + if (s == "SILENT") + return IterativeOptimizationParameters::SILENT; + else if (s == "COMPLEXITY") + return IterativeOptimizationParameters::COMPLEXITY; + else if (s == "ERROR") + return IterativeOptimizationParameters::ERROR; /* default is default */ - else return IterativeOptimizationParameters::SILENT; + else + return IterativeOptimizationParameters::SILENT; } /*****************************************************************************/ - string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { - if (verbosity == SILENT) return "SILENT"; - else if (verbosity == COMPLEXITY) return "COMPLEXITY"; - else if (verbosity == ERROR) return "ERROR"; - else return "UNKNOWN"; +string IterativeOptimizationParameters::verbosityTranslator( + IterativeOptimizationParameters::Verbosity verbosity) { + if (verbosity == SILENT) + return "SILENT"; + else if (verbosity == COMPLEXITY) + return "COMPLEXITY"; + else if (verbosity == ERROR) + return "ERROR"; + else + return "UNKNOWN"; } /*****************************************************************************/ -VectorValues IterativeSolver::optimize( - const GaussianFactorGraph &gfg, +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, boost::optional keyInfo, - boost::optional&> lambda) -{ - return optimize( - gfg, - keyInfo ? *keyInfo : KeyInfo(gfg), - lambda ? *lambda : std::map()); + boost::optional&> lambda) { + return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); } /*****************************************************************************/ -VectorValues IterativeSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda) -{ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda) { return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) - : ordering_(ordering) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) : + ordering_(ordering) { initialize(fg); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg) - : ordering_(Ordering::Natural(fg)) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg) : + ordering_(Ordering::Natural(fg)) { initialize(fg); } /****************************************************************************/ -void KeyInfo::initialize(const GaussianFactorGraph &fg){ +void KeyInfo::initialize(const GaussianFactorGraph &fg) { const map colspec = fg.getKeyDimMap(); const size_t n = ordering_.size(); size_t start = 0; - for ( size_t i = 0 ; i < n ; ++i ) { + for (size_t i = 0; i < n; ++i) { const size_t key = ordering_[i]; const size_t dim = colspec.find(key)->second; insert(make_pair(key, KeyInfoEntry(i, dim, start))); - start += dim ; + start += dim; } numCols_ = start; } @@ -108,7 +127,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg){ /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result[item.second.index()] = item.second.dim(); } return result; @@ -117,7 +136,7 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result.insert(item.first, Vector::Zero(item.second.dim())); } return result; @@ -128,7 +147,5 @@ Vector KeyInfo::x0vector() const { return Vector::Zero(numCols_); } - } - diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index d6e1b6e98..442a5fc6b 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -9,131 +9,178 @@ * -------------------------------------------------------------------------- */ +/** + * @file IterativeSolver.h + * @brief Some support classes for iterative solvers + * @date 2010 + * @author Yong-Dian Jian + */ + #pragma once -#include -#include #include +#include + #include -#include -#include +#include +#include + #include -#include #include -#include +#include namespace gtsam { - // Forward declarations - class KeyInfo; - class KeyInfoEntry; - class GaussianFactorGraph; - class Values; - class VectorValues; +// Forward declarations +class KeyInfo; +class KeyInfoEntry; +class GaussianFactorGraph; +class Values; +class VectorValues; - /************************************************************************************/ - /** - * parameters for iterative linear solvers - */ - class GTSAM_EXPORT IterativeOptimizationParameters { +/** + * parameters for iterative linear solvers + */ +class GTSAM_EXPORT IterativeOptimizationParameters { - public: +public: - typedef boost::shared_ptr shared_ptr; - enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; + typedef boost::shared_ptr shared_ptr; + enum Verbosity { + SILENT = 0, COMPLEXITY, ERROR + } verbosity_; - public: +public: - IterativeOptimizationParameters(Verbosity v = SILENT) - : verbosity_(v) {} + IterativeOptimizationParameters(Verbosity v = SILENT) : + verbosity_(v) { + } - virtual ~IterativeOptimizationParameters() {} + virtual ~IterativeOptimizationParameters() { + } - /* utility */ - inline Verbosity verbosity() const { return verbosity_; } - std::string getVerbosity() const; - void setVerbosity(const std::string &s) ; + /* utility */ + inline Verbosity verbosity() const { + return verbosity_; + } + std::string getVerbosity() const; + void setVerbosity(const std::string &s); - /* matlab interface */ - void print() const ; + /* matlab interface */ + void print() const; - /* virtual print function */ - virtual void print(std::ostream &os) const ; + /* virtual print function */ + virtual void print(std::ostream &os) const; - /* for serialization */ - friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p); + /* for serialization */ + friend std::ostream& operator<<(std::ostream &os, + const IterativeOptimizationParameters &p); - static Verbosity verbosityTranslator(const std::string &s); - static std::string verbosityTranslator(Verbosity v); - }; + static Verbosity verbosityTranslator(const std::string &s); + static std::string verbosityTranslator(Verbosity v); +}; - /************************************************************************************/ - class GTSAM_EXPORT IterativeSolver { - public: - typedef boost::shared_ptr shared_ptr; - IterativeSolver() {} - virtual ~IterativeSolver() {} +/** + * Base class for Iterative Solvers like SubgraphSolver + */ +class GTSAM_EXPORT IterativeSolver { +public: + typedef boost::shared_ptr shared_ptr; + IterativeSolver() { + } + virtual ~IterativeSolver() { + } - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, boost::optional = boost::none, - boost::optional&> lambda = boost::none - ); + boost::optional&> lambda = boost::none); - /* interface to the nonlinear optimizer, without initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda - ); + /* interface to the nonlinear optimizer, without initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, + const std::map &lambda); - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) = 0; + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) = 0; - }; +}; - /************************************************************************************/ - /* Handy data structure for iterative solvers - * key to (index, dimension, colstart) */ - class GTSAM_EXPORT KeyInfoEntry : public boost::tuple { - public: - typedef boost::tuple Base; - KeyInfoEntry(){} - KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} - size_t index() const { return this->get<0>(); } - size_t dim() const { return this->get<1>(); } - size_t colstart() const { return this->get<2>(); } - }; +/** + * Handy data structure for iterative solvers + * key to (index, dimension, colstart) + */ +class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { - /************************************************************************************/ - class GTSAM_EXPORT KeyInfo : public std::map { - public: - typedef std::map Base; - KeyInfo() : numCols_(0) {} - KeyInfo(const GaussianFactorGraph &fg); - KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); +public: - std::vector colSpec() const ; - VectorValues x0() const; - Vector x0vector() const; + typedef boost::tuple Base; - inline size_t numCols() const { return numCols_; } - inline const Ordering & ordering() const { return ordering_; } + KeyInfoEntry() { + } + KeyInfoEntry(size_t idx, size_t d, Key start) : + Base(idx, d, start) { + } + size_t index() const { + return this->get<0>(); + } + size_t dim() const { + return this->get<1>(); + } + size_t colstart() const { + return this->get<2>(); + } +}; - protected: +/** + * Handy data structure for iterative solvers + */ +class GTSAM_EXPORT KeyInfo: public std::map { - void initialize(const GaussianFactorGraph &fg); +public: - Ordering ordering_; - size_t numCols_; + typedef std::map Base; - }; +protected: + Ordering ordering_; + size_t numCols_; -} + void initialize(const GaussianFactorGraph &fg); + +public: + + /// Default Constructor + KeyInfo() : + numCols_(0) { + } + + /// Construct from Gaussian factor graph, use "Natural" ordering + KeyInfo(const GaussianFactorGraph &fg); + + /// Construct from Gaussian factor graph and a given ordering + KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); + + /// Return the total number of columns (scalar variables = sum of dimensions) + inline size_t numCols() const { + return numCols_; + } + + /// Return the ordering + inline const Ordering & ordering() const { + return ordering_; + } + + /// Return a vector of dimensions ordered by ordering() + std::vector colSpec() const; + + /// Return VectorValues with zeros, of correct dimension + VectorValues x0() const; + + /// Return zero Vector of correct dimension + Vector x0vector() const; + +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index eba06f99a..6e14960fd 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -66,10 +66,10 @@ JacobianFactor::JacobianFactor() : /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactor& gf) { // Copy the matrix data depending on what type of factor we're copying from - if (const JacobianFactor* rhs = dynamic_cast(&gf)) - *this = JacobianFactor(*rhs); - else if (const HessianFactor* rhs = dynamic_cast(&gf)) - *this = JacobianFactor(*rhs); + if (const JacobianFactor* asJacobian = dynamic_cast(&gf)) + *this = JacobianFactor(*asJacobian); + else if (const HessianFactor* asHessian = dynamic_cast(&gf)) + *this = JacobianFactor(*asHessian); else throw std::invalid_argument( "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); @@ -365,32 +365,50 @@ void JacobianFactor::print(const string& s, /* ************************************************************************* */ // Check if two linear factors are equal bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const { - if (!dynamic_cast(&f_)) + static const bool verbose = false; + if (!dynamic_cast(&f_)) { + if (verbose) + cout << "JacobianFactor::equals: Incorrect type" << endl; return false; - else { + } else { const JacobianFactor& f(static_cast(f_)); // Check keys - if (keys() != f.keys()) + if (keys() != f.keys()) { + if (verbose) + cout << "JacobianFactor::equals: keys do not match" << endl; return false; + } // Check noise model - if ((model_ && !f.model_) || (!model_ && f.model_)) + if ((model_ && !f.model_) || (!model_ && f.model_)) { + if (verbose) + cout << "JacobianFactor::equals: noise model mismatch" << endl; return false; - if (model_ && f.model_ && !model_->equals(*f.model_, tol)) + } + if (model_ && f.model_ && !model_->equals(*f.model_, tol)) { + if (verbose) + cout << "JacobianFactor::equals: noise modesl are not equal" << endl; return false; + } // Check matrix sizes - if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) + if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) { + if (verbose) + cout << "JacobianFactor::equals: augmented size mismatch" << endl; return false; + } // Check matrix contents constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); for (size_t row = 0; row < (size_t) Ab1.rows(); ++row) if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) - && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) + && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) { + if (verbose) + cout << "JacobianFactor::equals: matrix mismatch at row " << row << endl; return false; + } return true; } @@ -414,8 +432,6 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const { /* ************************************************************************* */ double JacobianFactor::error(const VectorValues& c) const { - if (empty()) - return 0; Vector weighted = error_vector(c); return 0.5 * weighted.dot(weighted); } @@ -479,6 +495,43 @@ map JacobianFactor::hessianBlockDiagonal() const { return blocks; } +/* ************************************************************************* */ +void JacobianFactor::updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_JacobianFactor); + + if (rows() == 0) return; + + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw invalid_argument( + "JacobianFactor::updateHessian: cannot update information with " + "constrained noise model"); + JacobianFactor whitenedFactor = whiten(); + whitenedFactor.updateHessian(infoKeys, info); + } else { + // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below + DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1; + + // Apply updates to the upper triangle + // Loop over blocks of A, including RHS with j==n + vector slots(n+1); + for (DenseIndex j = 0; j <= n; ++j) { + const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); + slots[j] = J; + // Fill off-diagonal blocks with Ai'*Aj + for (DenseIndex i = 0; i < j; ++i) { + const DenseIndex I = slots[i]; // because i& accumulatedDims) const { + + /// Use Eigen magic to access raw memory + typedef Eigen::Map VectorMap; + typedef Eigen::Map ConstVectorMap; + + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + /// Just iterate over all A matrices and multiply in correct config part (looping over keys) + /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + Ax += Ab_(pos) * ConstVectorMap(x + offset, dim); + } + /// Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + /// multiply with alpha + Ax *= alpha; + + /// Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax; + } +} + /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; @@ -629,8 +727,8 @@ std::pair, jointFactor->Ab_.matrix().triangularView().setZero(); // Split elimination result into conditional and remaining factor - GaussianConditional::shared_ptr conditional = jointFactor->splitConditional( - keys.size()); + GaussianConditional::shared_ptr conditional = // + jointFactor->splitConditional(keys.size()); return make_pair(conditional, jointFactor); } @@ -659,11 +757,11 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional( } GaussianConditional::shared_ptr conditional = boost::make_shared< GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); - const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) + const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim; const DenseIndex remainingRows = - model_ ? - std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) : + model_ ? std::min(model_->sigmas().size() - frontalDim, + maxRemainingRows) : maxRemainingRows; Ab_.rowStart() += frontalDim; Ab_.rowEnd() = Ab_.rowStart() + remainingRows; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 02ae21566..ea9958474 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -273,6 +273,13 @@ namespace gtsam { /** Get a view of the A matrix */ ABlock getA() { return Ab_.range(0, size()); } + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; + /** Return A*x */ Vector operator*(const VectorValues& x) const; @@ -283,6 +290,17 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; + /** + * Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * Requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! + * TODO(frank): we should probably kill this if no longer needed + */ + void multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const; + /// A'*b for Jacobian VectorValues gradientAtZero() const; @@ -349,7 +367,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(Ab_); ar & BOOST_SERIALIZATION_NVP(model_); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 2031a4b73..609c03acf 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include using namespace std; @@ -70,6 +72,11 @@ boost::optional checkIfDiagonal(const Matrix M) { } } +/* ************************************************************************* */ +Vector Base::sigmas() const { + throw("Base::sigmas: sigmas() not implemented for this noise model"); +} + /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { size_t m = R.rows(), n = R.cols(); @@ -79,24 +86,25 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { if (smart) diagonal = checkIfDiagonal(R); if (diagonal) - return Diagonal::Sigmas(reciprocal(*diagonal), true); + return Diagonal::Sigmas(diagonal->array().inverse(), true); else return shared_ptr(new Gaussian(R.rows(), R)); } /* ************************************************************************* */ -Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) { - size_t m = M.rows(), n = M.cols(); +Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart) { + size_t m = information.rows(), n = information.cols(); if (m != n) throw invalid_argument("Gaussian::Information: R not square"); boost::optional diagonal = boost::none; if (smart) - diagonal = checkIfDiagonal(M); + diagonal = checkIfDiagonal(information); if (diagonal) return Diagonal::Precisions(*diagonal, true); else { - Matrix R = RtR(M); - return shared_ptr(new Gaussian(R.rows(), R)); + Eigen::LLT llt(information); + Matrix R = llt.matrixU(); + return shared_ptr(new Gaussian(n, R)); } } @@ -111,13 +119,15 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, variances = checkIfDiagonal(covariance); if (variances) return Diagonal::Variances(*variances, true); - else - return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); + else { + // TODO: can we do this more efficiently and still get an upper triangular nmatrix?? + return Information(covariance.inverse(), false); + } } /* ************************************************************************* */ void Gaussian::print(const string& name) const { - gtsam::print(thisR(), "Gaussian"); + gtsam::print(thisR(), name + "Gaussian"); } /* ************************************************************************* */ @@ -129,6 +139,12 @@ bool Gaussian::equals(const Base& expected, double tol) const { return equal_with_abs_tol(R(), p->R(), sqrt(tol)); } +/* ************************************************************************* */ +Vector Gaussian::sigmas() const { + // TODO(frank): can this be done faster? + return Vector((thisR().transpose() * thisR()).inverse().diagonal()).cwiseSqrt(); +} + /* ************************************************************************* */ Vector Gaussian::whiten(const Vector& v) const { return thisR() * v; @@ -221,9 +237,11 @@ Diagonal::Diagonal() : } /* ************************************************************************* */ -Diagonal::Diagonal(const Vector& sigmas) : - Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_( - emul(invsigmas_, invsigmas_)) { +Diagonal::Diagonal(const Vector& sigmas) + : Gaussian(sigmas.size()), + sigmas_(sigmas), + invsigmas_(sigmas.array().inverse()), + precisions_(invsigmas_.array().square()) { } /* ************************************************************************* */ @@ -262,12 +280,12 @@ void Diagonal::print(const string& name) const { /* ************************************************************************* */ Vector Diagonal::whiten(const Vector& v) const { - return emul(v, invsigmas()); + return v.cwiseProduct(invsigmas_); } /* ************************************************************************* */ Vector Diagonal::unwhiten(const Vector& v) const { - return emul(v, sigmas_); + return v.cwiseProduct(sigmas_); } /* ************************************************************************* */ @@ -293,7 +311,7 @@ namespace internal { // switch precisions and invsigmas to finite value // TODO: why?? And, why not just ask s==0.0 below ? static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { - for (size_t i = 0; i < sigmas.size(); ++i) + for (Vector::Index i = 0; i < sigmas.size(); ++i) if (!std::isfinite(1. / sigmas[i])) { precisions[i] = 0.0; invsigmas[i] = 0.0; @@ -342,7 +360,7 @@ Vector Constrained::whiten(const Vector& v) const { assert (b.size()==a.size()); Vector c(n); for( size_t i = 0; i < n; i++ ) { - const double& ai = a(i), &bi = b(i); + const double& ai = a(i), bi = b(i); c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_() } return c; @@ -359,8 +377,11 @@ double Constrained::distance(const Vector& v) const { /* ************************************************************************* */ Matrix Constrained::Whiten(const Matrix& H) const { - // selective scaling - return vector_scale(invsigmas(), H, true); + Matrix A = H; + for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) + if (!constrained(i)) // if constrained, leave row of A as is + A.row(i) *= invsigmas_(i); + return A; } /* ************************************************************************* */ @@ -404,8 +425,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { list Rd; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector invsigmas = reciprocal(sigmas_); - Vector weights = emul(invsigmas,invsigmas); // calculate weights once + Vector invsigmas = sigmas_.array().inverse(); + Vector weights = invsigmas.array().square(); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding @@ -486,7 +507,7 @@ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smar /* ************************************************************************* */ void Isotropic::print(const string& name) const { - cout << name << "isotropic sigma " << " " << sigma_ << endl; + cout << boost::format("isotropic dim=%1% sigma=%2%") % dim() % sigma_ << endl; } /* ************************************************************************* */ @@ -514,6 +535,11 @@ void Isotropic::WhitenInPlace(Matrix& H) const { H *= invsigma_; } +/* ************************************************************************* */ +void Isotropic::whitenInPlace(Vector& v) const { + v *= invsigma_; +} + /* ************************************************************************* */ void Isotropic::WhitenInPlace(Eigen::Block H) const { H *= invsigma_; @@ -542,16 +568,6 @@ Vector Base::weight(const Vector &error) const { return w; } -/** square root version of the weight function */ -Vector Base::sqrtWeight(const Vector &error) const { - const size_t n = error.rows(); - Vector w(n); - for ( size_t i = 0 ; i < n ; ++i ) - w(i) = sqrtWeight(error(i)); - return w; -} - - /** The following three functions reweight block matrices and a vector * according to their weight implementation */ @@ -560,8 +576,7 @@ void Base::reweight(Vector& error) const { const double w = sqrtWeight(error.norm()); error *= w; } else { - const Vector w = sqrtWeight(error); - error.array() *= w.array(); + error.array() *= weight(error).cwiseSqrt().array(); } } @@ -579,7 +594,7 @@ void Base::reweight(vector &A, Vector &error) const { BOOST_FOREACH(Matrix& Aj, A) { vector_scale_inplace(W,Aj); } - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -593,7 +608,7 @@ void Base::reweight(Matrix &A, Vector &error) const { else { const Vector W = sqrtWeight(error); vector_scale_inplace(W,A); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -609,7 +624,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { const Vector W = sqrtWeight(error); vector_scale_inplace(W,A1); vector_scale_inplace(W,A2); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -627,7 +642,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { vector_scale_inplace(W,A1); vector_scale_inplace(W,A2); vector_scale_inplace(W,A3); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -641,7 +656,7 @@ void Null::print(const std::string &s="") const Null::shared_ptr Null::Create() { return shared_ptr(new Null()); } -Fair::Fair(const double c, const ReweightScheme reweight) +Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { if ( c_ <= 0 ) { cout << "mEstimator Fair takes only positive double in constructor. forced to 1.0" << endl; @@ -653,26 +668,26 @@ Fair::Fair(const double c, const ReweightScheme reweight) // Fair /* ************************************************************************* */ -double Fair::weight(const double &error) const +double Fair::weight(double error) const { return 1.0 / (1.0 + fabs(error)/c_); } void Fair::print(const std::string &s="") const { cout << s << "fair (" << c_ << ")" << endl; } -bool Fair::equals(const Base &expected, const double tol) const { +bool Fair::equals(const Base &expected, double tol) const { const Fair* p = dynamic_cast (&expected); if (p == NULL) return false; return fabs(c_ - p->c_ ) < tol; } -Fair::shared_ptr Fair::Create(const double c, const ReweightScheme reweight) +Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Fair(c, reweight)); } /* ************************************************************************* */ // Huber /* ************************************************************************* */ -Huber::Huber(const double k, const ReweightScheme reweight) +Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { if ( k_ <= 0 ) { cout << "mEstimator Huber takes only positive double in constructor. forced to 1.0" << endl; @@ -680,7 +695,7 @@ Huber::Huber(const double k, const ReweightScheme reweight) } } -double Huber::weight(const double &error) const { +double Huber::weight(double error) const { return (error < k_) ? (1.0) : (k_ / fabs(error)); } @@ -688,13 +703,13 @@ void Huber::print(const std::string &s="") const { cout << s << "huber (" << k_ << ")" << endl; } -bool Huber::equals(const Base &expected, const double tol) const { +bool Huber::equals(const Base &expected, double tol) const { const Huber* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(k_ - p->k_) < tol; } -Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { +Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Huber(c, reweight)); } @@ -702,7 +717,7 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { // Cauchy /* ************************************************************************* */ -Cauchy::Cauchy(const double k, const ReweightScheme reweight) +Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { if ( k_ <= 0 ) { cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl; @@ -710,7 +725,7 @@ Cauchy::Cauchy(const double k, const ReweightScheme reweight) } } -double Cauchy::weight(const double &error) const { +double Cauchy::weight(double error) const { return k_*k_ / (k_*k_ + error*error); } @@ -718,24 +733,24 @@ void Cauchy::print(const std::string &s="") const { cout << s << "cauchy (" << k_ << ")" << endl; } -bool Cauchy::equals(const Base &expected, const double tol) const { +bool Cauchy::equals(const Base &expected, double tol) const { const Cauchy* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(k_ - p->k_) < tol; } -Cauchy::shared_ptr Cauchy::Create(const double c, const ReweightScheme reweight) { +Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Cauchy(c, reweight)); } /* ************************************************************************* */ // Tukey /* ************************************************************************* */ -Tukey::Tukey(const double c, const ReweightScheme reweight) +Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double Tukey::weight(const double &error) const { +double Tukey::weight(double error) const { if (fabs(error) <= c_) { double xc2 = (error/c_)*(error/c_); double one_xc22 = (1.0-xc2)*(1.0-xc2); @@ -748,24 +763,24 @@ void Tukey::print(const std::string &s="") const { std::cout << s << ": Tukey (" << c_ << ")" << std::endl; } -bool Tukey::equals(const Base &expected, const double tol) const { +bool Tukey::equals(const Base &expected, double tol) const { const Tukey* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(c_ - p->c_) < tol; } -Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) { +Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Tukey(c, reweight)); } /* ************************************************************************* */ // Welsh /* ************************************************************************* */ -Welsh::Welsh(const double c, const ReweightScheme reweight) +Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double Welsh::weight(const double &error) const { +double Welsh::weight(double error) const { double xc2 = (error/c_)*(error/c_); return std::exp(-xc2); } @@ -774,13 +789,13 @@ void Welsh::print(const std::string &s="") const { std::cout << s << ": Welsh (" << c_ << ")" << std::endl; } -bool Welsh::equals(const Base &expected, const double tol) const { +bool Welsh::equals(const Base &expected, double tol) const { const Welsh* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(c_ - p->c_) < tol; } -Welsh::shared_ptr Welsh::Create(const double c, const ReweightScheme reweight) { +Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsh(c, reweight)); } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 098af8b6d..f2f8a01cf 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -26,7 +26,6 @@ #include #include #include -#include namespace gtsam { @@ -59,14 +58,15 @@ namespace gtsam { public: - /** primary constructor @param dim is the dimension of the model */ + /// primary constructor @param dim is the dimension of the model Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} - /// true if a constrained noise mode, saves slow/clumsy dynamic casting - virtual bool isConstrained() const { - return false; // default false - } + /// true if a constrained noise model, saves slow/clumsy dynamic casting + virtual bool isConstrained() const { return false; } // default false + + /// true if a unit noise model, saves slow/clumsy dynamic casting + virtual bool isUnit() const { return false; } // default false /// Dimensionality inline size_t dim() const { return dim_;} @@ -75,14 +75,16 @@ namespace gtsam { virtual bool equals(const Base& expected, double tol=1e-9) const = 0; - /** - * Whiten an error vector. - */ + /// Calculate standard deviations + virtual Vector sigmas() const; + + /// Whiten an error vector. virtual Vector whiten(const Vector& v) const = 0; - /** - * Unwhiten an error vector. - */ + /// Whiten a matrix. + virtual Matrix Whiten(const Matrix& H) const = 0; + + /// Unwhiten an error vector. virtual Vector unwhiten(const Vector& v) const = 0; virtual double distance(const Vector& v) const = 0; @@ -116,7 +118,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(dim_); } }; @@ -189,6 +191,7 @@ namespace gtsam { virtual void print(const std::string& name) const; virtual bool equals(const Base& expected, double tol=1e-9) const; + virtual Vector sigmas() const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; @@ -220,9 +223,9 @@ namespace gtsam { /** * Whiten a system, in place as well */ - virtual void WhitenSystem(std::vector& A, Vector& b) const ; - virtual void WhitenSystem(Matrix& A, Vector& b) const ; - virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; + virtual void WhitenSystem(std::vector& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; /** @@ -234,16 +237,20 @@ namespace gtsam { */ virtual boost::shared_ptr QR(Matrix& Ab) const; - /** - * Return R itself, but note that Whiten(H) is cheaper than R*H - */ + /// Return R itself, but note that Whiten(H) is cheaper than R*H virtual Matrix R() const { return thisR();} + /// Compute information matrix + virtual Matrix information() const { return thisR().transpose() * thisR(); } + + /// Compute covariance matrix + virtual Matrix covariance() const { return information().inverse(); } + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(sqrt_information_); } @@ -303,6 +310,7 @@ namespace gtsam { } virtual void print(const std::string& name) const; + virtual Vector sigmas() const { return sigmas_; } virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; @@ -312,7 +320,6 @@ namespace gtsam { /** * Return standard deviations (sqrt of diagonal) */ - inline const Vector& sigmas() const { return sigmas_; } inline double sigma(size_t i) const { return sigmas_(i); } /** @@ -338,7 +345,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian); ar & BOOST_SERIALIZATION_NVP(sigmas_); ar & BOOST_SERIALIZATION_NVP(invsigmas_); @@ -387,9 +394,7 @@ namespace gtsam { virtual ~Constrained() {} /// true if a constrained noise mode, saves slow/clumsy dynamic casting - virtual bool isConstrained() const { - return true; - } + virtual bool isConstrained() const { return true; } /// Return true if a particular dimension is free or constrained bool constrained(size_t i) const; @@ -489,7 +494,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal); ar & BOOST_SERIALIZATION_NVP(mu_); } @@ -545,6 +550,7 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; + virtual void whitenInPlace(Vector& v) const; virtual void WhitenInPlace(Eigen::Block H) const; /** @@ -556,7 +562,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal); ar & BOOST_SERIALIZATION_NVP(sigma_); ar & BOOST_SERIALIZATION_NVP(invsigma_); @@ -587,23 +593,26 @@ namespace gtsam { return shared_ptr(new Unit(dim)); } + /// true if a unit noise model, saves slow/clumsy dynamic casting + virtual bool isUnit() const { return true; } + virtual void print(const std::string& name) const; virtual double Mahalanobis(const Vector& v) const {return v.dot(v); } virtual Vector whiten(const Vector& v) const { return v; } virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } - virtual void WhitenInPlace(Matrix& H) const {} - virtual void WhitenInPlace(Eigen::Block H) const {} - virtual void whitenInPlace(Vector& v) const {} - virtual void unwhitenInPlace(Vector& v) const {} - virtual void whitenInPlace(Eigen::Block& v) const {} - virtual void unwhitenInPlace(Eigen::Block& v) const {} + virtual void WhitenInPlace(Matrix& /*H*/) const {} + virtual void WhitenInPlace(Eigen::Block /*H*/) const {} + virtual void whitenInPlace(Vector& /*v*/) const {} + virtual void unwhitenInPlace(Vector& /*v*/) const {} + virtual void whitenInPlace(Eigen::Block& /*v*/) const {} + virtual void unwhitenInPlace(Eigen::Block& /*v*/) const {} private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic); } }; @@ -629,20 +638,23 @@ namespace gtsam { virtual ~Base() {} /// robust error function to implement - virtual double weight(const double &error) const = 0; + virtual double weight(double error) const = 0; virtual void print(const std::string &s) const = 0; - virtual bool equals(const Base& expected, const double tol=1e-8) const = 0; + virtual bool equals(const Base& expected, double tol=1e-8) const = 0; - inline double sqrtWeight(const double &error) const - { return std::sqrt(weight(error)); } + double sqrtWeight(double error) const { + return std::sqrt(weight(error)); + } /** produce a weight vector according to an error vector and the implemented * robust function */ Vector weight(const Vector &error) const; /** square root version of the weight function */ - Vector sqrtWeight(const Vector &error) const; + Vector sqrtWeight(const Vector &error) const { + return weight(error).cwiseSqrt(); + } /** reweight block matrices and a vector according to their weight implementation */ void reweight(Vector &error) const; @@ -655,7 +667,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(reweight_); } }; @@ -667,16 +679,16 @@ namespace gtsam { Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} - virtual double weight(const double &error) const { return 1.0; } + virtual double weight(double /*error*/) const { return 1.0; } virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, const double tol=1e-8) const { return true; } + virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; @@ -686,12 +698,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Fair(const double c = 1.3998, const ReweightScheme reweight = Block); + Fair(double c = 1.3998, const ReweightScheme reweight = Block); virtual ~Fair() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double c, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; protected: double c_; @@ -700,7 +712,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(c_); } @@ -712,11 +724,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; virtual ~Huber() {} - Huber(const double k = 1.345, const ReweightScheme reweight = Block); - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + Huber(double k = 1.345, const ReweightScheme reweight = Block); + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double k_; @@ -725,7 +737,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(k_); } @@ -741,11 +753,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; virtual ~Cauchy() {} - Cauchy(const double k = 0.1, const ReweightScheme reweight = Block); - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + Cauchy(double k = 0.1, const ReweightScheme reweight = Block); + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double k_; @@ -754,7 +766,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(k_); } @@ -765,12 +777,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Tukey(const double c = 4.6851, const ReweightScheme reweight = Block); + Tukey(double c = 4.6851, const ReweightScheme reweight = Block); virtual ~Tukey() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double c_; @@ -779,7 +791,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(c_); } @@ -790,12 +802,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Welsh(const double c = 2.9846, const ReweightScheme reweight = Block); + Welsh(double c = 2.9846, const ReweightScheme reweight = Block); virtual ~Welsh() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double c_; @@ -804,7 +816,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(c_); } @@ -848,7 +860,9 @@ namespace gtsam { // TODO: functions below are dummy but necessary for the noiseModel::Base inline virtual Vector whiten(const Vector& v) const { Vector r = v; this->WhitenSystem(r); return r; } - inline virtual Vector unwhiten(const Vector& v) const + inline virtual Matrix Whiten(const Matrix& A) const + { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } + inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } inline virtual double distance(const Vector& v) const { return this->whiten(v).squaredNorm(); } @@ -867,7 +881,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & boost::serialization::make_nvp("robust_", const_cast(robust_)); ar & boost::serialization::make_nvp("noise_", const_cast(noise_)); diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 3698edc2f..caf7d0095 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -1,16 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.cpp - * - * Created on: Feb 14, 2012 - * Author: Yong-Dian Jian - * Author: Sungtae An + * @file PCGSolver.cpp + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Feb 14, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ -#include #include +#include #include +#include +#include #include + +#include #include #include @@ -21,7 +36,7 @@ namespace gtsam { /*****************************************************************************/ void PCGSolverParameters::print(ostream &os) const { Base::print(os); - os << "PCGSolverParameters:" << endl; + os << "PCGSolverParameters:" << endl; preconditioner_->print(os); } @@ -32,30 +47,27 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) { } /*****************************************************************************/ -VectorValues PCGSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial) -{ +VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { /* build preconditioner */ preconditioner_->build(gfg, keyInfo, lambda); /* apply pcg */ - const Vector sol = preconditionedConjugateGradient( - GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda), - initial.vector(keyInfo.ordering()), parameters_); + GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda); + Vector x0 = initial.vector(keyInfo.ordering()); + const Vector sol = preconditionedConjugateGradient(system, x0, parameters_); return buildVectorValues(sol, keyInfo); } /*****************************************************************************/ GaussianFactorGraphSystem::GaussianFactorGraphSystem( - const GaussianFactorGraph &gfg, - const Preconditioner &preconditioner, - const KeyInfo &keyInfo, - const std::map &lambda) - : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {} + const GaussianFactorGraph &gfg, const Preconditioner &preconditioner, + const KeyInfo &keyInfo, const std::map &lambda) : + gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_( + lambda) { +} /*****************************************************************************/ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { @@ -67,7 +79,7 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { /* substract A*x */ Vector Ax = Vector::Zero(r.rows(), 1); multiply(x, Ax); - r -= Ax ; + r -= Ax; } /*****************************************************************************/ @@ -75,7 +87,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ // Build a VectorValues for Vector x - VectorValues vvX = buildVectorValues(x,keyInfo_); + VectorValues vvX = buildVectorValues(x, keyInfo_); // VectorValues form of A'Ax for multiplyHessianAdd VectorValues vvAtAx; @@ -99,31 +111,33 @@ void GaussianFactorGraphSystem::getb(Vector &b) const { } /**********************************************************************************/ -void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const { +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, + Vector &y) const { // For a preconditioner M = L*L^T // Calculate y = L^{-1} x preconditioner_.solve(x, y); } /**********************************************************************************/ -void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const { +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, + Vector &y) const { // For a preconditioner M = L*L^T // Calculate y = L^{-T} x preconditioner_.transposeSolve(x, y); } /**********************************************************************************/ -VectorValues buildVectorValues(const Vector &v, - const Ordering &ordering, - const map & dimensions) { +VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, + const map & dimensions) { VectorValues result; DenseIndex offset = 0; - for ( size_t i = 0 ; i < ordering.size() ; ++i ) { + for (size_t i = 0; i < ordering.size(); ++i) { const Key &key = ordering[i]; map::const_iterator it = dimensions.find(key); - if ( it == dimensions.end() ) { - throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions"); + if (it == dimensions.end()) { + throw invalid_argument( + "buildVectorValues: inconsistent ordering and dimensions"); } const size_t dim = it->second; result.insert(key, v.segment(offset, dim)); @@ -137,7 +151,8 @@ VectorValues buildVectorValues(const Vector &v, VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { - result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); + result.insert(item.first, + v.segment(item.second.colstart(), item.second.dim())); } return result; } diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 0201000ad..f5b278ae5 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -1,20 +1,25 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.h - * - * Created on: Jan 31, 2012 - * Author: Yong-Dian Jian + * @file PCGSolver.h + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Jan 31, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ #pragma once -#include #include -#include -#include - -#include -#include -#include #include namespace gtsam { @@ -22,15 +27,19 @@ namespace gtsam { class GaussianFactorGraph; class KeyInfo; class Preconditioner; +class VectorValues; struct PreconditionerParameters; -/*****************************************************************************/ +/** + * Parameters for PCG + */ struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; typedef boost::shared_ptr shared_ptr; - PCGSolverParameters() {} + PCGSolverParameters() { + } virtual void print(std::ostream &os) const; @@ -42,8 +51,9 @@ public: boost::shared_ptr preconditioner_; }; -/*****************************************************************************/ -/* A virtual base class for the preconditioned conjugate gradient solver */ +/** + * A virtual base class for the preconditioned conjugate gradient solver + */ class GTSAM_EXPORT PCGSolver: public IterativeSolver { public: typedef IterativeSolver Base; @@ -57,7 +67,8 @@ protected: public: /* Interface to initialize a solver without a problem */ PCGSolver(const PCGSolverParameters &p); - virtual ~PCGSolver() {} + virtual ~PCGSolver() { + } using IterativeSolver::optimize; @@ -67,7 +78,9 @@ public: }; -/*****************************************************************************/ +/** + * System class needed for calling preconditionedConjugateGradient + */ class GTSAM_EXPORT GaussianFactorGraphSystem { public: @@ -97,13 +110,17 @@ public: void getb(Vector &b) const; }; -/* utility functions */ -/**********************************************************************************/ +/// @name utility functions +/// @{ + +/// Create VectorValues from a Vector VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const std::map & dimensions); -/**********************************************************************************/ +/// Create VectorValues from a Vector and a KeyInfo class VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); +/// @} + } diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h new file mode 100644 index 000000000..e5e545c36 --- /dev/null +++ b/gtsam/linear/RegularHessianFactor.h @@ -0,0 +1,214 @@ +/* ---------------------------------------------------------------------------- + + * 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 RegularHessianFactor.h + * @brief HessianFactor class with constant sized blocks + * @author Sungtae An + * @date March 2014 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +template +class RegularHessianFactor: public HessianFactor { + +public: + + typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix MatrixD; + + /** Construct an n-way factor. Gs contains the upper-triangle blocks of the + * quadratic term (the Hessian matrix) provided in row-order, gs the pieces + * of the linear vector term, and f the constant term. + */ + RegularHessianFactor(const std::vector& js, + const std::vector& Gs, const std::vector& gs, double f) : + HessianFactor(js, Gs, gs, f) { + checkInvariants(); + } + + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12, + const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) : + HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { + } + + /** Construct a ternary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, Key j3, + const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1, + const MatrixD& G22, const MatrixD& G23, const VectorD& g2, + const MatrixD& G33, const VectorD& g3, double f) : + HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) { + } + + /** Constructor with an arbitrary number of keys and with the augmented information matrix + * specified as a block matrix. */ + template + RegularHessianFactor(const KEYS& keys, + const SymmetricBlockMatrix& augmentedInformation) : + HessianFactor(keys, augmentedInformation) { + checkInvariants(); + } + + /// Construct from RegularJacobianFactor + RegularHessianFactor(const RegularJacobianFactor& jf) + : HessianFactor(jf) {} + + /// Construct from a GaussianFactorGraph + RegularHessianFactor(const GaussianFactorGraph& factors, + boost::optional scatter = boost::none) + : HessianFactor(factors, scatter) { + checkInvariants(); + } + +private: + + /// Check invariants after construction + void checkInvariants() { + if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D) + throw std::invalid_argument( + "RegularHessianFactor constructor was given non-regular factors"); + } + + // Use Eigen magic to access raw memory + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // Scratch space for multiplyHessianAdd + // According to link below this is thread-safe. + // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe + mutable std::vector y_; + +public: + + /** y += alpha * A'*A*x */ + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + HessianFactor::multiplyHessianAdd(alpha, x, y); + } + + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const double* x, + double* yvalues) const { + // Create a vector of temporary y_ values, corresponding to rows i + y_.resize(size()); + BOOST_FOREACH(VectorD & yi, y_) + yi.setZero(); + + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y_ values, to be added into yvalues after + VectorD xj(D); + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + Key key = keys_[j]; + const double* xj = x + key * D; + DenseIndex i = 0; + for (; i < j; ++i) + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + // blocks on the diagonal are only half + y_[i] += info_(j, j).selfadjointView() * ConstDMap(xj); + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex) size(); ++i) + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + Key key = keys_[i]; + DMap(yvalues + key * D) += alpha * y_[i]; + } + } + + /// Raw memory version, with offsets TODO document reasoning + void multiplyHessianAdd(double alpha, const double* x, double* yvalues, + std::vector offsets) const { + + // Create a vector of temporary y_ values, corresponding to rows i + y_.resize(size()); + BOOST_FOREACH(VectorD & yi, y_) + yi.setZero(); + + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y_ values, to be added into yvalues after + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + DenseIndex i = 0; + for (; i < j; ++i) + y_[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // blocks on the diagonal are only half + y_[i] += info_(j, j).selfadjointView() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex) size(); ++i) + y_[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) + DMap(yvalues + offsets[keys_[i]], + offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i]; + } + + /** Return the diagonal of the Hessian for this factor (raw memory version) */ + virtual void hessianDiagonal(double* d) const { + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + const Matrix& B = info_(pos, pos).selfadjointView(); + DMap(d + D * j) += B.diagonal(); + } + } + + /// Add gradient at zero to d TODO: is it really the goal to add ?? + virtual void gradientAtZero(double* d) const { + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + VectorD dj = -info_(pos, size()).knownOffDiagonal(); + DMap(d + D * j) += dj; + } + } + + /* ************************************************************************* */ + +}; +// end class RegularHessianFactor + +// traits +template struct traits > : public Testable< + RegularHessianFactor > { +}; + +} + diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h similarity index 55% rename from gtsam/slam/RegularJacobianFactor.h rename to gtsam/linear/RegularJacobianFactor.h index bb275ef3f..f954cba88 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/linear/RegularJacobianFactor.h @@ -21,25 +21,34 @@ #include #include #include -#include namespace gtsam { +/** + * JacobianFactor with constant sized blocks + * Provides raw memory access versions of linear operator. + * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD + */ template class RegularJacobianFactor: public JacobianFactor { private: - /** Use eigen magic to access raw memory */ + // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; typedef Eigen::Map ConstDMap; public: + /// Default constructor + RegularJacobianFactor() {} + /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the - * collection of keys and matrices making up the factor. */ + * collection of keys and matrices making up the factor. + * TODO Verify terms are regular + */ template RegularJacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : @@ -49,91 +58,68 @@ public: /** Constructor with arbitrary number keys, and where the augmented matrix is given all together * instead of in block terms. Note that only the active view of the provided augmented matrix * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed - * factor. */ + * factor. + * TODO Verify complies to regular + */ template RegularJacobianFactor(const KEYS& keys, - const VerticalBlockMatrix& augmentedMatrix, - const SharedDiagonal& sigmas = SharedDiagonal()) : + const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = + SharedDiagonal()) : JacobianFactor(keys, augmentedMatrix, sigmas) { } - /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x - * Note: this is not assuming a fixed dimension for the variables, - * but requires the vector accumulatedDims to tell the dimension of - * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, - * then accumulatedDims is [0 3 9 11 13] - * NOTE: size of accumulatedDims is size of keys + 1!! */ - void multiplyHessianAdd(double alpha, const double* x, double* y, - const std::vector& accumulatedDims) const { + using JacobianFactor::multiplyHessianAdd; - /// Use eigen magic to access raw memory - typedef Eigen::Matrix VectorD; - typedef Eigen::Map MapD; - typedef Eigen::Map ConstMapD; + /** y += alpha * A'*A*x */ + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + JacobianFactor::multiplyHessianAdd(alpha, x, y); + } + /** + * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) + * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way + */ + void multiplyHessianAdd(double alpha, const double* x, double* y) const { if (empty()) return; Vector Ax = zero(Ab_.rows()); - /// Just iterate over all A matrices and multiply in correct config part (looping over keys) - /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' - /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + // Just iterate over all A matrices and multiply in correct config part for (size_t pos = 0; pos < size(); ++pos) - { - Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); - } - /// Deal with noise properly, need to Double* whiten as we are dividing by variance + Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); + + // Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); } - /// multiply with alpha - Ax *= alpha; - - /// Again iterate over all A matrices and insert Ai^T into y - for (size_t pos = 0; pos < size(); ++pos){ - MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( - pos).transpose() * Ax; - } - } - - /** Raw memory access version of multiplyHessianAdd */ - void multiplyHessianAdd(double alpha, const double* x, double* y) const { - - if (empty()) return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } - // multiply with alpha Ax *= alpha; // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; poswhiten(column_k); dj(k) = dot(column_k, column_k); - }else{ + } else { dj(k) = Ab_(j).col(k).squaredNorm(); } } @@ -141,10 +127,13 @@ public: } } - /** Raw memory access version of gradientAtZero - * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n - */ - virtual void gradientAtZero(double* d) const { + /// Expose base class gradientAtZero + virtual VectorValues gradientAtZero() const { + return JacobianFactor::gradientAtZero(); + } + + /// Raw memory access version of gradientAtZero + void gradientAtZero(double* d) const { // Get vector b not weighted Vector b = getb(); @@ -156,19 +145,46 @@ public: } // Just iterate over all A matrices - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DVector dj; // gradient -= A'*b/sigma^2 // Computing with each column - for (size_t k = 0; k < D; ++k){ + for (size_t k = 0; k < D; ++k) { Vector column_k = Ab_(j).col(k); - dj(k) = -1.0*dot(b, column_k); + dj(k) = -1.0 * dot(b, column_k); } DMap(d + D * j) += dj; } } + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for (size_t pos = 0; pos < size(); ++pos) + DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E; + } + + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) + return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for (size_t pos = 0; pos < size(); ++pos) + Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); + + return model_ ? model_->whiten(Ax) : Ax; + } + }; +// end class RegularJacobianFactor -} - +}// end namespace gtsam diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp new file mode 100644 index 000000000..ed2af529f --- /dev/null +++ b/gtsam/linear/Scatter.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Scatter.cpp + * @author Richard Roberts + * @author Frank Dellaert + * @date June 2015 + */ + +#include +#include +#include + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +string SlotEntry::toString() const { + ostringstream oss; + oss << "SlotEntry: key=" << key << ", dim=" << dimension; + return oss.str(); +} + +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering) { + gttic(Scatter_Constructor); + + // If we have an ordering, pre-fill the ordered variables first + if (ordering) { + BOOST_FOREACH (Key key, *ordering) { + push_back(SlotEntry(key, 0)); + } + } + + // Now, find dimensions of variables and/or extend + BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { + if (!factor) continue; + + // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers + const JacobianFactor* asJacobian = dynamic_cast(factor.get()); + if (asJacobian && asJacobian->cols() <= 1) continue; + + // loop over variables + for (GaussianFactor::const_iterator variable = factor->begin(); + variable != factor->end(); ++variable) { + const Key key = *variable; + iterator it = find(key); // theoretically expensive, yet cache friendly + if (it!=end()) + it->dimension = factor->getDim(variable); + else + push_back(SlotEntry(key, factor->getDim(variable))); + } + } + + // To keep the same behavior as before, sort the keys after the ordering + iterator first = begin(); + if (ordering) first += ordering->size(); + if (first != end()) std::sort(first, end()); + + // Filter out keys with zero dimensions (if ordering had more keys) + erase(std::remove_if(begin(), end(), SlotEntry::Zero), end()); +} + +/* ************************************************************************* */ +FastVector::iterator Scatter::find(Key key) { + iterator it = begin(); + while(it != end()) { + if (it->key == key) + return it; + ++it; + } + return it; // end() +} + +/* ************************************************************************* */ + +} // gtsam diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h new file mode 100644 index 000000000..39bfa6b8d --- /dev/null +++ b/gtsam/linear/Scatter.h @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Scatter.h + * @brief Maps global variable indices to slot indices + * @author Richard Roberts + * @author Frank Dellaert + * @date June 2015 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +class GaussianFactorGraph; +class Ordering; + +/// One SlotEntry stores the slot index for a variable, as well its dim. +struct GTSAM_EXPORT SlotEntry { + Key key; + size_t dimension; + SlotEntry(Key _key, size_t _dimension) : key(_key), dimension(_dimension) {} + std::string toString() const; + friend bool operator<(const SlotEntry& p, const SlotEntry& q) { + return p.key < q.key; + } + static bool Zero(const SlotEntry& p) { return p.dimension==0;} +}; + +/** + * Scatter is an intermediate data structure used when building a HessianFactor + * incrementally, to get the keys in the right order. In spirit, it is a map + * from global variable indices to slot indices in the union of involved + * variables. We also include the dimensionality of the variable. + */ +class Scatter : public FastVector { + public: + /// Constructor + Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering = boost::none); + + private: + + /// Find the SlotEntry with the right key (linear time worst case) + iterator find(Key key); +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index f5ee4ddfc..ee2447d2f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -15,20 +15,40 @@ * @author: Frank Dellaert */ -#include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include -#include #include +#include +#include +#include +#include + +#include +#include +#include #include +#include #include -#include +#include +#include +#include +#include // accumulate #include #include +#include +#include #include #include diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 86bee2188..e74b92df1 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -17,13 +17,31 @@ #pragma once -#include #include +#include +#include #include #include #include +#include +#include +#include +#include +#include + +#include #include +#include +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ + namespace gtsam { // Forward declarations @@ -44,7 +62,7 @@ namespace gtsam { private: friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(index_); ar & BOOST_SERIALIZATION_NVP(weight_); } @@ -70,7 +88,7 @@ namespace gtsam { Subgraph(const std::vector &indices) ; inline const Edges& edges() const { return edges_; } - inline const size_t size() const { return edges_.size(); } + inline size_t size() const { return edges_.size(); } EdgeIndices edgeIndices() const; iterator begin() { return edges_.begin(); } @@ -85,7 +103,7 @@ namespace gtsam { private: friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(edges_); } }; diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 6f10d04ad..3c7256c29 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -9,73 +9,81 @@ * -------------------------------------------------------------------------- */ -#include -#include +/** + * @file SubgraphSolver.cpp + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + +#include #include #include #include #include -#include -#include -#include #include -#include -#include -#include using namespace std; namespace gtsam { /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(gfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(*jfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, Ab2); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, - const Parameters ¶meters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) : -parameters_(parameters), ordering_(ordering) -{ + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, Ab2); } /**************************************************************************************************/ VectorValues SubgraphSolver::optimize() { - VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); + VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } @@ -86,29 +94,33 @@ VectorValues SubgraphSolver::optimize(const VectorValues &initial) { } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) -{ - GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), - Ab2 = boost::make_shared(); +void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { + GaussianFactorGraph::shared_ptr Ab1 = + boost::make_shared(), Ab2 = boost::make_shared< + GaussianFactorGraph>(); - boost::tie(Ab1, Ab2) = splitGraph(jfg) ; + boost::tie(Ab1, Ab2) = splitGraph(jfg); if (parameters_.verbosity()) - cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() + << " factors" << endl; - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) -{ - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); +void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph::shared_ptr &Ab2) { + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -boost::tuple +boost::tuple // SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); @@ -116,39 +128,43 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { DSFVector D(n); GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); + GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); size_t t = 0; BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { - if ( gf->keys().size() > 2 ) { - throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); + if (gf->keys().size() > 2) { + throw runtime_error( + "SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); } - bool augment = false ; + bool augment = false; /* check whether this factor should be augmented to the "tree" graph */ - if ( gf->keys().size() == 1 ) augment = true; + if (gf->keys().size() == 1) + augment = true; else { - const Key u = gf->keys()[0], v = gf->keys()[1], - u_root = D.findSet(u), v_root = D.findSet(v); - if ( u_root != v_root ) { - t++; augment = true ; + const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u), + v_root = D.findSet(v); + if (u_root != v_root) { + t++; + augment = true; D.makeUnionInPlace(u_root, v_root); } } - if ( augment ) At->push_back(gf); - else Ac->push_back(gf); + if (augment) + At->push_back(gf); + else + Ac->push_back(gf); } return boost::tie(At, Ac); } /****************************************************************************/ -VectorValues SubgraphSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial -) { return VectorValues(); } +VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { + return VectorValues(); +} } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index ac8a9da87..318c029f3 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -9,27 +9,37 @@ * -------------------------------------------------------------------------- */ +/** + * @file SubgraphSolver.h + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + #pragma once - #include -#include -#include -#include namespace gtsam { - // Forward declarations - class GaussianFactorGraph; - class GaussianBayesNet; - class SubgraphPreconditioner; +// Forward declarations +class GaussianFactorGraph; +class GaussianBayesNet; +class SubgraphPreconditioner; -class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { +class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; - SubgraphSolverParameters() : Base() {} - void print() const { Base::print(); } - virtual void print(std::ostream &os) const { Base::print(os); } + SubgraphSolverParameters() : + Base() { + } + void print() const { + Base::print(); + } + virtual void print(std::ostream &os) const { + Base::print(os); + } }; /** @@ -53,8 +63,7 @@ public: * * \nosubgrouping */ - -class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { +class GTSAM_EXPORT SubgraphSolver: public IterativeSolver { public: typedef SubgraphSolverParameters Parameters; @@ -62,41 +71,64 @@ public: protected: Parameters parameters_; Ordering ordering_; - boost::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object public: - /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ - SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &A, const Parameters ¶meters, const Ordering& ordering); - /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ - SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Ab1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, + const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &A, + const Parameters ¶meters, const Ordering& ordering); + + /** + * The user specify the subgraph part and the constraint part + * may throw exception if A1 is underdetermined + */ + SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, + const Parameters ¶meters, const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); /* The same as above, but the A1 is solved before */ - SubgraphSolver(const boost::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + SubgraphSolver(const boost::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering); - virtual ~SubgraphSolver() {} + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); - VectorValues optimize () ; - VectorValues optimize (const VectorValues &initial) ; + /// Destructor + virtual ~SubgraphSolver() { + } - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) ; + /// Optimize from zero + VectorValues optimize(); + + /// Optimize from given initial values + VectorValues optimize(const VectorValues &initial); + + /** Interface that IterativeSolver subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial); protected: void initialize(const GaussianFactorGraph &jfg); - void initialize(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2); + void initialize(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2); - boost::tuple, boost::shared_ptr > - splitGraph(const GaussianFactorGraph &gfg) ; + boost::tuple, + boost::shared_ptr > + splitGraph(const GaussianFactorGraph &gfg); }; } // namespace gtsam diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 664fcf3b7..33c62cfb6 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -66,6 +66,18 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + VectorValues::iterator VectorValues::insert(const std::pair& key_value) { + // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as + // it is inserted into the values_ map. + std::pair result = values_.insert(key_value); + if(!result.second) + throw std::invalid_argument( + "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) + + "' already in this VectorValues."); + return result.first; + } + /* ************************************************************************* */ void VectorValues::update(const VectorValues& values) { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index ce33116ab..d04d9faac 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -181,23 +181,14 @@ namespace gtsam { * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ iterator insert(Key j, const Vector& value) { - return insert(std::make_pair(j, value)); // Note only passing a reference to the Vector + return insert(std::make_pair(j, value)); } /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator insert(const std::pair& key_value) { - // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as - // it is inserted into the values_ map. - std::pair result = values_.insert(key_value); - if(!result.second) - throw std::invalid_argument( - "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) - + "' already in this VectorValues."); - return result.first; - } + iterator insert(const std::pair& key_value); /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be * inserted are already used. */ @@ -376,7 +367,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(values_); } }; // VectorValues definition diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index 3f62ed6d8..a4168af2d 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -17,24 +17,28 @@ */ #include - +#include #include +#include namespace gtsam { /* ************************************************************************* */ const char* IndeterminantLinearSystemException::what() const throw() { - if(!description_) + if(!description_) { description_ = String( "\nIndeterminant linear system detected while working near variable\n" - + boost::lexical_cast(j_) + ".\n" + + boost::lexical_cast(j_) + + + " (Symbol: " + boost::lexical_cast( + gtsam::DefaultKeyFormatter(gtsam::Symbol(j_))) + ").\n" "\n\ Thrown when a linear system is ill-posed. The most common cause for this\n\ error is having underconstrained variables. Mathematically, the system is\n\ underdetermined. See the GTSAM Doxygen documentation at\n\ http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for\n\ more information."); + } return description_->c_str(); } diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index 63bc61e80..32db27fc9 100644 --- a/gtsam/linear/linearExceptions.h +++ b/gtsam/linear/linearExceptions.h @@ -17,9 +17,8 @@ */ #pragma once -#include -#include -#include +#include +#include namespace gtsam { diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 05f8c3d2a..4f7c5c335 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -83,13 +83,30 @@ TEST( GaussianBayesTree, eliminate ) { GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering); + Scatter scatter(chain); + EXPECT_LONGS_EQUAL(4, scatter.size()); + EXPECT_LONGS_EQUAL(1, scatter.at(0).key); + EXPECT_LONGS_EQUAL(2, scatter.at(1).key); + EXPECT_LONGS_EQUAL(3, scatter.at(2).key); + EXPECT_LONGS_EQUAL(4, scatter.at(3).key); + Matrix two = (Matrix(1, 1) << 2.).finished(); Matrix one = (Matrix(1, 1) << 1.).finished(); GaussianBayesTree bayesTree_expected; bayesTree_expected.insertRoot( - MakeClique(GaussianConditional(pair_list_of(x3, (Matrix(2, 1) << 2., 0.).finished()) (x4, (Matrix(2, 1) << 2., 2.).finished()), 2, Vector2(2., 2.)), list_of - (MakeClique(GaussianConditional(pair_list_of(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.).finished()) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.)).finished()) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.)).finished()), 2, (Vector(2) << -2.*sqrt(2.), 0.).finished()))))); + MakeClique( + GaussianConditional( + pair_list_of(x3, (Matrix21() << 2., 0.).finished())( + x4, (Matrix21() << 2., 2.).finished()), 2, Vector2(2., 2.)), + list_of( + MakeClique( + GaussianConditional( + pair_list_of(x2, + (Matrix21() << -2. * sqrt(2.), 0.).finished())(x1, + (Matrix21() << -sqrt(2.), -sqrt(2.)).finished())(x3, + (Matrix21() << -sqrt(2.), sqrt(2.)).finished()), 2, + (Vector(2) << -2. * sqrt(2.), 0.).finished()))))); EXPECT(assert_equal(bayesTree_expected, bt)); } diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 3a2cd0fd4..10fb34988 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testCholeskyFactor.cpp + * @file testHessianFactor.cpp * @author Richard Roberts * @date Dec 15, 2010 */ @@ -38,6 +38,16 @@ using namespace gtsam; const double tol = 1e-5; +/* ************************************************************************* */ +TEST(HessianFactor, Slot) +{ + FastVector keys = list_of(2)(4)(1); + EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2)); + EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4)); + EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1)); + EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist +} + /* ************************************************************************* */ TEST(HessianFactor, emptyConstructor) { @@ -270,30 +280,69 @@ TEST(HessianFactor, ConstructorNWay) } /* ************************************************************************* */ -TEST(HessianFactor, CombineAndEliminate) -{ - Matrix A01 = (Matrix(3,3) << - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0).finished(); +TEST(HessianFactor, CombineAndEliminate1) { + Matrix3 A01 = 3.0 * I_3x3; + Vector3 b0(1, 0, 0); + + Matrix3 A21 = 4.0 * I_3x3; + Vector3 b2 = Vector3::Zero(); + + GaussianFactorGraph gfg; + gfg.add(1, A01, b0); + gfg.add(1, A21, b2); + + Matrix63 A1; + A1 << A01, A21; + Vector6 b; + b << b0, b2; + + // create a full, uneliminated version of the factor + JacobianFactor jacobian(1, A1, b); + + // Make sure combining works + HessianFactor hessian(gfg); + VectorValues v; + v.insert(1, Vector3(1, 0, 0)); + EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9); + EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6)); + EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9)); + EXPECT( + assert_equal(jacobian.augmentedInformation(), + hessian.augmentedInformation(), 1e-9)); + + // perform elimination on jacobian + Ordering ordering = list_of(1); + GaussianConditional::shared_ptr expectedConditional; + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + jacobian.eliminate(ordering); + + // Eliminate + GaussianConditional::shared_ptr actualConditional; + HessianFactor::shared_ptr actualHessian; + boost::tie(actualConditional, actualHessian) = // + EliminateCholesky(gfg, ordering); + + EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); + EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); + EXPECT( + assert_equal(expectedFactor->augmentedInformation(), + actualHessian->augmentedInformation(), 1e-9)); + EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); +} + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate2) { + Matrix A01 = I_3x3; Vector3 b0(1.5, 1.5, 1.5); Vector3 s0(1.6, 1.6, 1.6); - Matrix A10 = (Matrix(3,3) << - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0).finished(); - Matrix A11 = (Matrix(3,3) << - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0).finished(); + Matrix A10 = 2.0 * I_3x3; + Matrix A11 = -2.0 * I_3x3; Vector3 b1(2.5, 2.5, 2.5); Vector3 s1(2.6, 2.6, 2.6); - Matrix A21 = (Matrix(3,3) << - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0).finished(); + Matrix A21 = 3.0 * I_3x3; Vector3 b2(3.5, 3.5, 3.5); Vector3 s2(3.6, 3.6, 3.6); @@ -302,27 +351,45 @@ TEST(HessianFactor, CombineAndEliminate) gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector9 b; b << b1, b0, b2; - Vector9 sigmas; sigmas << s1, s0, s2; + Matrix93 A0, A1; + A0 << A10, Z_3x3, Z_3x3; + A1 << A11, A01, A21; + Vector9 b, sigmas; + b << b1, b0, b2; + sigmas << s1, s0, s2; // create a full, uneliminated version of the factor - JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + JacobianFactor jacobian(0, A0, 1, A1, b, + noiseModel::Diagonal::Sigmas(sigmas, true)); + + // Make sure combining works + HessianFactor hessian(gfg); + EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6)); + EXPECT( + assert_equal(jacobian.augmentedInformation(), + hessian.augmentedInformation(), 1e-9)); // perform elimination on jacobian + Ordering ordering = list_of(0); GaussianConditional::shared_ptr expectedConditional; - JacobianFactor::shared_ptr expectedRemainingFactor; - boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0))); + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + jacobian.eliminate(ordering); // Eliminate GaussianConditional::shared_ptr actualConditional; - HessianFactor::shared_ptr actualCholeskyFactor; - boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0))); + HessianFactor::shared_ptr actualHessian; + boost::tie(actualConditional, actualHessian) = // + EliminateCholesky(gfg, ordering); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); - EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6)); + VectorValues v; + v.insert(1, Vector3(1, 2, 3)); + EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); + EXPECT( + assert_equal(expectedFactor->augmentedInformation(), + actualHessian->augmentedInformation(), 1e-9)); + EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 7b2d59171..17ceaf5f0 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -540,9 +540,9 @@ TEST(JacobianFactor, EliminateQR) EXPECT(assert_equal(size_t(2), actualJF.keys().size())); EXPECT(assert_equal(Key(9), actualJF.keys()[0])); EXPECT(assert_equal(Key(11), actualJF.keys()[1])); - EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001)); - EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); - EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); + EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001)); EXPECT(!actualJF.get_model()); } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 5c6b2729d..b2afb1709 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -33,15 +33,16 @@ using namespace gtsam; using namespace noiseModel; using namespace boost::assign; -static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; -static Matrix R = (Matrix(3, 3) << +static const double kSigma = 2, s_1=1.0/kSigma, kVariance = kSigma*kSigma, prc = 1.0/kVariance; +static const Matrix R = (Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, 0.0, 0.0, s_1).finished(); -static Matrix Sigma = (Matrix(3, 3) << - var, 0.0, 0.0, - 0.0, var, 0.0, - 0.0, 0.0, var).finished(); +static const Matrix kCovariance = (Matrix(3, 3) << + kVariance, 0.0, 0.0, + 0.0, kVariance, 0.0, + 0.0, 0.0, kVariance).finished(); +static const Vector3 kSigmas(kSigma, kSigma, kSigma); //static double inf = numeric_limits::infinity(); @@ -53,15 +54,19 @@ TEST(NoiseModel, constructors) // Construct noise models vector m; - m.push_back(Gaussian::SqrtInformation(R)); - m.push_back(Gaussian::Covariance(Sigma)); - //m.push_back(Gaussian::Information(Q)); - m.push_back(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished())); - m.push_back(Diagonal::Variances((Vector(3) << var, var, var).finished())); - m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished())); - m.push_back(Isotropic::Sigma(3, sigma)); - m.push_back(Isotropic::Variance(3, var)); - m.push_back(Isotropic::Precision(3, prc)); + m.push_back(Gaussian::SqrtInformation(R,false)); + m.push_back(Gaussian::Covariance(kCovariance,false)); + m.push_back(Gaussian::Information(kCovariance.inverse(),false)); + m.push_back(Diagonal::Sigmas(kSigmas,false)); + m.push_back(Diagonal::Variances((Vector(3) << kVariance, kVariance, kVariance).finished(),false)); + m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished(),false)); + m.push_back(Isotropic::Sigma(3, kSigma,false)); + m.push_back(Isotropic::Variance(3, kVariance,false)); + m.push_back(Isotropic::Precision(3, prc,false)); + + // test kSigmas + BOOST_FOREACH(Gaussian::shared_ptr mi, m) + EXPECT(assert_equal(kSigmas,mi->sigmas())); // test whiten BOOST_FOREACH(Gaussian::shared_ptr mi, m) @@ -117,9 +122,9 @@ TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); - Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()), + Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << kSigma, kSigma, kSigma).finished()), d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); - Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), + Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma), i2 = Isotropic::Sigma(3, 0.7); EXPECT(assert_equal(*g1,*g1)); @@ -155,7 +160,7 @@ TEST(NoiseModel, ConstrainedConstructors ) Constrained::shared_ptr actual; size_t d = 3; double m = 100.0; - Vector sigmas = (Vector(3) << sigma, 0.0, 0.0).finished(); + Vector sigmas = (Vector(3) << kSigma, 0.0, 0.0).finished(); Vector mu = Vector3(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? @@ -182,7 +187,7 @@ TEST(NoiseModel, ConstrainedMixed ) { Vector feasible = Vector3(1.0, 0.0, 1.0), infeasible = Vector3(1.0, 1.0, 1.0); - Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma).finished()); + Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << kSigma, 0.0, kSigma).finished()); // NOTE: we catch constrained variables elsewhere, so whitening does nothing EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible))); EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible))); @@ -357,6 +362,44 @@ TEST(NoiseModel, robustNoise) DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8); } +/* ************************************************************************* */ +#define TEST_GAUSSIAN(gaussian)\ + EQUALITY(info, gaussian->information());\ + EQUALITY(cov, gaussian->covariance());\ + EXPECT(assert_equal(white, gaussian->whiten(e)));\ + EXPECT(assert_equal(e, gaussian->unwhiten(white)));\ + EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\ + Matrix A = R.inverse(); Vector b = e;\ + gaussian->WhitenSystem(A, b);\ + EXPECT(assert_equal(I, A));\ + EXPECT(assert_equal(white, b)); + +TEST(NoiseModel, NonDiagonalGaussian) +{ + Matrix3 R; + R << 6, 5, 4, 0, 3, 2, 0, 0, 1; + const Matrix3 info = R.transpose() * R; + const Matrix3 cov = info.inverse(); + const Vector3 e(1, 1, 1), white = R * e; + Matrix I = Matrix3::Identity(); + + + { + SharedGaussian gaussian = Gaussian::SqrtInformation(R); + TEST_GAUSSIAN(gaussian); + } + + { + SharedGaussian gaussian = Gaussian::Information(info); + TEST_GAUSSIAN(gaussian); + } + + { + SharedGaussian gaussian = Gaussian::Covariance(cov); + TEST_GAUSSIAN(gaussian); + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp new file mode 100644 index 000000000..f53803dd1 --- /dev/null +++ b/gtsam/linear/tests/testRegularHessianFactor.cpp @@ -0,0 +1,139 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRegularHessianFactor.cpp + * @author Frank Dellaert + * @date March 4, 2014 + */ + +#include +#include +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +/* ************************************************************************* */ +TEST(RegularHessianFactor, Constructors) +{ + // First construct a regular JacobianFactor + // 0.5*|x0 + x1 + x3 - [1;2]|^2 = 0.5*|A*x-b|^2, with A=[I I I] + Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2; + Vector2 b(1,2); + vector > terms; + terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3); + RegularJacobianFactor<2> jf(terms, b); + + // Test conversion from JacobianFactor + RegularHessianFactor<2> factor(jf); + + // 0.5*|A*x-b|^2 = 0.5*(Ax-b)'*(Ax-b) = 0.5*x'*A'A*x - x'*A'b + 0.5*b'*b + // Compare with comment in HessianFactor: E(x) = 0.5 x^T G x - x^T g + 0.5 f + // Hence G = I6, g A'*b = [b;b;b], and f = b'*b = 1+4 = 5 + Matrix G11 = I_2x2; + Matrix G12 = I_2x2; + Matrix G13 = I_2x2; + + Matrix G22 = I_2x2; + Matrix G23 = I_2x2; + + Matrix G33 = I_2x2; + + Vector2 g1 = b, g2 = b, g3 = b; + + double f = 5; + + // Test ternary constructor + RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); + EXPECT(assert_equal(factor,factor2)); + + // Test n-way constructor + vector keys; keys += 0, 1, 3; + vector Gs; Gs += G11, G12, G13, G22, G23, G33; + vector gs; gs += g1, g2, g3; + RegularHessianFactor<2> factor3(keys, Gs, gs, f); + EXPECT(assert_equal(factor, factor3)); + + // Test constructor from Gaussian Factor Graph + GaussianFactorGraph gfg; + gfg += jf; + RegularHessianFactor<2> factor4(gfg); + EXPECT(assert_equal(factor, factor4)); + GaussianFactorGraph gfg2; + gfg2 += factor; + RegularHessianFactor<2> factor5(gfg); + EXPECT(assert_equal(factor, factor5)); + + // Test constructor from Information matrix + Matrix info = factor.augmentedInformation(); + vector dims; dims += 2, 2, 2; + SymmetricBlockMatrix sym(dims, info, true); + RegularHessianFactor<2> factor6(keys, sym); + EXPECT(assert_equal(factor, factor6)); + + // multiplyHessianAdd: + { + // brute force + Matrix AtA = factor.information(); + HessianFactor::const_iterator i1 = factor.begin(); + HessianFactor::const_iterator i2 = i1 + 1; + Vector X(6); X << 1,2,3,4,5,6; + Vector Y(6); Y << 9, 12, 9, 12, 9, 12; + EXPECT(assert_equal(Y,AtA*X)); + + VectorValues x = map_list_of + (0, Vector2(1,2)) + (1, Vector2(3,4)) + (3, Vector2(5,6)); + + VectorValues expected; + expected.insert(0, Y.segment<2>(0)); + expected.insert(1, Y.segment<2>(2)); + expected.insert(3, Y.segment<2>(4)); + + // VectorValues version + double alpha = 1.0; + VectorValues actualVV; + actualVV.insert(0, Vector2::Zero()); + actualVV.insert(1, Vector2::Zero()); + actualVV.insert(3, Vector2::Zero()); + factor.multiplyHessianAdd(alpha, x, actualVV); + EXPECT(assert_equal(expected, actualVV)); + + // RAW ACCESS + Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12; + Vector fast_y = Vector8::Zero(); + double xvalues[8] = {1,2,3,4,0,0,5,6}; + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); + EXPECT(assert_equal(expected_y, fast_y)); + + // now, do it with non-zero y + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); + EXPECT(assert_equal(2*expected_y, fast_y)); + + // check some expressions + EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal())); + EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView())); + EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal())); + } +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/linear/tests/testRegularJacobianFactor.cpp similarity index 95% rename from gtsam/slam/tests/testRegularJacobianFactor.cpp rename to gtsam/linear/tests/testRegularJacobianFactor.cpp index b56b65d7b..b8c4aa689 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/linear/tests/testRegularJacobianFactor.cpp @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRegularJacobianFactor.cpp * @brief unit test regular jacobian factors @@ -5,13 +16,13 @@ * @date Nov 12, 2014 */ -#include -#include - -#include +#include #include #include #include +#include + +#include #include #include diff --git a/gtsam/linear/tests/testScatter.cpp b/gtsam/linear/tests/testScatter.cpp new file mode 100644 index 000000000..575f29c26 --- /dev/null +++ b/gtsam/linear/tests/testScatter.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testScatter.cpp + * @author Frank Dellaert + * @date June, 2015 + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate) { + static const size_t m = 3, n = 3; + Matrix A01 = + (Matrix(m, n) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(); + Vector3 b0(1.5, 1.5, 1.5); + Vector3 s0(1.6, 1.6, 1.6); + + Matrix A10 = + (Matrix(m, n) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0).finished(); + Matrix A11 = (Matrix(m, n) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0) + .finished(); + Vector3 b1(2.5, 2.5, 2.5); + Vector3 s1(2.6, 2.6, 2.6); + + Matrix A21 = + (Matrix(m, n) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished(); + Vector3 b2(3.5, 3.5, 3.5); + Vector3 s2(3.6, 3.6, 3.6); + + GaussianFactorGraph gfg; + gfg.add(X(1), A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(X(0), A10, X(1), A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(X(1), A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + Scatter scatter(gfg); + EXPECT_LONGS_EQUAL(2, scatter.size()); + EXPECT(assert_equal(X(0), scatter.at(0).key)); + EXPECT(assert_equal(X(1), scatter.at(1).key)); + EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension); + EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 42bcb8027..fbf7d51a1 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -97,7 +97,7 @@ public: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); } @@ -179,7 +179,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 08b75c00d..b61a861d6 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -122,7 +122,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -206,7 +206,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1c22bab9a..3547719ac 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -33,19 +33,20 @@ using namespace std; //------------------------------------------------------------------------------ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, - const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration), - biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), - biasAccOmegaInit_(biasAccOmegaInit) -{ + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, + const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, + const bool use2ndOrderIntegration) : + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_( + biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_( + biasAccOmegaInit) { preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ +void CombinedImuFactor::CombinedPreintegratedMeasurements::print( + const string& s) const { PreintegrationBase::print(s); cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; @@ -54,31 +55,35 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s } //------------------------------------------------------------------------------ -bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ - return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol) - && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol) - &&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) +bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals( + const CombinedPreintegratedMeasurements& expected, double tol) const { + return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, + tol) + && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, + tol) + && equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ +void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, - double deltaT, boost::optional body_P_sensor, + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor, boost::optional F_test, boost::optional G_test) { // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + correctedAcc, correctedOmega, body_P_sensor); const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr @@ -86,7 +91,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, + deltaT); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we @@ -98,11 +104,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); // Single Jacobians to propagate covariance - Matrix3 H_vel_biasacc = - R_i * deltaT; - Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT; + Matrix3 H_vel_biasacc = -R_i * deltaT; + Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(15,15); + Matrix F(15, 15); // for documentation: // F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, // Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, @@ -110,45 +116,47 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; F.setZero(); - F.block<9,9>(0,0) = F_9x9; - F.block<6,6>(9,9) = I_6x6; - F.block<3,3>(3,9) = H_vel_biasacc; - F.block<3,3>(6,12) = H_angles_biasomega; + F.block<9, 9>(0, 0) = F_9x9; + F.block<6, 6>(9, 9) = I_6x6; + F.block<3, 3>(3, 9) = H_vel_biasacc; + F.block<3, 3>(6, 12) = H_angles_biasomega; // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Matrix G_measCov_Gt = Matrix::Zero(15,15); + Matrix G_measCov_Gt = Matrix::Zero(15, 15); // BLOCK DIAGONAL TERMS - G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance(); - G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * - (accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) * - (H_vel_biasacc.transpose()); - G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * - (gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) * - (H_angles_biasomega.transpose()); - G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_; - G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_; + G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance(); + G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc) + * (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0)) + * (H_vel_biasacc.transpose()); + G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega) + * (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3)) + * (H_angles_biasomega.transpose()); + G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_; + G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_; // OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose(); - G_measCov_Gt.block<3,3>(3,6) = block23; - G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); + Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0) + * H_angles_biasomega.transpose(); + G_measCov_Gt.block<3, 3>(3, 6) = block23; + G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; // F_test and G_test are used for testing purposes and are not needed by the factor - if(F_test){ - F_test->resize(15,15); + if (F_test) { + F_test->resize(15, 15); (*F_test) << F; } - if(G_test){ - G_test->resize(15,21); + if (G_test) { + G_test->resize(15, 21); // This is for testing & documentation ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, - Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; + (*G_test) << // + I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, // + Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, // + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; } } @@ -156,16 +164,22 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, Z_6x6) { +} //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, +CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, + Key vel_j, Key bias_i, Key bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), - _PIM_(preintegratedMeasurements) {} + Base( + noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, + vel_j, bias_i, bias_j), ImuFactorBase(gravity, omegaCoriolis, + body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -174,13 +188,11 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { } //------------------------------------------------------------------------------ -void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," +void CombinedImuFactor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "CombinedImuFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; ImuFactorBase::print(""); _PIM_.print(" preintegrated measurements:"); @@ -188,11 +200,11 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) } //------------------------------------------------------------------------------ -bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && _PIM_.equals(e->_PIM_, tol) - && ImuFactorBase::equals(*e, tol); +bool CombinedImuFactor::equals(const NonlinearFactor& expected, + double tol) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -220,39 +232,39 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, // if we need the jacobians if (H1) { H1->resize(15, 6); - H1->block < 9, 6 > (0, 0) = D_r_pose_i; + H1->block<9, 6>(0, 0) = D_r_pose_i; // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] - H1->block < 6, 6 > (9, 0) = Z_6x6; + H1->block<6, 6>(9, 0) = Z_6x6; } if (H2) { H2->resize(15, 3); - H2->block < 9, 3 > (0, 0) = D_r_vel_i; + H2->block<9, 3>(0, 0) = D_r_vel_i; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H2->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); + H2->block<6, 3>(9, 0) = Matrix::Zero(6, 3); } if (H3) { H3->resize(15, 6); - H3->block < 9, 6 > (0, 0) = D_r_pose_j; + H3->block<9, 6>(0, 0) = D_r_pose_j; // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] - H3->block < 6, 6 > (9, 0) = Z_6x6; + H3->block<6, 6>(9, 0) = Z_6x6; } if (H4) { H4->resize(15, 3); - H4->block < 9, 3 > (0, 0) = D_r_vel_j; + H4->block<9, 3>(0, 0) = D_r_vel_j; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H4->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); + H4->block<6, 3>(9, 0) = Matrix::Zero(6, 3); } if (H5) { H5->resize(15, 6); - H5->block < 9, 6 > (0, 0) = D_r_bias_i; + H5->block<9, 6>(0, 0) = D_r_bias_i; // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] - H5->block < 6, 6 > (9, 0) = Hbias_i; + H5->block<6, 6>(9, 0) = Hbias_i; } if (H6) { H6->resize(15, 6); - H6->block < 9, 6 > (0, 0) = Matrix::Zero(9, 6); + H6->block<9, 6>(0, 0) = Matrix::Zero(9, 6); // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] - H6->block < 6, 6 > (9, 0) = Hbias_j; + H6->block<6, 6>(9, 0) = Hbias_j; } // overall error diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a7c6ecd39..a65a4d3f7 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -65,7 +65,8 @@ namespace gtsam { * the correlation between the bias uncertainty and the preintegrated * measurements uncertainty. */ -class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ +class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase { public: /** @@ -82,11 +83,11 @@ public: protected: - Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk + Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration + Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration - Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements + Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation ///< between the preintegrated measurements and the biases @@ -105,16 +106,20 @@ public: * @param use2ndOrderIntegration Controls the order of integration * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) */ - CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false); + const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = + false); /// print void print(const std::string& s = "Preintegrated Measurements:") const; /// equals - bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const; + bool equals(const CombinedPreintegratedMeasurements& expected, double tol = + 1e-9) const; /// Re-initialize CombinedPreintegratedMeasurements void resetIntegration(); @@ -126,19 +131,23 @@ public: * @param deltaT Time interval between two consecutive IMU measurements * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) */ - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor = boost::none, - boost::optional F_test = boost::none, boost::optional G_test = boost::none); + boost::optional F_test = boost::none, + boost::optional G_test = boost::none); /// methods to access class variables - Matrix preintMeasCov() const { return preintMeasCov_;} + Matrix preintMeasCov() const { + return preintMeasCov_; + } private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } @@ -147,7 +156,8 @@ public: private: typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; + typedef NoiseModelFactor6 Base; CombinedPreintegratedMeasurements _PIM_; @@ -177,12 +187,15 @@ public: * @param body_P_sensor Optional pose of the sensor frame in the body frame * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, + Key bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); - virtual ~CombinedImuFactor() {} + virtual ~CombinedImuFactor() { + } /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; @@ -190,42 +203,59 @@ public: /** implement functions needed for Testable */ /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** Access the preintegrated measurements. */ const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return _PIM_; } + return _PIM_; + } /** implement functions needed to derive from Factor */ /// vector of errors - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const; + boost::optional H1 = boost::none, boost::optional H2 = + boost::none, boost::optional H3 = boost::none, + boost::optional H4 = boost::none, boost::optional H5 = + boost::none, boost::optional H6 = boost::none) const; + + /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) { + PoseVelocityBias PVB( + PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected_out, + deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; + } private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; // class CombinedImuFactor +}; +// class CombinedImuFactor typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 6902f81f1..cd5fa71d2 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -103,7 +103,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index ce93bd740..571fb7249 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -17,8 +17,10 @@ #pragma once +#include +#include +#include #include -#include /* * NOTES: @@ -36,57 +38,63 @@ namespace gtsam { /// All bias models live in the imuBias namespace namespace imuBias { - class ConstantBias { - private: - Vector3 biasAcc_; - Vector3 biasGyro_; +class ConstantBias { +private: + Vector3 biasAcc_; + Vector3 biasGyro_; - public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 6; +public: + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 6; - ConstantBias(): - biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { - } + ConstantBias() : + biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { + } - ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro): + ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) : biasAcc_(biasAcc), biasGyro_(biasGyro) { - } + } - ConstantBias(const Vector6& v): + ConstantBias(const Vector6& v) : biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { + } + + /** return the accelerometer and gyro biases in a single vector */ + Vector6 vector() const { + Vector6 v; + v << biasAcc_, biasGyro_; + return v; + } + + /** get accelerometer bias */ + const Vector3& accelerometer() const { + return biasAcc_; + } + + /** get gyroscope bias */ + const Vector3& gyroscope() const { + return biasGyro_; + } + + /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ + Vector3 correctAccelerometer(const Vector3& measurement, + boost::optional H = boost::none) const { + if (H) { + H->resize(3, 6); + (*H) << -Matrix3::Identity(), Matrix3::Zero(); } + return measurement - biasAcc_; + } - /** return the accelerometer and gyro biases in a single vector */ - Vector6 vector() const { - Vector6 v; - v << biasAcc_, biasGyro_; - return v; - } - - /** get accelerometer bias */ - const Vector3& accelerometer() const { return biasAcc_; } - - /** get gyroscope bias */ - const Vector3& gyroscope() const { return biasGyro_; } - - /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ - Vector3 correctAccelerometer(const Vector3& measurement, boost::optional H=boost::none) const { - if (H) { - H->resize(3, 6); - (*H) << -Matrix3::Identity(), Matrix3::Zero(); - } - return measurement - biasAcc_; - } - - /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ - Vector3 correctGyroscope(const Vector3& measurement, boost::optional H=boost::none) const { - if (H) { - H->resize(3, 6); - (*H) << Matrix3::Zero(), -Matrix3::Identity(); - } - return measurement - biasGyro_; + /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ + Vector3 correctGyroscope(const Vector3& measurement, + boost::optional H = boost::none) const { + if (H) { + H->resize(3, 6); + (*H) << Matrix3::Zero(), -Matrix3::Identity(); } + return measurement - biasGyro_; + } // // H1: Jacobian w.r.t. IMUBias // // H2: Jacobian w.r.t. pose @@ -118,76 +126,91 @@ namespace imuBias { //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; // } - /// @} - /// @name Testable - /// @{ +/// @} +/// @name Testable +/// @{ - /// print with optional string - void print(const std::string& s = "") const { - // explicit printing for now. - std::cout << s + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl; - std::cout << s + ".biasGyro [" << biasGyro_.transpose() << "]" << std::endl; - } +/// print with optional string + void print(const std::string& s = "") const { + // explicit printing for now. + std::cout << s + ".Acc [" << biasAcc_.transpose() << "]" << std::endl; + std::cout << s + ".Gyro [" << biasGyro_.transpose() << "]" << std::endl; + } - /** equality up to tolerance */ - inline bool equals(const ConstantBias& expected, double tol=1e-5) const { - return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) - && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); - } + /** equality up to tolerance */ + inline bool equals(const ConstantBias& expected, double tol = 1e-5) const { + return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) + && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); + } - /// @} - /// @name Group - /// @{ + /// @} + /// @name Group + /// @{ - /** identity for group operation */ - static ConstantBias identity() { return ConstantBias(); } + /** identity for group operation */ + static ConstantBias identity() { + return ConstantBias(); + } - /** inverse */ - inline ConstantBias operator-() const { - return ConstantBias(-biasAcc_, -biasGyro_); - } + /** inverse */ + inline ConstantBias operator-() const { + return ConstantBias(-biasAcc_, -biasGyro_); + } - /** addition */ - ConstantBias operator+(const ConstantBias& b) const { - return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); - } + /** addition */ + ConstantBias operator+(const ConstantBias& b) const { + return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); + } - /** subtraction */ - ConstantBias operator-(const ConstantBias& b) const { - return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_); - } + /** subtraction */ + ConstantBias operator-(const ConstantBias& b) const { + return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_); + } - /// @} + /// @} - /// @name Deprecated - /// @{ - ConstantBias inverse() { return -(*this);} - ConstantBias compose(const ConstantBias& q) { return (*this)+q;} - ConstantBias between(const ConstantBias& q) { return q-(*this);} - Vector6 localCoordinates(const ConstantBias& q) { return between(q).vector();} - ConstantBias retract(const Vector6& v) {return compose(ConstantBias(v));} - static Vector6 Logmap(const ConstantBias& p) {return p.vector();} - static ConstantBias Expmap(const Vector6& v) { return ConstantBias(v);} - /// @} + /// @name Deprecated + /// @{ + ConstantBias inverse() { + return -(*this); + } + ConstantBias compose(const ConstantBias& q) { + return (*this) + q; + } + ConstantBias between(const ConstantBias& q) { + return q - (*this); + } + Vector6 localCoordinates(const ConstantBias& q) { + return between(q).vector(); + } + ConstantBias retract(const Vector6& v) { + return compose(ConstantBias(v)); + } + static Vector6 Logmap(const ConstantBias& p) { + return p.vector(); + } + static ConstantBias Expmap(const Vector6& v) { + return ConstantBias(v); + } + /// @} - private: +private: - /// @name Advanced Interface - /// @{ + /// @name Advanced Interface + /// @{ - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this); - ar & BOOST_SERIALIZATION_NVP(biasAcc_); - ar & BOOST_SERIALIZATION_NVP(biasGyro_); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this); + ar & BOOST_SERIALIZATION_NVP(biasAcc_); + ar & BOOST_SERIALIZATION_NVP(biasGyro_); + } - /// @} + /// @} - }; // ConstantBias class +}; // ConstantBias class } // namespace imuBias template<> @@ -197,4 +220,3 @@ struct traits : public internal::VectorSpace< } // namespace gtsam - diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 81120b7c6..01de5a8f3 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -33,29 +33,28 @@ using namespace std; //------------------------------------------------------------------------------ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, - measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration) -{ + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration) { preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { PreintegrationBase::print(s); - cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ -bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { +bool ImuFactor::PreintegratedMeasurements::equals( + const PreintegratedMeasurements& expected, double tol) const { return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) - && PreintegrationBase::equals(expected, tol); + && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::resetIntegration(){ +void ImuFactor::PreintegratedMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } @@ -63,11 +62,12 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor, - OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { + boost::optional body_P_sensor, OptionalJacobian<9, 9> F_test, + OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + correctedAcc, correctedOmega, body_P_sensor); const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr @@ -87,34 +87,37 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise, - // as G and measurementCovariance are blockdiagonal matrices + // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise, + // as G and measurementCovariance are block-diagonal matrices preintMeasCov_ = F * preintMeasCov_ * F.transpose(); - preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT; - preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT; - preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; + preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT; + preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance() + * R_i.transpose() * deltaT; + preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega + * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; Matrix3 F_pos_noiseacc; - if(use2ndOrderIntegration()){ + if (use2ndOrderIntegration()) { F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT; - preintMeasCov_.block<3,3>(0,0) += (1/deltaT) * F_pos_noiseacc * accelerometerCovariance() * F_pos_noiseacc.transpose(); + preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc + * accelerometerCovariance() * F_pos_noiseacc.transpose(); Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT - preintMeasCov_.block<3,3>(0,3) += temp; - preintMeasCov_.block<3,3>(3,0) += temp.transpose(); + preintMeasCov_.block<3, 3>(0, 3) += temp; + preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); } // F_test and G_test are given as output for testing purposes and are not needed by the factor - if(F_test){ // This in only for testing + if (F_test) { // This in only for testing (*F_test) << F; } - if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise - if(!use2ndOrderIntegration()) + if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise + if (!use2ndOrderIntegration()) F_pos_noiseacc = Z_3x3; // intNoise accNoise omegaNoise - (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos - Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle + (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos + Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle } } @@ -122,19 +125,20 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) { +} //------------------------------------------------------------------------------ -ImuFactor::ImuFactor( - Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, +ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor, - const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), - pose_i, vel_i, pose_j, vel_j, bias), - ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), - _PIM_(preintegratedMeasurements) {} + boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : + Base( + noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, + vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, + use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { @@ -144,23 +148,21 @@ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { //------------------------------------------------------------------------------ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "ImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << ")\n"; + cout << s << "ImuFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) + << ")\n"; ImuFactorBase::print(""); _PIM_.print(" preintegrated measurements:"); - this->noiseModel_->print(" noise model: "); + // Print standard deviations on covariance only + cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl; } //------------------------------------------------------------------------------ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && _PIM_.equals(e->_PIM_, tol) - && ImuFactorBase::equals(*e, tol); + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b91c76ade..50e6c835f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -58,7 +58,8 @@ namespace gtsam { * (which are usually slowly varying quantities), which is up to the caller. * See also CombinedImuFactor for a class that does this for you. */ -class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { +class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { public: /** @@ -75,7 +76,7 @@ public: protected: - Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). public: @@ -90,14 +91,17 @@ public: * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) */ PreintegratedMeasurements(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false); + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const bool use2ndOrderIntegration = false); /// print void print(const std::string& s = "Preintegrated Measurements:") const; /// equals - bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const; + bool equals(const PreintegratedMeasurements& expected, + double tol = 1e-9) const; /// Re-initialize PreintegratedMeasurements void resetIntegration(); @@ -106,36 +110,41 @@ public: * Add a single IMU measurement to the preintegration. * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) - * @param deltaT Time interval between two consecutive IMU measurements + * @param deltaT Time interval between this and the last IMU measurement * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) * @param Fout, Gout Jacobians used internally (only needed for testing) */ - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor = boost::none, - OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); + OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = + boost::none); /// methods to access class variables - Matrix preintMeasCov() const { return preintMeasCov_;} + Matrix preintMeasCov() const { + return preintMeasCov_; + } private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; - private: +private: typedef ImuFactor This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactor5 Base; PreintegratedMeasurements _PIM_; - public: +public: /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 @@ -163,9 +172,11 @@ public: ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); - virtual ~ImuFactor() {} + virtual ~ImuFactor() { + } /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; @@ -173,41 +184,58 @@ public: /** implement functions needed for Testable */ /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** Access the preintegrated measurements. */ const PreintegratedMeasurements& preintegratedMeasurements() const { - return _PIM_; } + return _PIM_; + } /** implement functions needed to derive from Factor */ /// vector of errors - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const; + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, + boost::optional H3 = boost::none, boost::optional H4 = + boost::none, boost::optional H5 = boost::none) const; - private: + /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const PreintegratedMeasurements& PIM, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) { + PoseVelocityBias PVB( + PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected_out, + deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; + } + +private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor5", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; // class ImuFactor +}; +// class ImuFactor typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 1b7919a82..1e4e51220 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -33,15 +33,16 @@ protected: Vector3 gravity_; Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect public: /** Default constructor - only use for serialization */ ImuFactorBase() : - gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), - body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_( + boost::none), use2ndOrderCoriolis_(false) { + } /** * Default constructor, stores basic quantities required by the Imu factors @@ -51,21 +52,29 @@ public: * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect */ ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : - gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) : + gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_( + body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) { + } /// Methods to access class variables - const Vector3& gravity() const { return gravity_; } - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + const Vector3& gravity() const { + return gravity_; + } + const Vector3& omegaCoriolis() const { + return omegaCoriolis_; + } /// Needed for testable //------------------------------------------------------------------------------ - void print(const std::string& s) const { + void print(const std::string& /*s*/) const { std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl; - if(this->body_P_sensor_) + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" + << std::endl; + std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" + << std::endl; + if (this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); } @@ -73,10 +82,11 @@ public: //------------------------------------------------------------------------------ bool equals(const ImuFactorBase& expected, double tol) const { return equal_with_abs_tol(gravity_, expected.gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) - && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) - && ((!body_P_sensor_ && !expected.body_P_sensor_) || - (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); + && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) + && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) + && ((!body_P_sensor_ && !expected.body_P_sensor_) + || (body_P_sensor_ && expected.body_P_sensor_ + && body_P_sensor_->equals(*expected.body_P_sensor_))); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 67deb2d99..2379f58af 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -32,12 +32,12 @@ namespace gtsam { */ class PreintegratedRotation { - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j /// Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delRdelBiasOmega_; - Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements + Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements public: @@ -45,35 +45,46 @@ public: * Default constructor, initializes the variables in the base class */ PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : - deltaRij_(Rot3()), deltaTij_(0.0), - delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {} + deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_( + measuredOmegaCovariance) { + } /// methods to access class variables - Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive - Vector3 thetaRij(OptionalJacobian<3,3> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive - const double& deltaTij() const{return deltaTij_;} - const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} - const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;} + Matrix3 deltaRij() const { + return deltaRij_.matrix(); + } // expensive + Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const { + return Rot3::Logmap(deltaRij_, H); + } // super-expensive + const double& deltaTij() const { + return deltaTij_; + } + const Matrix3& delRdelBiasOmega() const { + return delRdelBiasOmega_; + } + const Matrix3& gyroscopeCovariance() const { + return gyroscopeCovariance_; + } /// Needed for testable void print(const std::string& s) const { std::cout << s << std::endl; - std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl; - std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl; + std::cout << " deltaTij [" << deltaTij_ << "]" << std::endl; + std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl; } /// Needed for testable bool equals(const PreintegratedRotation& expected, double tol) const { return deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol) - && equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol); + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, + tol) + && equal_with_abs_tol(gyroscopeCovariance_, + expected.gyroscopeCovariance_, tol); } /// Re-initialize PreintegratedMeasurements - void resetIntegration(){ + void resetIntegration() { deltaRij_ = Rot3(); deltaTij_ = 0.0; delRdelBiasOmega_ = Z_3x3; @@ -81,7 +92,7 @@ public: /// Update preintegrated measurements void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, - OptionalJacobian<3, 3> H = boost::none){ + OptionalJacobian<3, 3> H = boost::none) { deltaRij_ = deltaRij_.compose(incrR, H, boost::none); deltaTij_ += deltaT; } @@ -90,15 +101,16 @@ public: * Update Jacobians to be used during preintegration * TODO: explain arguments */ - void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, - double deltaT) { + void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, + const Rot3& incrR, double deltaT) { const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - D_Rincr_integratedOmega * deltaT; } /// Return a bias corrected version of the integrated rotation - expensive Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { - return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); + return deltaRij_ * Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); } /// Get so<3> version of bias corrected rotation, with optional Jacobian @@ -111,7 +123,8 @@ public: if (H) { Matrix3 Jrinv_theta_bc; // This was done via an expmap, now we go *back* to so<3> - const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, + Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = // Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; @@ -131,7 +144,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp new file mode 100644 index 000000000..496569599 --- /dev/null +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -0,0 +1,357 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "PreintegrationBase.h" + +namespace gtsam { + +PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, + const bool use2ndOrderIntegration) + : PreintegratedRotation(measuredOmegaCovariance), + biasHat_(bias), + use2ndOrderIntegration_(use2ndOrderIntegration), + deltaPij_(Vector3::Zero()), + deltaVij_(Vector3::Zero()), + delPdelBiasAcc_(Z_3x3), + delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), + delVdelBiasOmega_(Z_3x3), + accelerometerCovariance_(measuredAccCovariance), + integrationCovariance_(integrationErrorCovariance) { +} + +/// Needed for testable +void PreintegrationBase::print(const std::string& s) const { + PreintegratedRotation::print(s); + std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; + biasHat_.print(" biasHat"); +} + +/// Needed for testable +bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { + return PreintegratedRotation::equals(other, tol) && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol) + && equal_with_abs_tol(accelerometerCovariance_, other.accelerometerCovariance_, tol) + && equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol); +} + +/// Re-initialize PreintegratedMeasurements +void PreintegrationBase::resetIntegration() { + PreintegratedRotation::resetIntegration(); + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; +} + +/// Update preintegrated measurements +void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc, + const Rot3& incrR, const double deltaT, + OptionalJacobian<9, 9> F) { + + const Matrix3 dRij = deltaRij(); // expensive + const Vector3 temp = dRij * correctedAcc * deltaT; + if (!use2ndOrderIntegration_) { + deltaPij_ += deltaVij_ * deltaT; + } else { + deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; + } + deltaVij_ += temp; + + Matrix3 R_i, F_angles_angles; + if (F) + R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij + updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0); + + if (F) { + const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; + Matrix3 F_pos_angles; + if (use2ndOrderIntegration_) + F_pos_angles = 0.5 * F_vel_angles * deltaT; + else + F_pos_angles = Z_3x3; + + // pos vel angle + *F << // + I_3x3, I_3x3 * deltaT, F_pos_angles, // pos + Z_3x3, I_3x3, F_vel_angles, // vel + Z_3x3, Z_3x3, F_angles_angles; // angle + } +} + +/// Update Jacobians to be used during preintegration +void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc, + const Matrix3& D_Rincr_integratedOmega, + const Rot3& incrR, double deltaT) { + const Matrix3 dRij = deltaRij(); // expensive + const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); + if (!use2ndOrderIntegration_) { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + } else { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; + delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5); + } + delVdelBiasAcc_ += -dRij * deltaT; + delVdelBiasOmega_ += temp; + update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); +} + +void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( + const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, boost::optional body_P_sensor) { + correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame + if (body_P_sensor) { + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc + - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } +} + +/// Predict state at time j +//------------------------------------------------------------------------------ +PoseVelocityBias PreintegrationBase::predict( + const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, + boost::optional deltaPij_biascorrected_out, + boost::optional deltaVij_biascorrected_out) const { + + const imuBias::ConstantBias biasIncr = bias_i - biasHat(); + const Vector3& biasAccIncr = biasIncr.accelerometer(); + const Vector3& biasOmegaIncr = biasIncr.gyroscope(); + + const Rot3& Rot_i = pose_i.rotation(); + const Matrix3 Rot_i_matrix = Rot_i.matrix(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + + delPdelBiasOmega() * biasOmegaIncr; + if (deltaPij_biascorrected_out) // if desired, store this + *deltaPij_biascorrected_out = deltaPij_biascorrected; + + const double dt = deltaTij(), dt2 = dt * dt; + Vector3 pos_j = pos_i + Rot_i_matrix * deltaPij_biascorrected + vel_i * dt + - omegaCoriolis.cross(vel_i) * dt2 // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * dt2; + + const Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + + delVdelBiasOmega() * biasOmegaIncr; + if (deltaVij_biascorrected_out) // if desired, store this + *deltaVij_biascorrected_out = deltaVij_biascorrected; + + Vector3 vel_j = Vector3( + vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term + + gravity * dt); + + if (use2ndOrderCoriolis) { + pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // 2nd order coriolis term for position + vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) + + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); + const Vector3 correctedOmega = biascorrectedOmega + - Rot_i_matrix.transpose() * omegaCoriolis * dt; // Coriolis term + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); + const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij); + + const Pose3 pose_j = Pose3(Rot_j, Point3(pos_j)); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant +} + +/// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const Vector3& gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis, + OptionalJacobian<9, 6> H1, + OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4, + OptionalJacobian<9, 6> H5) const { + + // We need the mismatch w.r.t. the biases used for preintegration + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3& Ri = pose_i.rotation(); + const Matrix3 Ri_transpose_matrix = Ri.transpose(); + + const Rot3& Rj = pose_j.rotation(); + const Vector3 pos_j = pose_j.translation().vector(); + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 deltaPij_biascorrected, deltaVij_biascorrected; + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected, + deltaVij_biascorrected); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fp = Ri_transpose_matrix * (pos_j - predictedState_j.pose.translation().vector()); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fv = Ri_transpose_matrix * (vel_j - predictedState_j.velocity); + + // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) + + /* ---------------------------------------------------------------------------------------------------- */ + // Get Get so<3> version of bias corrected rotation + // If H5 is asked for, we will need the Jacobian, which we store in H5 + // H5 will then be corrected below to take into account the Coriolis effect + Matrix3 D_cThetaRij_biasOmegaIncr; + const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, + H5 ? &D_cThetaRij_biasOmegaIncr : 0); + + // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration + const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; + + // Calculate Jacobians, matrices below needed only for some Jacobians... + Vector3 fR; + Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, Ritranspose_omegaCoriolisHat; + + if (H1 || H2) + Ritranspose_omegaCoriolisHat = Ri_transpose_matrix * skewSymmetric(omegaCoriolis); + + // This is done to save computation: we ask for the jacobians only when they are needed + const double dt = deltaTij(), dt2 = dt*dt; + Rot3 fRrot; + const Rot3 RiBetweenRj = Rot3(Ri_transpose_matrix) * Rj; + if (H1 || H2 || H3 || H4 || H5) { + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(RiBetweenRj); + fR = Rot3::Logmap(fRrot, D_fR_fRrot); + D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); + } else { + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(RiBetweenRj); + fR = Rot3::Logmap(fRrot); + } + if (H1) { + Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3; + if (use2ndOrderCoriolis) { + // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() + const Matrix3 temp = Ritranspose_omegaCoriolisHat + * (-Ritranspose_omegaCoriolisHat.transpose()); + dfPdPi += 0.5 * temp * dt2; + dfVdPi += temp * dt; + } + (*H1) << + // dfP/dRi + skewSymmetric(fp + deltaPij_biascorrected), + // dfP/dPi + dfPdPi, + // dfV/dRi + skewSymmetric(fv + deltaVij_biascorrected), + // dfV/dPi + dfVdPi, + // dfR/dRi + D_fR_fRrot * (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), + // dfR/dPi + Z_3x3; + } + if (H2) { + (*H2) << + // dfP/dVi + -Ri_transpose_matrix * dt + Ritranspose_omegaCoriolisHat * dt2, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + -Ri_transpose_matrix + 2 * Ritranspose_omegaCoriolisHat * dt, // Coriolis term + // dfR/dVi + Z_3x3; + } + if (H3) { + (*H3) << + // dfP/dPosej + Z_3x3, Ri_transpose_matrix * Rj.matrix(), + // dfV/dPosej + Matrix::Zero(3, 6), + // dfR/dPosej + D_fR_fRrot, Z_3x3; + } + if (H4) { + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + Ri_transpose_matrix, + // dfR/dVj + Z_3x3; + } + if (H5) { + // H5 by this point already contains 3*3 biascorrectedThetaRij derivative + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; + (*H5) << + // dfP/dBias + -delPdelBiasAcc(), -delPdelBiasOmega(), + // dfV/dBias + -delVdelBiasAcc(), -delVdelBiasOmega(), + // dfR/dBias + Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega); + } + Vector9 r; + r << fp, fv, fR; + return r; +} + +ImuBase::ImuBase() + : gravity_(Vector3(0.0, 0.0, 9.81)), + omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), + body_P_sensor_(boost::none), + use2ndOrderCoriolis_(false) { +} + +ImuBase::ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor, const bool use2ndOrderCoriolis) + : gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis) { +} + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index bd0fd62e0..f6b24e752 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -23,6 +23,9 @@ #include #include +#include +#include +#include namespace gtsam { @@ -34,9 +37,10 @@ struct PoseVelocityBias { Vector3 velocity; imuBias::ConstantBias bias; - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias) + : pose(_pose), + velocity(_velocity), + bias(_bias) { } }; @@ -48,22 +52,22 @@ struct PoseVelocityBias { */ class PreintegrationBase : public PreintegratedRotation { - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - bool use2ndOrderIntegration_; ///< Controls the order of integration + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + bool use2ndOrderIntegration_; ///< Controls the order of integration - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements - Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty + const Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements + const Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty /// (to compensate errors in Euler integration) -public: + public: /** * Default constructor, initializes the variables in the base class @@ -71,328 +75,92 @@ public: * @param use2ndOrderIntegration Controls the order of integration * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) */ - PreintegrationBase(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, - const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) : - PreintegratedRotation(measuredOmegaCovariance), - biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), - deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - accelerometerCovariance_(measuredAccCovariance), - integrationCovariance_(integrationErrorCovariance) {} + PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration); /// methods to access class variables - const bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;} - const Vector3& deltaPij() const {return deltaPij_;} - const Vector3& deltaVij() const {return deltaVij_;} - const imuBias::ConstantBias& biasHat() const { return biasHat_;} - Vector6 biasHatVector() const { return biasHat_.vector();} // expensive - const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} - const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} - const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} - const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;} - - const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;} - const Matrix3& integrationCovariance() const { return integrationCovariance_;} - - /// Needed for testable - void print(const std::string& s) const { - PreintegratedRotation::print(s); - std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl; - std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - biasHat_.print(" biasHat"); + bool use2ndOrderIntegration() const { + return use2ndOrderIntegration_; + } + const Vector3& deltaPij() const { + return deltaPij_; + } + const Vector3& deltaVij() const { + return deltaVij_; + } + const imuBias::ConstantBias& biasHat() const { + return biasHat_; + } + Vector6 biasHatVector() const { + return biasHat_.vector(); + } // expensive + const Matrix3& delPdelBiasAcc() const { + return delPdelBiasAcc_; + } + const Matrix3& delPdelBiasOmega() const { + return delPdelBiasOmega_; + } + const Matrix3& delVdelBiasAcc() const { + return delVdelBiasAcc_; + } + const Matrix3& delVdelBiasOmega() const { + return delVdelBiasOmega_; } - /// Needed for testable - bool equals(const PreintegrationBase& expected, double tol) const { - return PreintegratedRotation::equals(expected, tol) - && biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol) - && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol); + const Matrix3& accelerometerCovariance() const { + return accelerometerCovariance_; } + const Matrix3& integrationCovariance() const { + return integrationCovariance_; + } + + /// print + void print(const std::string& s) const; + + /// check equality + bool equals(const PreintegrationBase& other, double tol) const; /// Re-initialize PreintegratedMeasurements - void resetIntegration(){ - PreintegratedRotation::resetIntegration(); - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; - } + void resetIntegration(); /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { - - Matrix3 dRij = deltaRij(); // expensive - Vector3 temp = dRij * correctedAcc * deltaT; - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; - } - deltaVij_ += temp; - - Matrix3 R_i, F_angles_angles; - if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij - updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0); - - if(F){ - Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; - Matrix3 F_pos_angles; - if(use2ndOrderIntegration_) - F_pos_angles = 0.5 * F_vel_angles * deltaT; - else - F_pos_angles = Z_3x3; - - // pos vel angle - *F << I_3x3, I_3x3 * deltaT, F_pos_angles, // pos - Z_3x3, I_3x3, F_vel_angles, // vel - Z_3x3, Z_3x3, F_angles_angles;// angle - } - } + void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR, + const double deltaT, OptionalJacobian<9, 9> F); /// Update Jacobians to be used during preintegration void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){ - Matrix3 dRij = deltaRij(); // expensive - Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); - if (!use2ndOrderIntegration_) { - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - } else { - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; - delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5); - } - delVdelBiasAcc_ += -dRij * deltaT; - delVdelBiasOmega_ += temp; - update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); - } + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, + double deltaT); void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, boost::optional body_P_sensor) { - correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities - // (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - } + const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, + boost::optional body_P_sensor); /// Predict state at time j - //------------------------------------------------------------------------------ - PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + PoseVelocityBias predict( + const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, - boost::optional deltaVij_biascorrected_out = boost::none) const { - - const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); - - const Rot3& Rot_i = pose_i.rotation(); - const Vector3& pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; - if(deltaPij_biascorrected_out)// if desired, store this - *deltaPij_biascorrected_out = deltaPij_biascorrected; - - Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected - + vel_i * deltaTij() - - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij()*deltaTij(); - - Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr; - if(deltaVij_biascorrected_out)// if desired, store this - *deltaVij_biascorrected_out = deltaVij_biascorrected; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected - - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term - + gravity * deltaTij()); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position - vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); - Vector3 correctedOmega = biascorrectedOmega - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term - const Rot3 correctedDeltaRij = - Rot3::Expmap( correctedOmega ); - const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant - } + boost::optional deltaVij_biascorrected_out = boost::none) const; /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j - //------------------------------------------------------------------------------ - Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, - const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, - OptionalJacobian<9, 6> H1 = boost::none, - OptionalJacobian<9, 3> H2 = boost::none, - OptionalJacobian<9, 6> H3 = boost::none, - OptionalJacobian<9, 3> H4 = boost::none, - OptionalJacobian<9, 6> H5 = boost::none) const { + Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, + const Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis, OptionalJacobian<9, 6> H1 = + boost::none, + OptionalJacobian<9, 3> H2 = boost::none, + OptionalJacobian<9, 6> H3 = boost::none, + OptionalJacobian<9, 3> H4 = boost::none, + OptionalJacobian<9, 6> H5 = boost::none) const; - // We need the mismatch w.r.t. the biases used for preintegration - // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary - const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3& Ri = pose_i.rotation(); - const Rot3& Rj = pose_j.rotation(); - const Vector3& pos_j = pose_j.translation().vector(); - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected, deltaVij_biascorrected; - PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, - omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); - - // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) - - // Jacobian computation - /* ---------------------------------------------------------------------------------------------------- */ - // Get Get so<3> version of bias corrected rotation - // If H5 is asked for, we will need the Jacobian, which we store in H5 - // H5 will then be corrected below to take into account the Coriolis effect - Matrix3 D_cThetaRij_biasOmegaIncr; - Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0); - - // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration - const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); - const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - Vector3 correctedOmega = biascorrectedOmega - coriolis; - - Rot3 correctedDeltaRij, fRrot; - Vector3 fR; - - // Accessory matrix, used to build the jacobians - Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; - - // This is done to save computation: we ask for the jacobians only when they are needed - if(H1 || H2 || H3 || H4 || H5){ - correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRrot, D_fR_fRrot); - D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); - }else{ - correctedDeltaRij = Rot3::Expmap( correctedOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRrot); - } - if(H1) { - H1->resize(9,6); - Matrix3 dfPdPi = -I_3x3; - Matrix3 dfVdPi = Z_3x3; - if(use2ndOrderCoriolis){ - // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() - Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose()); - dfPdPi += 0.5 * temp * deltaTij()*deltaTij(); - dfVdPi += temp * deltaTij(); - } - (*H1) << - // dfP/dRi - skewSymmetric(fp + deltaPij_biascorrected), - // dfP/dPi - dfPdPi, - // dfV/dRi - skewSymmetric(fv + deltaVij_biascorrected), - // dfV/dPi - dfVdPi, - // dfR/dRi - D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), - // dfR/dPi - Z_3x3; - } - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - Ri.transpose() * deltaTij() - + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Ri.transpose() - + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term - // dfR/dVi - Z_3x3; - } - if(H3) { - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Z_3x3, Ri.transpose() * Rj.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - D_fR_fRrot, Z_3x3; - } - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - Ri.transpose(), - // dfR/dVj - Z_3x3; - } - if(H5) { - // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; - H5->resize(9,6); - (*H5) << - // dfP/dBias - - delPdelBiasAcc(), - delPdelBiasOmega(), - // dfV/dBias - - delVdelBiasAcc(), - delVdelBiasOmega(), - // dfR/dBias - Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); - } - Vector9 r; r << fp, fv, fR; - return r; - } - -private: + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(deltaPij_); @@ -406,27 +174,30 @@ private: class ImuBase { -protected: + protected: Vector3 gravity_; Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect -public: + public: - ImuBase() : - gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), - body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + /// Default constructor, with decent gravity and zero coriolis + ImuBase(); + /// Fully fledge constructor ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : - gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); - const Vector3& gravity() const { return gravity_; } - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + const Vector3& gravity() const { + return gravity_; + } + const Vector3& omegaCoriolis() const { + return omegaCoriolis_; + } }; -} /// namespace gtsam +} /// namespace gtsam diff --git a/gtsam/navigation/tests/CMakeLists.txt b/gtsam/navigation/tests/CMakeLists.txt index b03b8167c..0beca7769 100644 --- a/gtsam/navigation/tests/CMakeLists.txt +++ b/gtsam/navigation/tests/CMakeLists.txt @@ -14,15 +14,10 @@ if(GTSAM_INSTALL_GEOGRAPHICLIB) endif() else() - if(GEOGRAPHICLIB_FOUND) + if(GeographicLib_LIBRARIES) # If we're not installing, but it's already installed, use the installed one include_directories(${GeographicLib_INCLUDE_DIRS}) - list(APPEND test_link_libraries ) - #if(MSVC) - # list(APPEND test_link_libraries GeographicLib_STATIC) - #else() - list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) - #endif() + list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) else() # We don't have GeographicLib set(tests_excluded testGeographicLib.cpp testGPSFactor.cpp testMagFactor.cpp) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 25b3518bc..928c0f74f 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -132,7 +132,7 @@ TEST(AHRSFactor, Error) { pre_int_data.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, false); + AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, boost::none); Vector3 errorActual = factor.evaluateError(x1, x2, bias); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 3f7109543..9fb0f939b 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -44,36 +44,39 @@ namespace { // Auxiliary functions to test Jacobians F and G used for // covariance propagation during preintegration /* ************************************************************************* */ -Vector updatePreintegratedMeasurementsTest( - const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const imuBias::ConstantBias& bias_old, - const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, +Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old, + const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, + const Vector3& correctedOmega, const double deltaT, const bool use2ndOrderIntegration) { Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; Vector3 deltaPij_new; - if(!use2ndOrderIntegration){ + if (!use2ndOrderIntegration) { deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - }else{ + } else { deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; } Vector3 deltaVij_new = deltaVij_old + temp; - Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT); + Rot3 deltaRij_new = deltaRij_old + * Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT); Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more imuBias::ConstantBias bias_new(bias_old); - Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); + Vector result(15); + result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); return result; } -Rot3 updatePreintegratedMeasurementsRot( - const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const imuBias::ConstantBias& bias_old, - const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, - const bool use2ndOrderIntegration){ +Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old, + const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, + const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration) { - Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old, - bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration); + Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega, + deltaT, use2ndOrderIntegration); return Rot3::Expmap(result.segment<3>(6)); } @@ -82,60 +85,53 @@ Rot3 updatePreintegratedMeasurementsRot( // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); - for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } return result; } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaPij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaVij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaVij(); } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return Rot3(evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaRij()); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaRij()); } } /* ************************************************************************* */ -TEST( CombinedImuFactor, PreintegratedMeasurements ) -{ +TEST( CombinedImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; double tol = 1e-6; @@ -145,58 +141,75 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix::Zero(6, 6)); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol)); - EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol)); - EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); + EXPECT( + assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), + tol)); + EXPECT( + assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), + tol)); + EXPECT( + assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), + tol)); DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } /* ************************************************************************* */ -TEST( CombinedImuFactor, ErrorWithBiases ) -{ +TEST( CombinedImuFactor, ErrorWithBiases ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(0.5, 0.0, 0.0); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(0.5, 0.0, 0.0); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; double tol = 1e-6; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6); - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, + deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr Combinedmodel = + noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), + Combined_pre_int_data, gravity, omegaCoriolis); Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); - Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, + bias2); EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); @@ -206,7 +219,8 @@ TEST( CombinedImuFactor, ErrorWithBiases ) // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; - (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); + (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, + H3a, H4a, H5a, H6a); EXPECT(assert_equal(H1e, H1a.topRows(9))); EXPECT(assert_equal(H2e, H2a.topRows(9))); @@ -216,101 +230,128 @@ TEST( CombinedImuFactor, ErrorWithBiases ) } /* ************************************************************************* */ -TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) -{ +TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } // Actual preintegrated values CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + EXPECT( + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT( + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT( + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), + 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST(CombinedImuFactor, PredictPositionAndVelocity){ - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) +TEST(CombinedImuFactor, PredictPositionAndVelocity) { + imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,1,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 1.1, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); + bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); - for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i < 1000; ++i) + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, + deltaT); // Create factor - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr Combinedmodel = + noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), + Combined_pre_int_data, gravity, omegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, + bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; + Vector3 expectedVelocity; + expectedVelocity << 0, 1, 0; EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); + EXPECT( + assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); } /* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - Matrix I6x6(6,6); + Matrix I6x6(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); - Vector3 measuredAcc; measuredAcc << 0, 0, -9.81; - Vector3 gravity; gravity<<0,0,9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0,0,0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; + bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); + Vector3 measuredAcc; + measuredAcc << 0, 0, -9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0; double deltaT = 0.001; double tol = 1e-4; for (int i = 0; i < 1000; ++i) @@ -320,16 +361,16 @@ TEST(CombinedImuFactor, PredictRotation) { Combined_pre_int_data, gravity, omegaCoriolis); // Predict - Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); - Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); + Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; + Vector3 v(0, 0, 0), v2; + CombinedImuFactor::Predict(x, v, x2, v2, bias, + Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); + Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, x2, tol)); } /* ************************************************************************* */ -TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) -{ +TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { // Linearization point imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases Pose3 body_P_sensor = Pose3(); @@ -338,35 +379,36 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } // Actual preintegrated values CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs); + evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, + deltaTs); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement Matrix oldPreintCovariance = preintegrated.preintMeasCov(); Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, - body_P_sensor, Factual, Gactual); + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); bool use2ndOrderIntegration = false; @@ -374,44 +416,51 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) // COMPUTE NUMERICAL DERIVATIVES FOR F ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected F wrt positions (15,3) - Matrix df_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - _1, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + Matrix df_dpos = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaPij_old); // rotation part has to be done properly, on manifold - df_dpos.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - _1, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + df_dpos.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaPij_old); // Compute expected F wrt velocities (15,3) - Matrix df_dvel = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, _1, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + Matrix df_dvel = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaVij_old); // rotation part has to be done properly, on manifold - df_dvel.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, _1, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + df_dvel.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1, + deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), deltaVij_old); // Compute expected F wrt angles (15,3) - Matrix df_dangle = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, _1, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix df_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), deltaRij_old); // rotation part has to be done properly, on manifold - df_dangle.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, _1, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + df_dangle.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), deltaRij_old); // Compute expected F wrt biases (15,6) - Matrix df_dbias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + Matrix df_dbias = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); // rotation part has to be done properly, on manifold - df_dbias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + df_dbias.block<3, 6>(6, 0) = + numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); - Matrix Fexpected(15,15); + Matrix Fexpected(15, 15); Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; EXPECT(assert_equal(Fexpected, Factual)); @@ -419,55 +468,65 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) // COMPUTE NUMERICAL DERIVATIVES FOR G ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected G wrt integration noise - Matrix df_dintNoise(15,3); + Matrix df_dintNoise(15, 3); df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; // Compute expected G wrt acc noise (15,3) - Matrix df_daccNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + Matrix df_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // rotation part has to be done properly, on manifold - df_daccNoise.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // Compute expected G wrt gyro noise (15,3) - Matrix df_domegaNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + Matrix df_domegaNoise = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, + use2ndOrderIntegration), newMeasuredOmega); // rotation part has to be done properly, on manifold - df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, + use2ndOrderIntegration), newMeasuredOmega); // Compute expected G wrt bias random walk noise (15,6) - Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries + Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries df_rwBias.setZero(); - df_rwBias.block<6,6>(9,0) = eye(6); + df_rwBias.block<6, 6>(9, 0) = eye(6); // Compute expected G wrt gyro noise (15,6) - Matrix df_dinitBias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + Matrix df_dinitBias = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); // rotation part has to be done properly, on manifold - df_dinitBias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); - df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows + df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); + df_dinitBias.block<6, 6>(9, 0) = Matrix::Zero(6, 6); // only has to influence first 9 rows - Matrix Gexpected(15,21); + Matrix Gexpected(15, 21); Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + - (1/newDeltaT) * Gactual * Gactual.transpose(); + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index bd321d51d..4d5df3f05 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -21,23 +21,111 @@ using namespace std; using namespace gtsam; +Vector biasAcc1(Vector3(0.2, -0.1, 0)); +Vector biasGyro1(Vector3(0.1, -0.3, -0.2)); +imuBias::ConstantBias bias1(biasAcc1, biasGyro1); + +Vector biasAcc2(Vector3(0.1, 0.2, 0.04)); +Vector biasGyro2(Vector3(-0.002, 0.005, 0.03)); +imuBias::ConstantBias bias2(biasAcc2, biasGyro2); /* ************************************************************************* */ -TEST( ImuBias, Constructor) -{ - Vector bias_acc(Vector3(0.1,0.2,0.4)); - Vector bias_gyro(Vector3(-0.2, 0.5, 0.03)); - +TEST( ImuBias, Constructor) { // Default Constructor - gtsam::imuBias::ConstantBias bias1; + imuBias::ConstantBias bias1; // Acc + Gyro Constructor - gtsam::imuBias::ConstantBias bias2(bias_acc, bias_gyro); + imuBias::ConstantBias bias2(biasAcc2, biasGyro2); // Copy Constructor - gtsam::imuBias::ConstantBias bias3(bias2); + imuBias::ConstantBias bias3(bias2); } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +TEST( ImuBias, inverse) { + imuBias::ConstantBias biasActual = bias1.inverse(); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, + -biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, compose) { + imuBias::ConstantBias biasActual = bias2.compose(bias1); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, between) { + // p.between(q) == q - p + imuBias::ConstantBias biasActual = bias2.between(bias1); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, localCoordinates) { + Vector deltaActual = Vector(bias2.localCoordinates(bias1)); + Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2, + biasGyro1 - biasGyro2)).vector(); + EXPECT(assert_equal(deltaExpected, deltaActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, retract) { + Vector6 delta; + delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; + imuBias::ConstantBias biasActual = bias2.retract(delta); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0)); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, Logmap) { + Vector deltaActual = bias2.Logmap(bias1); + Vector deltaExpected = bias1.vector(); + EXPECT(assert_equal(deltaExpected, deltaActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, Expmap) { + Vector6 delta; + delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; + imuBias::ConstantBias biasActual = bias2.Expmap(delta); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, operatorSub) { + imuBias::ConstantBias biasActual = -bias1; + imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, operatorAdd) { + imuBias::ConstantBias biasActual = bias2 + bias1; + imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, + biasGyro2 + biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +TEST( ImuBias, operatorSubB) { + imuBias::ConstantBias biasActual = bias2 - bias1; + imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, + biasGyro2 - biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 9c82a7dfa..c27921fc9 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -39,37 +40,39 @@ using symbol_shorthand::B; namespace { // Auxiliary functions to test evaluate error in ImuFactor /* ************************************************************************* */ -Vector callEvaluateError(const ImuFactor& factor, - const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias){ +Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias) { return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); } -Rot3 evaluateRotationError(const ImuFactor& factor, - const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias){ - return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; +Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias) { + return Rot3::Expmap( + factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3)); } // Auxiliary functions to test Jacobians F and G used for // covariance propagation during preintegration /* ************************************************************************* */ -Vector updatePreintegratedPosVel( - const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, - const bool use2ndOrderIntegration_) { +Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, + const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, + const double deltaT, const bool use2ndOrderIntegration_) { Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * correctedAcc * deltaT; Vector3 deltaPij_new; - if(!use2ndOrderIntegration_){ + if (!use2ndOrderIntegration_) { deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - }else{ + } else { deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; } Vector3 deltaVij_new = deltaVij_old + temp; - Vector result(6); result << deltaPij_new, deltaVij_new; + Vector result(6); + result << deltaPij_new, deltaVij_new; return result; } @@ -79,138 +82,160 @@ Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, return deltaRij_new; } -// Auxiliary functions to test preintegrated Jacobians -// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +// Define covariance matrices /* ************************************************************************* */ double accNoiseVar = 0.01; double omegaNoiseVar = 0.03; double intNoiseVar = 0.0001; +const Matrix3 kMeasuredAccCovariance = accNoiseVar * Matrix3::Identity(); +const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * Matrix3::Identity(); +const Matrix3 kIntegrationErrorCovariance = intNoiseVar * Matrix3::Identity(); + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const bool use2ndOrderIntegration = false){ - ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(), - omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity(),use2ndOrderIntegration); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs, + const bool use2ndOrderIntegration = false) { + ImuFactor::PreintegratedMeasurements result(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); - for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } return result; } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaPij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaVij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaVij(); } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return Rot3(evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaRij()); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaRij()); } -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){ +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, + const double deltaT) { return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); } -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { + return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); } } /* ************************************************************************* */ -TEST( ImuFactor, PreintegratedMeasurements ) -{ +TEST( ImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; // Expected preintegrated values - Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5*0.1*0.5*0.5, 0, 0; + Vector3 expectedDeltaP1; + expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0; Vector3 expectedDeltaV1(0.05, 0.0, 0.0); - Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); + Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT1(0.5); bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements actual1(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); - EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); // Integrate again - Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0; - Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5; - Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); + Vector3 expectedDeltaP2; + expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; + Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + + expectedDeltaR1.matrix() * measuredAcc * 0.5; + Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT2(1); // Actual preintegrated values ImuFactor::PreintegratedMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); - EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobians ) -{ +TEST( ImuFactor, ErrorAndJacobians ) { // Linearization point imuBias::ConstantBias bias; // Bias - Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << M_PI / 100, 0, 0; Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); double deltaT = 1.0; bool use2ndOrderIntegration = true; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements pre_int_data(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); // Expected error - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + Vector errorExpected(9); + errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Actual Jacobians @@ -219,61 +244,102 @@ TEST( ImuFactor, ErrorAndJacobians ) // Expected Jacobians /////////////////// H1 /////////////////////////// - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); // Jacobians are around zero, so the rotation part is the same as: - Matrix H1Rot3 = numericalDerivative11( + Matrix H1Rot3 = numericalDerivative11( boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3))); EXPECT(assert_equal(H1e, H1a)); /////////////////// H2 /////////////////////////// - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); EXPECT(assert_equal(H2e, H2a)); /////////////////// H3 /////////////////////////// - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); // Jacobians are around zero, so the rotation part is the same as: - Matrix H3Rot3 = numericalDerivative11( + Matrix H3Rot3 = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3))); EXPECT(assert_equal(H3e, H3a)); /////////////////// H4 /////////////////////////// - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); EXPECT(assert_equal(H4e, H4a)); /////////////////// H5 /////////////////////////// - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); EXPECT(assert_equal(H5e, H5a)); + + //////////////////////////////////////////////////////////////////////////// + // Evaluate error with wrong values + Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); + errorActual = factor.evaluateError(x1, v1, x2, v2_wrong, bias); + errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; + EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2_wrong); + values.insert(B(1), bias); + errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; + EXPECT(assert_equal(factor.unwhitenedError(values), errorActual, 1e-6)); + + // Make sure the whitening is done correctly + Matrix cov = pre_int_data.preintMeasCov(); + Matrix R = RtR(cov.inverse()); + Vector whitened = R * errorActual; + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6)); + + /////////////////////////////////////////////////////////////////////////////// + // Make sure linearization is correct + // Create expected value by numerical differentiation + JacobianFactor expected = linearizeNumerically(factor, values, 1e-8); + + // Create actual value by linearize + GaussianFactor::shared_ptr linearized = factor.linearize(values); + JacobianFactor* actual = dynamic_cast(linearized.get()); + + // Check cast result and then equality + EXPECT(assert_equal(expected, *actual, 1e-4)); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWithBiases ) -{ +TEST( ImuFactor, ErrorAndJacobianWithBiases ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0), + Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); SETDEBUG("ImuFactor evaluateError", false); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -285,23 +351,23 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( + Matrix RH1e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( + Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( + Matrix RH5e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); // Actual Jacobians @@ -316,29 +382,36 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) -{ +TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0), + Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor Pose3 bodyPsensor = Pose3(); bool use2ndOrderCoriolis = true; - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); SETDEBUG("ImuFactor evaluateError", false); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -350,23 +423,23 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( + Matrix RH1e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( + Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( + Matrix RH5e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); // Actual Jacobians @@ -381,39 +454,43 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivative_wrt_Bias ) -{ +TEST( ImuFactor, PartialDerivative_wrt_Bias ) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega; + biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0.1, 0, 0; double deltaT = 0.5; // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( - &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); + Matrix expectedDelRdelBiasOmega = numericalDerivative11( + boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), + Vector3(biasOmega)); - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); - Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeLogmap ) -{ +TEST( ImuFactor, PartialDerivativeLogmap ) { // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + Vector3 thetahat; + thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 deltatheta; deltatheta << 0, 0, 0; + Vector3 deltatheta; + deltatheta << 0, 0, 0; // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( - &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); + Matrix expectedDelFdeltheta = numericalDerivative11( + boost::bind(&evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); @@ -422,28 +499,33 @@ TEST( ImuFactor, PartialDerivativeLogmap ) } /* ************************************************************************* */ -TEST( ImuFactor, fistOrderExponential ) -{ +TEST( ImuFactor, fistOrderExponential ) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega; + biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0.1, 0, 0; double deltaT = 1.0; // change w.r.t. linearization point double alpha = 0.0; - Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; + Vector3 deltabiasOmega; + deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); - Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign - const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + const Matrix expectedRot = Rot3::Expmap( + (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); - const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); - const Matrix3 actualRot = - hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + const Matrix3 hatRot = + Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 actualRot = hatRot + * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); // This is a first order expansion so the equality is only an approximation @@ -451,61 +533,70 @@ TEST( ImuFactor, fistOrderExponential ) } /* ************************************************************************* */ -TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) -{ +TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } // Actual preintegrated values ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + EXPECT( + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT( + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT( + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), + 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) -{ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); @@ -514,68 +605,71 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } bool use2ndOrderIntegration = false; // Actual preintegrated values ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs, use2ndOrderIntegration); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement Matrix oldPreintCovariance = preintegrated.preintMeasCov(); Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, - body_P_sensor, Factual, Gactual); + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR F ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected f_pos_vel wrt positions - Matrix dfpv_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + Matrix dfpv_dpos = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaPij_old); // Compute expected f_pos_vel wrt velocities - Matrix dfpv_dvel = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + Matrix dfpv_dvel = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaVij_old); // Compute expected f_pos_vel wrt angles - Matrix dfpv_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix dfpv_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaRij_old); - Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; + Matrix FexpectedTop6(6, 9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; // Compute expected f_rot wrt angles - Matrix dfr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - _1, newMeasuredOmega, newDeltaT), deltaRij_old); + Matrix dfr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), + deltaRij_old); - Matrix FexpectedBottom3(3,9); + Matrix FexpectedBottom3(3, 9); FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; - Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3; + Matrix Fexpected(9, 9); + Fexpected << FexpectedTop6, FexpectedBottom3; EXPECT(assert_equal(Fexpected, Factual)); @@ -583,50 +677,50 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) // COMPUTE NUMERICAL DERIVATIVES FOR G ////////////////////////////////////////////////////////////////////////////////////////////// // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6,3); + Matrix dgpv_dintNoise(6, 3); dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; // Compute jacobian wrt acc noise - Matrix dgpv_daccNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + Matrix dgpv_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // Compute expected F wrt gyro noise - Matrix dgpv_domegaNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); - Matrix GexpectedTop6(6,9); + Matrix dgpv_domegaNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), + newMeasuredOmega); + Matrix GexpectedTop6(6, 9); GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; // Compute expected f_rot wrt gyro noise - Matrix dgr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - deltaRij_old, _1, newDeltaT), newMeasuredOmega); + Matrix dgr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), + newMeasuredOmega); - Matrix GexpectedBottom3(3,9); + Matrix GexpectedBottom3(3, 9); GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; - Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3; + Matrix Gexpected(9, 9); + Gexpected << GexpectedTop6, GexpectedBottom3; EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation Matrix9 measurementCovariance; - measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3, - Z_3x3, accNoiseVar*I_3x3, Z_3x3, - Z_3x3, Z_3x3, omegaNoiseVar*I_3x3; + measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar + * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + - (1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); } /* ************************************************************************* */ -TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) -{ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); @@ -635,68 +729,71 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + for (int i = 1; i < 100; i++) { measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } bool use2ndOrderIntegration = true; // Actual preintegrated values ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs, use2ndOrderIntegration); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement Matrix oldPreintCovariance = preintegrated.preintMeasCov(); Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, - body_P_sensor, Factual, Gactual); + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR F ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected f_pos_vel wrt positions - Matrix dfpv_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + Matrix dfpv_dpos = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaPij_old); // Compute expected f_pos_vel wrt velocities - Matrix dfpv_dvel = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + Matrix dfpv_dvel = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaVij_old); // Compute expected f_pos_vel wrt angles - Matrix dfpv_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix dfpv_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), + deltaRij_old); - Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; + Matrix FexpectedTop6(6, 9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; // Compute expected f_rot wrt angles - Matrix dfr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - _1, newMeasuredOmega, newDeltaT), deltaRij_old); + Matrix dfr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), + deltaRij_old); - Matrix FexpectedBottom3(3,9); + Matrix FexpectedBottom3(3, 9); FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; - Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3; + Matrix Fexpected(9, 9); + Fexpected << FexpectedTop6, FexpectedBottom3; EXPECT(assert_equal(Fexpected, Factual)); @@ -704,42 +801,43 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) // COMPUTE NUMERICAL DERIVATIVES FOR G ////////////////////////////////////////////////////////////////////////////////////////////// // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6,3); + Matrix dgpv_dintNoise(6, 3); dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; // Compute jacobian wrt acc noise - Matrix dgpv_daccNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + Matrix dgpv_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // Compute expected F wrt gyro noise - Matrix dgpv_domegaNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); - Matrix GexpectedTop6(6,9); + Matrix dgpv_domegaNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), + newMeasuredOmega); + Matrix GexpectedTop6(6, 9); GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; // Compute expected f_rot wrt gyro noise - Matrix dgr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - deltaRij_old, _1, newDeltaT), newMeasuredOmega); + Matrix dgr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), + newMeasuredOmega); - Matrix GexpectedBottom3(3,9); + Matrix GexpectedBottom3(3, 9); GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; - Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3; + Matrix Gexpected(9, 9); + Gexpected << GexpectedTop6, GexpectedBottom3; EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation Matrix9 measurementCovariance; - measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3, - Z_3x3, accNoiseVar*I_3x3, Z_3x3, - Z_3x3, Z_3x3, omegaNoiseVar*I_3x3; + measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar + * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + - (1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); @@ -801,93 +899,109 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) //} /* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) -{ +TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); + const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), + Point3(1, 0, 0)); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ -TEST(ImuFactor, PredictPositionAndVelocity){ +TEST(ImuFactor, PredictPositionAndVelocity) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,1,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, 0; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 1, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i < 1000; ++i) + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, + omegaCoriolis); + Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + Vector3 expectedVelocity; + expectedVelocity << 0, 1, 0; + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); } /* ************************************************************************* */ @@ -895,33 +1009,46 @@ TEST(ImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,0,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 0, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + ImuFactor::PreintegratedMeasurements pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i < 1000; ++i) + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis); - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,0,0; - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + // Predict + Pose3 x1, x2; + Vector3 v1 = Vector3(0, 0.0, 0.0); + Vector3 v2; + ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), + gravity, omegaCoriolis); + Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + Vector3 expectedVelocity; + expectedVelocity << 0, 0, 0; + EXPECT(assert_equal(expectedPose, x2)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 7295f3160..341bb7d10 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -33,7 +33,7 @@ namespace detail { template struct Origin { T operator()() { return traits::Identity();} }; -// but dimple manifolds don't have one, so we just use the default constructor +// but simple manifolds don't have one, so we just use the default constructor template struct Origin { T operator()() { return T();} }; @@ -65,7 +65,12 @@ struct Canonical { } }; -/// Adapt ceres-style autodiff +/** + * The AdaptAutoDiff class uses ceres-style autodiff to adapt a ceres-style + * Function evaluation, i.e., a function F that defines an operator + * template bool operator()(const T* const, const T* const, T* predicted) const; + * For now only binary operators are supported. + */ template class AdaptAutoDiff { @@ -104,7 +109,7 @@ public: // Get derivatives with AutoDiff double *parameters[] = { v1.data(), v2.data() }; - double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack + double rowMajor1[N * M1], rowMajor2[N * M2]; // on the stack double *jacobians[] = { rowMajor1, rowMajor2 }; success = AutoDiff::Differentiate(f, parameters, 2, result.data(), jacobians); diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt index 5bb2bd7d2..ad4824817 100644 --- a/gtsam/nonlinear/CMakeLists.txt +++ b/gtsam/nonlinear/CMakeLists.txt @@ -2,5 +2,8 @@ file(GLOB nonlinear_headers "*.h") install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear) +file(GLOB nonlinear_headers_internal "internal/*.h") +install(FILES ${nonlinear_headers_internal} DESTINATION include/gtsam/nonlinear/internal) + # Build tests add_subdirectory(tests) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index a86e7312a..6c6c155c7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,773 +19,244 @@ #pragma once -#include -#include -#include +#include -#include #include -#include -#include - -// template meta-programming headers -#include -namespace MPL = boost::mpl::placeholders; - -#include // operator typeid -#include - -class ExpressionFactorBinaryTest; -// Forward declare for testing +#include +#include namespace gtsam { -const unsigned TraceAlignment = 16; - template -T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { - // right now only word sized types are supported. - // Easy to extend if needed, - // by somehow inferring the unsigned integer of same size - BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); - size_t & uiValue = reinterpret_cast(value); - size_t misAlignment = uiValue % requiredAlignment; - if (misAlignment) { - uiValue += requiredAlignment - misAlignment; - } - return value; -} -template -T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { - return upAlign(value, requiredAlignment); +void Expression::print(const std::string& s) const { + std::cout << s << *root_ << std::endl; } template -class Expression; - -/** - * Expressions are designed to write their derivatives into an already allocated - * Jacobian of the correct size, of type VerticalBlockMatrix. - * The JacobianMap provides a mapping from keys to the underlying blocks. - */ -class JacobianMap { - const FastVector& keys_; - VerticalBlockMatrix& Ab_; -public: - JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : - keys_(keys), Ab_(Ab) { - } - /// Access via key - VerticalBlockMatrix::Block operator()(Key key) { - FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), - key); - DenseIndex block = it - keys_.begin(); - return Ab_(block); - } -}; - -//----------------------------------------------------------------------------- - -namespace internal { - -template -struct UseBlockIf { - static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - // block makes HUGE difference - jacobians(key).block( - 0, 0) += dTdA; - } - ; -}; -/// Handle Leaf Case for Dynamic Matrix type (slower) -template -struct UseBlockIf { - static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - jacobians(key) += dTdA; - } -}; +Expression::Expression(const T& value) : + root_(new internal::ConstantExpression(value)) { } -/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians -template -void handleLeafCase(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - internal::UseBlockIf< - Derived::RowsAtCompileTime != Eigen::Dynamic - && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( - dTdA, jacobians, key); +template +Expression::Expression(const Key& key) : + root_(new internal::LeafExpression(key)) { } -//----------------------------------------------------------------------------- -/** - * The ExecutionTrace class records a tree-structured expression's execution. - * - * The class looks a bit complicated but it is so for performance. - * It is a tagged union that obviates the need to create - * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead - * the key for the leaf is stored in the space normally used to store a - * CallRecord*. Nothing is stored for a Constant. - * - * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: - * Trace(Function) -> - * BinaryRecord with two traces in it - * trace1(Function) -> - * UnaryRecord with one trace in it - * trace1(Function) -> - * BinaryRecord with two traces in it - * trace1(Leaf) - * trace2(Constant) - * trace2(Leaf) - * Hence, there are three Record structs, written to memory by traceExecution - */ -template -class ExecutionTrace { +template +Expression::Expression(const Symbol& symbol) : + root_(new internal::LeafExpression(symbol)) { +} + +template +Expression::Expression(unsigned char c, size_t j) : + root_(new internal::LeafExpression(Symbol(c, j))) { +} + +/// Construct a unary function expression +template +template +Expression::Expression(typename UnaryFunction::type function, + const Expression& expression) : + root_(new internal::UnaryExpression(function, expression)) { +} + +/// Construct a binary function expression +template +template +Expression::Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2) : + root_( + new internal::BinaryExpression(function, expression1, + expression2)) { +} + +/// Construct a ternary function expression +template +template +Expression::Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3) : + root_( + new internal::TernaryExpression(function, expression1, + expression2, expression3)) { +} + +/// Construct a nullary method expression +template +template +Expression::Expression(const Expression& expression, + T (A::*method)(typename MakeOptionalJacobian::type) const) : + root_( + new internal::UnaryExpression(boost::bind(method, _1, _2), + expression)) { +} + +/// Construct a unary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2) : + root_( + new internal::BinaryExpression( + boost::bind(method, _1, _2, _3, _4), expression1, expression2)) { +} + +/// Construct a binary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3) : + root_( + new internal::TernaryExpression( + boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, + expression2, expression3)) { +} + +template +std::set Expression::keys() const { + return root_->keys(); +} + +template +void Expression::dims(std::map& map) const { + root_->dims(map); +} + +template +T Expression::value(const Values& values, + boost::optional&> H) const { + + if (H) { + // Call private version that returns derivatives in H + KeysAndDims pair = keysAndDims(); + return valueAndDerivatives(values, pair.first, pair.second, *H); + } else + // no derivatives needed, just return value + return root_->value(values); +} + +template +const boost::shared_ptr >& Expression::root() const { + return root_; +} + +template +size_t Expression::traceSize() const { + return root_->traceSize(); +} + +// Private methods: + +template +T Expression::valueAndDerivatives(const Values& values, + const FastVector& keys, const FastVector& dims, + std::vector& H) const { + + // H should be pre-allocated + assert(H.size()==keys.size()); + + // Pre-allocate and zero VerticalBlockMatrix static const int Dim = traits::dimension; - enum { - Constant, Leaf, Function - } kind; - union { - Key key; - CallRecord* ptr; - } content; -public: - /// Pointer always starts out as a Constant - ExecutionTrace() : - kind(Constant) { - } - /// Change pointer to a Leaf Record - void setLeaf(Key key) { - kind = Leaf; - content.key = key; - } - /// Take ownership of pointer to a Function Record - void setFunction(CallRecord* record) { - kind = Function; - content.ptr = record; - } - /// Print - void print(const std::string& indent = "") const { - if (kind == Constant) - std::cout << indent << "Constant" << std::endl; - else if (kind == Leaf) - std::cout << indent << "Leaf, key = " << content.key << std::endl; - else if (kind == Function) { - std::cout << indent << "Function" << std::endl; - content.ptr->print(indent + " "); - } - } - /// Return record pointer, quite unsafe, used only for testing - template - boost::optional record() { - if (kind != Function) - return boost::none; - else { - Record* p = dynamic_cast(content.ptr); - return p ? boost::optional(p) : boost::none; - } - } - /** - * *** This is the main entry point for reverse AD, called from Expression *** - * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) - */ - typedef Eigen::Matrix JacobianTT; - void startReverseAD1(JacobianMap& jacobians) const { - if (kind == Leaf) { - // This branch will only be called on trivial Leaf expressions, i.e. Priors - static const JacobianTT I = JacobianTT::Identity(); - handleLeafCase(I, jacobians, content.key); - } else if (kind == Function) - // This is the more typical entry point, starting the AD pipeline - // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. - content.ptr->startReverseAD2(jacobians); - } - // Either add to Jacobians (Leaf) or propagate (Function) - template - void reverseAD1(const Eigen::MatrixBase & dTdA, - JacobianMap& jacobians) const { - if (kind == Leaf) - handleLeafCase(dTdA, jacobians, content.key); - else if (kind == Function) - content.ptr->reverseAD2(dTdA, jacobians); - } + VerticalBlockMatrix Ab(dims, Dim); + Ab.matrix().setZero(); + internal::JacobianMap jacobianMap(keys, Ab); - /// Define type so we can apply it as a meta-function - typedef ExecutionTrace type; -}; + // Call unsafe version + T result = valueAndJacobianMap(values, jacobianMap); -/// Storage type for the execution trace. -/// It enforces the proper alignment in a portable way. -/// Provide a traceSize() sized array of this type to traceExecution as traceStorage. -typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < static_cast(keys.size()); i++) + H[i] = Ab(i); -//----------------------------------------------------------------------------- -/** - * Expression node. The superclass for objects that do the heavy lifting - * An Expression has a pointer to an ExpressionNode underneath - * allowing Expressions to have polymorphic behaviour even though they - * are passed by value. This is the same way boost::function works. - * http://loki-lib.sourceforge.net/html/a00652.html - */ -template -class ExpressionNode { + return result; +} -protected: - - size_t traceSize_; - - /// Constructor, traceSize is size of the execution trace of expression rooted here - ExpressionNode(size_t traceSize = 0) : - traceSize_(traceSize) { - } - -public: - - /// Destructor - virtual ~ExpressionNode() { - } - - /// Streaming - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, - const ExpressionNode& node) { - os << "Expression of type " << typeid(T).name(); - if (node.traceSize_>0) os << ", trace size = " << node.traceSize_; - os << "\n"; - return os; - } - - /// Return keys that play in this expression as a set - virtual std::set keys() const { - std::set keys; - return keys; - } - - /// Return dimensions for each argument, as a map - virtual void dims(std::map& map) const { - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return traceSize_; - } - - /// Return value - virtual T value(const Values& values) const = 0; - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const = 0; -}; - -//----------------------------------------------------------------------------- -/// Constant Expression -template -class ConstantExpression: public ExpressionNode { - - /// The constant value - T constant_; - - /// Constructor with a value, yielding a constant - ConstantExpression(const T& value) : - constant_(value) { - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - return constant_; - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return constant_; - } -}; - - -//----------------------------------------------------------------------------- -/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value template -class LeafExpression : public ExpressionNode { - typedef T value_type; +T Expression::traceExecution(const Values& values, + internal::ExecutionTrace& trace, void* traceStorage) const { + return root_->traceExecution(values, trace, + static_cast(traceStorage)); +} - /// The key into values - Key key_; +template +T Expression::valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const { + // The following piece of code is absolutely crucial for performance. + // We allocate a block of memory on the stack, which can be done at runtime + // with modern C++ compilers. The traceExecution then fills this memory + // with an execution trace, made up entirely of "Record" structs, see + // the FunctionalNode class in expression-inl.h + size_t size = traceSize(); - /// Constructor with a single key - LeafExpression(Key key) : - key_(key) { - } - // todo: do we need a virtual destructor here too? - - friend class Expression ; - -public: - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys; - keys.insert(key_); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - map[key_] = traits::dimension; - } - - /// Return value - virtual T value(const Values& values) const { - return values.at(key_); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - trace.setLeaf(key_); - return values.at(key_); - } - -}; - -//----------------------------------------------------------------------------- -// Below we use the "Class Composition" technique described in the book -// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost -// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. -// to recursively generate a class, that will be the base for function nodes. -// -// The class generated, for three arguments A1, A2, and A3 will be -// -// struct Base1 : Argument, FunctionalBase { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Base2 : Argument, Base1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Base3 : Argument, Base2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct FunctionalNode : Base3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// -// All this magic happens when we generate the Base3 base class of FunctionalNode -// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode -// -// Similarly, the inner Record struct will be -// -// struct Record1 : JacobianTrace, CallRecord::value> { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Record2 : JacobianTrace, Record1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Record3 : JacobianTrace, Record2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct Record : Record3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// - -//----------------------------------------------------------------------------- - -/// meta-function to generate fixed-size JacobianTA type -template -struct Jacobian { - typedef Eigen::Matrix::dimension, - traits::dimension> type; -}; - -/// meta-function to generate JacobianTA optional reference -template -struct MakeOptionalJacobian { - typedef OptionalJacobian::dimension, - traits::dimension> type; -}; - -/** - * Base case for recursive FunctionalNode class - */ -template -struct FunctionalBase: ExpressionNode { - static size_t const N = 0; // number of arguments - - struct Record { - void print(const std::string& indent) const { - } - void startReverseAD4(JacobianMap& jacobians) const { - } - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { - } - }; - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - // base case: does not do anything - } -}; - -/** - * Building block for recursive FunctionalNode class - * The integer argument N is to guarantee a unique type signature, - * so we are guaranteed to be able to extract their values by static cast. - */ -template -struct Argument { - /// Expression that will generate value/derivatives for argument - boost::shared_ptr > expression; -}; - -/** - * Building block for Recursive Record Class - * Records the evaluation of a single argument in a functional expression - */ -template -struct JacobianTrace { - A value; - ExecutionTrace trace; - typename Jacobian::type dTdA; -}; - -// Recursive Definition of Functional ExpressionNode -// The reason we inherit from Argument is because we can then -// case to this unique signature to retrieve the expression at any level -template -struct GenerateFunctionalNode: Argument, Base { - - static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy - typedef Argument This; ///< The storage we have direct access to - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys = Base::keys(); - std::set myKeys = This::expression->keys(); - keys.insert(myKeys.begin(), myKeys.end()); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - Base::dims(map); - This::expression->dims(map); - } - - // Recursive Record Class for Functional Expressions - // The reason we inherit from JacobianTrace is because we can then - // case to this unique signature to retrieve the value/trace at any level - struct Record: JacobianTrace, Base::Record { - - typedef T return_type; - typedef JacobianTrace This; - - /// Print to std::cout - void print(const std::string& indent) const { - Base::Record::print(indent); - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); - } - - /// Start the reverse AD process - void startReverseAD4(JacobianMap& jacobians) const { - Base::Record::startReverseAD4(jacobians); - // This is the crucial point where the size of the AD pipeline is selected. - // One pipeline is started for each argument, but the number of rows in each - // pipeline is the same, namely the dimension of the output argument T. - // For example, if the entire expression is rooted by a binary function - // yielding a 2D result, then the matrix dTdA will have 2 rows. - // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 - // which calls the correctly sized CallRecord::reverseAD3, which in turn - // calls reverseAD4 below. - This::trace.reverseAD1(This::dTdA, jacobians); - } - - /// Given df/dT, multiply in dT/dA and continue reverse AD process - // Cols is always known at compile time - template - void reverseAD4(const SomeMatrix & dFdT, - JacobianMap& jacobians) const { - Base::Record::reverseAD4(dFdT, jacobians); - This::trace.reverseAD1(dFdT * This::dTdA, jacobians); - } - }; - - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - Base::trace(values, record, traceStorage); // recurse - // Write an Expression execution trace in record->trace - // Iff Constant or Leaf, this will not write to traceStorage, only to trace. - // Iff the expression is functional, write all Records in traceStorage buffer - // Return value of type T is recorded in record->value - record->Record::This::value = This::expression->traceExecution(values, - record->Record::This::trace, traceStorage); - // traceStorage is never modified by traceExecution, but if traceExecution has - // written in the buffer, the next caller expects we advance the pointer - traceStorage += This::expression->traceSize(); - } -}; - -/** - * Recursive GenerateFunctionalNode class Generator - */ -template -struct FunctionalNode { - - /// The following typedef generates the recursively defined Base class - typedef typename boost::mpl::fold, - GenerateFunctionalNode >::type Base; - - /** - * The type generated by this meta-function derives from Base - * and adds access functions as well as the crucial [trace] function - */ - struct type: public Base { - - // Argument types and derived, note these are base 0 ! - // These are currently not used - useful for Phoenix in future -#ifdef EXPRESSIONS_PHOENIX - typedef TYPES Arguments; - typedef typename boost::mpl::transform >::type Jacobians; - typedef typename boost::mpl::transform >::type Optionals; + // Windows does not support variable length arrays, so memory must be dynamically + // allocated on Visual Studio. For more information see the issue below + // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio +#ifdef _MSC_VER + internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size]; +#else + internal::ExecutionTraceStorage traceStorage[size]; #endif - /// Reset expression shared pointer - template - void reset(const boost::shared_ptr >& ptr) { - static_cast &>(*this).expression = ptr; - } + internal::ExecutionTrace trace; + T value(this->traceExecution(values, trace, traceStorage)); + trace.startReverseAD1(jacobians); - /// Access Expression shared pointer - template - boost::shared_ptr > expression() const { - return static_cast const &>(*this).expression; - } +#ifdef _MSC_VER + delete[] traceStorage; +#endif - /// Provide convenience access to Record storage and implement - /// the virtual function based interface of CallRecord using the CallRecordImplementor - struct Record: public internal::CallRecordImplementor::dimension>, public Base::Record { - using Base::Record::print; - using Base::Record::startReverseAD4; - using Base::Record::reverseAD4; - - virtual ~Record() { - } - - /// Access Value - template - const A& value() const { - return static_cast const &>(*this).value; - } - - /// Access Jacobian - template - typename Jacobian::type& jacobian() { - return static_cast&>(*this).dTdA; - } - }; - - /// Construct an execution trace for reverse AD - Record* trace(const Values& values, - ExecutionTraceStorage* traceStorage) const { - assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); - - // Create the record and advance the pointer - Record* record = new (traceStorage) Record(); - traceStorage += upAligned(sizeof(Record)); - - // Record the traces for all arguments - // After this, the traceStorage pointer is set to after what was written - Base::trace(values, record, traceStorage); - - // Return the record for this function evaluation - return record; - } - }; -}; -//----------------------------------------------------------------------------- - -/// Unary Function Expression -template -class UnaryExpression: public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - -public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a unary function f, and input argument e - UnaryExpression(Function f, const Expression& e1) : - function_(f) { - this->template reset(e1.root()); - ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - return function_(this->template expression()->value(values), boost::none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_(record->template value(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Binary Expression - -template -class BinaryExpression: - public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; - -public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a ternary function f, and three input arguments - BinaryExpression(Function f, const Expression& e1, - const Expression& e2) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); - } - - friend class Expression ; - friend class ::ExpressionFactorBinaryTest; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_(record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Ternary Expression - -template -class TernaryExpression: - public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; - typedef typename MakeOptionalJacobian::type OJ3; - -public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a ternary function f, and three input arguments - TernaryExpression(Function f, const Expression& e1, - const Expression& e2, const Expression& e3) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - this->template reset(e3.root()); - ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() - + e3.traceSize(); - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - this->template expression()->value(values), - none, none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_( - record->template value(), record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian(), record->template jacobian()); - } - -}; -//----------------------------------------------------------------------------- + return value; } + +template +typename Expression::KeysAndDims Expression::keysAndDims() const { + std::map map; + dims(map); + size_t n = map.size(); + KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); + boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); + boost::copy(map | boost::adaptors::map_values, pair.second.begin()); + return pair; +} + +namespace internal { +// http://stackoverflow.com/questions/16260445/boost-bind-to-operator +template +struct apply_compose { + typedef T result_type; + static const int Dim = traits::dimension; + T operator()(const T& x, const T& y, OptionalJacobian H1 = + boost::none, OptionalJacobian H2 = boost::none) const { + return x.compose(y, H1, H2); + } +}; +} + +// Global methods: + +/// Construct a product expression, assumes T::compose(T) -> T +template +Expression operator*(const Expression& expression1, + const Expression& expression2) { + return Expression( + boost::bind(internal::apply_compose(), _1, _2, _3, _4), expression1, + expression2); +} + +/// Construct an array of leaves +template +std::vector > createUnknowns(size_t n, char c, size_t start) { + std::vector > unknowns; + unknowns.reserve(n); + for (size_t i = start; i < start + n; i++) + unknowns.push_back(Expression(c, i)); + return unknowns; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index a39b1557c..d24a568f5 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -19,21 +19,28 @@ #pragma once -#include +#include #include -#include +#include #include -#include -#include +#include +#include +// Forward declare tests class ExpressionFactorShallowTest; namespace gtsam { -// Forward declare +// Forward declares +class Values; template class ExpressionFactor; +namespace internal { +template class ExecutionTrace; +template class ExpressionNode; +} + /** * Expression class that supports automatic differentiation */ @@ -48,110 +55,97 @@ public: private: // Paul's trick shared pointer, polymorphic root of entire expression tree - boost::shared_ptr > root_; + boost::shared_ptr > root_; public: + // Expressions wrap trees of functions that can evaluate their own derivatives. + // The meta-functions below are useful to specify the type of those functions. + // Example, a function taking a camera and a 3D point and yielding a 2D point: + // Expression::BinaryFunction::type + template + struct UnaryFunction { + typedef boost::function< + T(const A1&, typename MakeOptionalJacobian::type)> type; + }; + + template + struct BinaryFunction { + typedef boost::function< + T(const A1&, const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> type; + }; + + template + struct TernaryFunction { + typedef boost::function< + T(const A1&, const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> type; + }; + /// Print - void print(const std::string& s) const { - std::cout << s << *root_ << std::endl; - } + void print(const std::string& s) const; - // Construct a constant expression - Expression(const T& value) : - root_(new ConstantExpression(value)) { - } + /// Construct a constant expression + Expression(const T& value); - // Construct a leaf expression, with Key - Expression(const Key& key) : - root_(new LeafExpression(key)) { - } + /// Construct a leaf expression, with Key + Expression(const Key& key); - // Construct a leaf expression, with Symbol - Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)) { - } + /// Construct a leaf expression, with Symbol + Expression(const Symbol& symbol); - // Construct a leaf expression, creating Symbol - Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { - } + /// Construct a leaf expression, creating Symbol + Expression(unsigned char c, size_t j); + + /// Construct a unary function expression + template + Expression(typename UnaryFunction::type function, + const Expression& expression); + + /// Construct a binary function expression + template + Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2); + + /// Construct a ternary function expression + template + Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3); /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename MakeOptionalJacobian::type) const) : - root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { - } - - /// Construct a unary function expression - template - Expression(typename UnaryExpression::Function function, - const Expression& expression) : - root_(new UnaryExpression(function, expression)) { - } + T (A::*method)(typename MakeOptionalJacobian::type) const); /// Construct a unary method expression template Expression(const Expression& expression1, T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type) const, - const Expression& expression2) : - root_( - new BinaryExpression(boost::bind(method, _1, _2, _3, _4), - expression1, expression2)) { - } - - /// Construct a binary function expression - template - Expression(typename BinaryExpression::Function function, - const Expression& expression1, const Expression& expression2) : - root_(new BinaryExpression(function, expression1, expression2)) { - } + const Expression& expression2); /// Construct a binary method expression template Expression(const Expression& expression1, T (A1::*method)(const A2&, const A3&, - typename TernaryExpression::OJ1, - typename TernaryExpression::OJ2, - typename TernaryExpression::OJ3) const, - const Expression& expression2, const Expression& expression3) : - root_( - new TernaryExpression( - boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, - expression2, expression3)) { - } + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3); - /// Construct a ternary function expression - template - Expression(typename TernaryExpression::Function function, - const Expression& expression1, const Expression& expression2, - const Expression& expression3) : - root_( - new TernaryExpression(function, expression1, - expression2, expression3)) { - } - - /// Return root - const boost::shared_ptr >& root() const { - return root_; - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return root_->traceSize(); + /// Destructor + virtual ~Expression() { } /// Return keys that play in this expression - std::set keys() const { - return root_->keys(); - } + std::set keys() const; /// Return dimensions for each argument, as a map - void dims(std::map& map) const { - root_->dims(map); - } + void dims(std::map& map) const; /** * @brief Return value and optional derivatives, reverse AD version @@ -159,16 +153,7 @@ public: * The order of the Jacobians is same as keys in either keys() or dims() */ T value(const Values& values, boost::optional&> H = - boost::none) const { - - if (H) { - // Call private version that returns derivatives in H - KeysAndDims pair = keysAndDims(); - return value(values, pair.first, pair.second, *H); - } else - // no derivatives needed, just return value - return root_->value(values); - } + boost::none) const; /** * @return a "deep" copy of this Expression @@ -179,116 +164,55 @@ public: return boost::make_shared(*this); } + /// Return root + const boost::shared_ptr >& root() const; + + /// Return size needed for memory buffer in traceExecution + size_t traceSize() const; + private: - /// Vaguely unsafe keys and dimensions in same order + /// Keys and dimensions in same order typedef std::pair, FastVector > KeysAndDims; - KeysAndDims keysAndDims() const { - std::map map; - dims(map); - size_t n = map.size(); - KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); - boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); - boost::copy(map | boost::adaptors::map_values, pair.second.begin()); - return pair; - } + KeysAndDims keysAndDims() const; /// private version that takes keys and dimensions, returns derivatives - T value(const Values& values, const FastVector& keys, - const FastVector& dims, std::vector& H) const { - - // H should be pre-allocated - assert(H.size()==keys.size()); - - // Pre-allocate and zero VerticalBlockMatrix - static const int Dim = traits::dimension; - VerticalBlockMatrix Ab(dims, Dim); - Ab.matrix().setZero(); - JacobianMap jacobianMap(keys, Ab); - - // Call unsafe version - T result = value(values, jacobianMap); - - // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < static_cast(keys.size()); i++) - H[i] = Ab(i); - - return result; - } + T valueAndDerivatives(const Values& values, const FastVector& keys, + const FastVector& dims, std::vector& H) const; /// trace execution, very unsafe - T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return root_->traceExecution(values, trace, traceStorage); - } + T traceExecution(const Values& values, internal::ExecutionTrace& trace, + void* traceStorage) const; - /** - * @brief Return value and derivatives, reverse AD version - * This very unsafe method needs a JacobianMap with correctly allocated - * and initialized VerticalBlockMatrix, hence is declared private. - */ - T value(const Values& values, JacobianMap& jacobians) const { - // The following piece of code is absolutely crucial for performance. - // We allocate a block of memory on the stack, which can be done at runtime - // with modern C++ compilers. The traceExecution then fills this memory - // with an execution trace, made up entirely of "Record" structs, see - // the FunctionalNode class in expression-inl.h - size_t size = traceSize(); - - // Windows does not support variable length arrays, so memory must be dynamically - // allocated on Visual Studio. For more information see the issue below - // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio -#ifdef _MSC_VER - ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size]; -#else - ExecutionTraceStorage traceStorage[size]; -#endif - - ExecutionTrace trace; - T value(traceExecution(values, trace, traceStorage)); - trace.startReverseAD1(jacobians); - -#ifdef _MSC_VER - delete[] traceStorage; -#endif - - return value; - } + /// brief Return value and derivatives, reverse AD version + T valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const; // be very selective on who can access these private methods: friend class ExpressionFactor ; + friend class internal::ExpressionNode; + + // and add tests friend class ::ExpressionFactorShallowTest; - }; -// http://stackoverflow.com/questions/16260445/boost-bind-to-operator -template -struct apply_compose { - typedef T result_type; - static const int Dim = traits::dimension; - T operator()(const T& x, const T& y, OptionalJacobian H1 = - boost::none, OptionalJacobian H2 = boost::none) const { - return x.compose(y, H1, H2); - } -}; - -/// Construct a product expression, assumes T::compose(T) -> T +/** + * Construct a product expression, assumes T::compose(T) -> T + * Example: + * Expression a(0), b(1), c = a*b; + */ template -Expression operator*(const Expression& expression1, - const Expression& expression2) { - return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), - expression1, expression2); -} +Expression operator*(const Expression& e1, const Expression& e2); -/// Construct an array of leaves +/** + * Construct an array of unknown expressions with successive symbol keys + * Example: + * createUnknowns(3,'x') creates unknown expressions for x0,x1,x2 + */ template -std::vector > createUnknowns(size_t n, char c, size_t start = 0) { - std::vector > unknowns; - unknowns.reserve(n); - for (size_t i = start; i < start + n; i++) - unknowns.push_back(Expression(c, i)); - return unknowns; -} +std::vector > createUnknowns(size_t n, char c, size_t start = 0); -} +} // namespace gtsam + +#include diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4769e5048..b32b84df3 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -32,7 +32,9 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { -protected: + protected: + + typedef ExpressionFactor This; T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled @@ -40,7 +42,9 @@ protected: static const int Dim = traits::dimension; -public: + public: + + typedef boost::shared_ptr > shared_ptr; /// Constructor ExpressionFactor(const SharedNoiseModel& noiseModel, // @@ -65,8 +69,8 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if (H) { - const T value = expression_.value(x, keys_, dims_, *H); + if (H) { + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); return traits::Local(measurement_, value); } else { const T value = expression_.value(x); @@ -89,25 +93,30 @@ public: // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); - JacobianMap jacobianMap(keys_, Ab); + internal::JacobianMap jacobianMap(keys_, Ab); // Zero out Jacobian so we can simply add to it Ab.matrix().setZero(); // Get value and Jacobians, writing directly into JacobianFactor - T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! + T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here ! // Evaluate error and set RHS vector b Ab(size()).col(0) = -traits::Local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS - Vector dummy(Dim); - noiseModel_->WhitenSystem(Ab.matrix(), dummy); + Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models + noiseModel_->WhitenSystem(Ab.matrix(), b); return factor; } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } }; // ExpressionFactor -} // \ namespace gtsam +}// \ namespace gtsam diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 41ee52b3a..4e8c0e303 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -19,6 +19,8 @@ #include #include // for selective linearization thresholds #include +#include // for GTSAM_USE_TBB + #include #include @@ -84,11 +86,11 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool u } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, +void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, - FastSet& replacedKeys, Base::Nodes& nodes, - FastSet& fixedVariables) + KeySet& replacedKeys, Base::Nodes& nodes, + KeySet& fixedVariables) { variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); BOOST_FOREACH(Key key, unusedKeys) { @@ -103,10 +105,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVect } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, +KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - FastSet relinKeys; + KeySet relinKeys; if(const double* threshold = boost::get(&relinearizeThreshold)) { @@ -131,7 +133,7 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, +void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization @@ -153,7 +155,7 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thresho } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, +void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { @@ -185,11 +187,11 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, +KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - FastSet relinKeys; + KeySet relinKeys; BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { if(relinearizeThreshold.type() == typeid(double)) CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); @@ -200,7 +202,7 @@ FastSet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& keys, const FastSet& markedMask) +void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask) { static const bool debug = false; // does the separator contain any of the variables? @@ -224,7 +226,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, co /* ************************************************************************* */ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, - const FastSet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) + const KeySet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions @@ -272,7 +274,7 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, /* ************************************************************************* */ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector& roots, - const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold) { + const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; @@ -300,7 +302,7 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector /* ************************************************************************* */ namespace internal { -void updateRgProd(const boost::shared_ptr& clique, const FastSet& replacedKeys, +void updateRgProd(const boost::shared_ptr& clique, const KeySet& replacedKeys, const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need @@ -344,7 +346,7 @@ void updateRgProd(const boost::shared_ptr& clique, const FastSet& replacedKeys, +size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues& RgProd) { // Update variables diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index a8d03df13..d34480144 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -57,10 +57,10 @@ struct GTSAM_EXPORT ISAM2::Impl { /** * Remove variables from the ISAM2 system. */ - static void RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, + static void RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, FastSet& replacedKeys, Base::Nodes& nodes, - FastSet& fixedVariables); + VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes, + KeySet& fixedVariables); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -71,7 +71,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationFull(const VectorValues& delta, + static KeySet CheckRelinearizationFull(const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** @@ -85,7 +85,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationPartial(const FastVector& roots, + static KeySet CheckRelinearizationPartial(const FastVector& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** @@ -103,7 +103,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const FastSet& markedMask); + static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -119,7 +119,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void ExpmapMasked(Values& values, const VectorValues& delta, - const FastSet& mask, + const KeySet& mask, boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); @@ -127,13 +127,13 @@ struct GTSAM_EXPORT ISAM2::Impl { * Update the Newton's method step point, using wildfire */ static size_t UpdateGaussNewtonDelta(const FastVector& roots, - const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold); + const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold); /** * Update the RgProd (R*g) incrementally taking into account which variables * have been recalculated in \c replacedKeys. Only used in Dogleg. */ - static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet& replacedKeys, + static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues& RgProd); /** diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index f7c6d0345..538c66068 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -36,7 +36,7 @@ VALUE ISAM2::calculateEstimate(Key key) const { namespace internal { template void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) + KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the @@ -113,7 +113,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, template bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) + KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the @@ -245,8 +245,8 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh /* ************************************************************************* */ template -size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) { - FastSet changed; +size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { + KeySet changed; int count = 0; // starting from the root, call optimize on each conditional if(root) @@ -256,9 +256,9 @@ size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, /* ************************************************************************* */ template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) +size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { - FastSet changed; + KeySet changed; size_t count = 0; if (root) { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 97735b5d5..52af4fee3 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -98,7 +98,7 @@ DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationMode } /* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) const { +ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) { std::string s = str; boost::algorithm::to_upper(s); if (s == "QR") return ISAM2Params::QR; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; @@ -108,7 +108,7 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::strin } /* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) const { +std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) { std::string s; switch (value) { case ISAM2Params::QR: s = "QR"; break; @@ -194,7 +194,7 @@ FastSet ISAM2::getAffectedFactors(const FastList& keys) const { // (note that the remaining stuff is summarized in the cached factors) GaussianFactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const { gttic(getAffectedFactors); FastSet candidates = getAffectedFactors(affectedKeys); @@ -204,7 +204,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastS gttic(affectedKeysSet); // for fast lookup below - FastSet affectedKeysSet; + KeySet affectedKeysSet; affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); gttoc(affectedKeysSet); @@ -260,9 +260,9 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } /* ************************************************************************* */ -boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, const FastSet& relinKeys, +boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys, const vector& observedKeys, - const FastSet& unusedIndices, + const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result) { @@ -326,7 +326,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result + boost::shared_ptr affectedKeysSet(new KeySet()); // Will return this result if(affectedKeys.size() >= theta_.size() * batchThreshold) { @@ -566,17 +566,17 @@ ISAM2Result ISAM2::update( variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors); // Compute unused keys and indices - FastSet unusedKeys; - FastSet unusedIndices; + KeySet unusedKeys; + KeySet unusedIndices; { // Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors. - FastSet removedAndEmpty; + KeySet removedAndEmpty; BOOST_FOREACH(Key key, removeFactors.keys()) { if(variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } - FastSet newFactorSymbKeys = newFactors.keys(); + KeySet newFactorSymbKeys = newFactors.keys(); std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); @@ -602,10 +602,10 @@ ISAM2Result ISAM2::update( gttic(gather_involved_keys); // 3. Mark linear update - FastSet markedKeys = newFactors.keys(); // Get keys from new factors + KeySet markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors { - FastSet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors + KeySet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys @@ -632,7 +632,7 @@ ISAM2Result ISAM2::update( gttoc(gather_involved_keys); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold - FastSet relinKeys; + KeySet relinKeys; if (relinearizeThisStep) { gttic(gather_relinearize_keys); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. @@ -659,7 +659,7 @@ ISAM2Result ISAM2::update( result.detail->variableStatus[key].isRelinearized = true; } } // Add the variables being relinearized to the marked keys - FastSet markedRelinMask; + KeySet markedRelinMask; BOOST_FOREACH(const Key key, relinKeys) markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); @@ -674,7 +674,7 @@ ISAM2Result ISAM2::update( // Relin involved keys for detailed results if(params_.enableDetailedResults) { - FastSet involvedRelinKeys; + KeySet involvedRelinKeys; BOOST_FOREACH(const sharedClique& root, roots_) Impl::FindAll(root, involvedRelinKeys, markedRelinMask); BOOST_FOREACH(Key key, involvedRelinKeys) { @@ -726,7 +726,7 @@ ISAM2Result ISAM2::update( gttic(recalculate); // 8. Redo top of Bayes tree - boost::shared_ptr > replacedKeys; + boost::shared_ptr replacedKeys; if(!markedKeys.empty() || !observedKeys.empty()) replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); @@ -758,7 +758,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, boost::optional&> deletedFactorsIndices) { // Convert to ordered set - FastSet leafKeys(leafKeysList.begin(), leafKeysList.end()); + KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. @@ -769,7 +769,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, FastSet factorIndicesToRemove; // Keep track of variables removed in subtrees - FastSet leafKeysRemoved; + KeySet leafKeysRemoved; // Remove each variable and its subtrees BOOST_FOREACH(Key j, leafKeys) { @@ -881,7 +881,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the conditional - const FastSet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); + const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); FastVector cliqueFrontalsToEliminate; std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), std::back_inserter(cliqueFrontalsToEliminate)); @@ -957,7 +957,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(FastSet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, nodes_, fixedVariables_); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 9adceee6a..fdc0021e5 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -186,6 +186,7 @@ struct GTSAM_EXPORT ISAM2Params { enableDetailedResults(false), enablePartialRelinearizationCheck(false), findUnusedFactorSlots(false) {} + /// print iSAM2 parameters void print(const std::string& str = "") const { std::cout << str << "\n"; if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) @@ -214,7 +215,9 @@ struct GTSAM_EXPORT ISAM2Params { std::cout.flush(); } - /** Getters and Setters for all properties */ + /// @name Getters and Setters for all properties + /// @{ + OptimizationParams getOptimizationParams() const { return this->optimizationParams; } RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; } int getRelinearizeSkip() const { return relinearizeSkip; } @@ -237,14 +240,21 @@ struct GTSAM_EXPORT ISAM2Params { void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; } void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; } - Factorization factorizationTranslator(const std::string& str) const; - std::string factorizationTranslator(const Factorization& value) const; - GaussianFactorGraph::Eliminate getEliminationFunction() const { return factorization == CHOLESKY ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky : (GaussianFactorGraph::Eliminate)EliminateQR; } + + /// @} + + /// @name Some utilities + /// @{ + + static Factorization factorizationTranslator(const std::string& str); + static std::string factorizationTranslator(const Factorization& value); + + /// @} }; @@ -404,7 +414,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(cachedFactor_); ar & BOOST_SERIALIZATION_NVP(gradientContribution_); @@ -450,7 +460,7 @@ protected: * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable FastSet deltaReplacedMask_; // TODO: Make sure accessed in the right way + mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way /** All original nonlinear factors are stored here to use during relinearization */ NonlinearFactorGraph nonlinearFactors_; @@ -466,7 +476,7 @@ protected: /** Set of variables that are involved with linear factors from marginalized * variables and thus cannot have their linearization points changed. */ - FastSet fixedVariables_; + KeySet fixedVariables_; int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization @@ -544,8 +554,15 @@ public: boost::optional&> marginalFactorsIndices = boost::none, boost::optional&> deletedFactorsIndices = boost::none); - /** Access the current linearization point */ - const Values& getLinearizationPoint() const { return theta_; } + /// Access the current linearization point + const Values& getLinearizationPoint() const { + return theta_; + } + + /// Check whether variable with given key exists in linearization point + bool valueExists(Key key) const { + return theta_.exists(key); + } /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only @@ -597,7 +614,7 @@ public: const VariableIndex& getVariableIndex() const { return variableIndex_; } /** Access the nonlinear variable index */ - const FastSet& getFixedVariables() const { return fixedVariables_; } + const KeySet& getFixedVariables() const { return fixedVariables_; } size_t lastAffectedVariableCount; size_t lastAffectedFactorCount; @@ -624,11 +641,11 @@ public: protected: FastSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; + GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); - virtual boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const std::vector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); + virtual boost::shared_ptr recalculate(const KeySet& markedKeys, const KeySet& relinKeys, + const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 @@ -649,11 +666,11 @@ template<> struct traits : public Testable {}; /// @return The number of variables that were solved for template size_t optimizeWildfire(const boost::shared_ptr& root, - double threshold, const FastSet& replaced, VectorValues& delta); + double threshold, const KeySet& replaced, VectorValues& delta); template size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const FastSet& replaced, VectorValues& delta); + double threshold, const KeySet& replaced, VectorValues& delta); /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) template diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index ef1f1eff1..59bb2d42b 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -25,9 +25,10 @@ #include #include +#include +#include #include #include -#include using namespace std; @@ -42,6 +43,8 @@ LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTrans boost::algorithm::to_upper(s); if (s == "SILENT") return LevenbergMarquardtParams::SILENT; + if (s == "SUMMARY") + return LevenbergMarquardtParams::SUMMARY; if (s == "LAMBDA") return LevenbergMarquardtParams::LAMBDA; if (s == "TRYLAMBDA") @@ -65,6 +68,9 @@ std::string LevenbergMarquardtParams::verbosityLMTranslator( case LevenbergMarquardtParams::SILENT: s = "SILENT"; break; + case LevenbergMarquardtParams::SUMMARY: + s = "SUMMARY"; + break; case LevenbergMarquardtParams::TERMINATION: s = "TERMINATION"; break; @@ -99,8 +105,8 @@ void LevenbergMarquardtParams::print(const std::string& str) const { std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n"; std::cout << " minModelFidelity: " << minModelFidelity << "\n"; std::cout << " diagonalDamping: " << diagonalDamping << "\n"; - std::cout << " min_diagonal: " << min_diagonal_ << "\n"; - std::cout << " max_diagonal: " << max_diagonal_ << "\n"; + std::cout << " minDiagonal: " << minDiagonal << "\n"; + std::cout << " maxDiagonal: " << maxDiagonal << "\n"; std::cout << " verbosityLM: " << verbosityLMTranslator(verbosityLM) << "\n"; std::cout.flush(); @@ -113,19 +119,19 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { /* ************************************************************************* */ void LevenbergMarquardtOptimizer::increaseLambda() { - if (params_.useFixedLambdaFactor_) { + if (params_.useFixedLambdaFactor) { state_.lambda *= params_.lambdaFactor; } else { state_.lambda *= params_.lambdaFactor; params_.lambdaFactor *= 2.0; } - params_.reuse_diagonal_ = true; + state_.reuseDiagonal = true; } /* ************************************************************************* */ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { - if (params_.useFixedLambdaFactor_) { + if (params_.useFixedLambdaFactor) { state_.lambda /= params_.lambdaFactor; } else { // CHECK_GT(step_quality, 0.0); @@ -133,7 +139,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { params_.lambdaFactor = 2.0; } state_.lambda = std::max(params_.lambdaLowerBound, state_.lambda); - params_.reuse_diagonal_ = false; + state_.reuseDiagonal = false; } @@ -146,12 +152,12 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( cout << "building damped system with lambda " << state_.lambda << endl; // Only retrieve diagonal vector when reuse_diagonal = false - if (params_.diagonalDamping && params_.reuse_diagonal_ == false) { + if (params_.diagonalDamping && state_.reuseDiagonal == false) { state_.hessianDiagonal = linear.hessianDiagonal(); BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { for (int aa = 0; aa < v.size(); aa++) { - v(aa) = std::min(std::max(v(aa), params_.min_diagonal_), - params_.max_diagonal_); + v(aa) = std::min(std::max(v(aa), params_.minDiagonal), + params_.maxDiagonal); v(aa) = sqrt(v(aa)); } } @@ -173,7 +179,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); damped += boost::make_shared(key_vector.first, A, b, model); - } catch (std::exception e) { + } catch (const std::exception& e) { // Don't attempt any damping if no key found in diagonal continue; } @@ -219,9 +225,15 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); - if(state_.totalNumberInnerIterations==0) // write initial error + if(state_.totalNumberInnerIterations==0) { // write initial error writeLogFile(state_.error); + if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { + cout << "Initial error: " << state_.error << ", values: " << state_.values.size() + << std::endl; + } + } + // Keep increasing lambda until we make make progress while (true) { @@ -236,7 +248,7 @@ void LevenbergMarquardtOptimizer::iterate() { double modelFidelity = 0.0; bool step_is_successful = false; bool stopSearchingLambda = false; - double newError; + double newError = numeric_limits::infinity(); Values newValues; VectorValues delta; @@ -245,13 +257,14 @@ void LevenbergMarquardtOptimizer::iterate() { // ============ Solve is where most computation happens !! ================= delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - // ========================================================================= - } catch (IndeterminantLinearSystemException) { + } catch (const IndeterminantLinearSystemException& e) { systemSolvedSuccessfully = false; } + double linearizedCostChange = 0, + newlinearizedError = 0; if (systemSolvedSuccessfully) { - params_.reuse_diagonal_ = true; + state_.reuseDiagonal = true; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl; @@ -259,9 +272,9 @@ void LevenbergMarquardtOptimizer::iterate() { delta.print("delta"); // cost change in the linearized system (old - new) - double newlinearizedError = linear->error(delta); + newlinearizedError = linear->error(delta); - double linearizedCostChange = state_.error - newlinearizedError; + linearizedCostChange = state_.error - newlinearizedError; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "newlinearizedError = " << newlinearizedError << " linearizedCostChange = " << linearizedCostChange << endl; @@ -308,6 +321,12 @@ void LevenbergMarquardtOptimizer::iterate() { } } + if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { + cout << "[" << state_.iterations << "]: " << "new error = " << newlinearizedError + << ", delta = " << linearizedCostChange << ", lambda = " << state_.lambda + << ", success = " << systemSolvedSuccessfully << std::endl; + } + ++state_.totalNumberInnerIterations; if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity @@ -324,7 +343,8 @@ void LevenbergMarquardtOptimizer::iterate() { // check if lambda is too big if (state_.lambda >= params_.lambdaUpperBound) { - if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION) + if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION || + lmVerbosity == LevenbergMarquardtParams::SUMMARY) cout << "Warning: Levenberg-Marquardt giving up because " "cannot decrease error with maximum lambda" << endl; break; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 20f9dec3c..a965c6cf0 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -38,7 +38,7 @@ class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { public: /** See LevenbergMarquardtParams::lmVerbosity */ enum VerbosityLM { - SILENT = 0, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA + SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA }; static VerbosityLM verbosityLMTranslator(const std::string &s); @@ -52,70 +52,84 @@ public: double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration - std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] + std::string logFile; ///< an optional CSV log file, with [iteration, time, error, lambda] bool diagonalDamping; ///< if true, use diagonal of Hessian - bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (related to efficiency) - bool useFixedLambdaFactor_; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor - double min_diagonal_; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) - double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) + bool useFixedLambdaFactor; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor + double minDiagonal; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) + double maxDiagonal; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) - LevenbergMarquardtParams() : - lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound( - 0.0), verbosityLM(SILENT), minModelFidelity(1e-3), - diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true), - min_diagonal_(1e-6), max_diagonal_(1e32) { - } - virtual ~LevenbergMarquardtParams() { + LevenbergMarquardtParams() + : verbosityLM(SILENT), + diagonalDamping(false), + minDiagonal(1e-6), + maxDiagonal(1e32) { + SetLegacyDefaults(this); } + static void SetLegacyDefaults(LevenbergMarquardtParams* p) { + // Relevant NonlinearOptimizerParams: + p->maxIterations = 100; + p->relativeErrorTol = 1e-5; + p->absoluteErrorTol = 1e-5; + // LM-specific: + p->lambdaInitial = 1e-5; + p->lambdaFactor = 10.0; + p->lambdaUpperBound = 1e5; + p->lambdaLowerBound = 0.0; + p->minModelFidelity = 1e-3; + p->diagonalDamping = false; + p->useFixedLambdaFactor = true; + } + + // these do seem to work better for SFM + static void SetCeresDefaults(LevenbergMarquardtParams* p) { + // Relevant NonlinearOptimizerParams: + p->maxIterations = 50; + p->absoluteErrorTol = 0; // No corresponding option in CERES + p->relativeErrorTol = 1e-6; // This is function_tolerance + // LM-specific: + p->lambdaUpperBound = 1e32; + p->lambdaLowerBound = 1e-16; + p->lambdaInitial = 1e-04; + p->lambdaFactor = 2.0; + p->minModelFidelity = 1e-3; // options.min_relative_decrease in CERES + p->diagonalDamping = true; + p->useFixedLambdaFactor = false; // This is important + } + + static LevenbergMarquardtParams LegacyDefaults() { + LevenbergMarquardtParams p; + SetLegacyDefaults(&p); + return p; + } + + static LevenbergMarquardtParams CeresDefaults() { + LevenbergMarquardtParams p; + SetCeresDefaults(&p); + return p; + } + + virtual ~LevenbergMarquardtParams() {} virtual void print(const std::string& str = "") const; - inline double getlambdaInitial() const { - return lambdaInitial; - } - inline double getlambdaFactor() const { - return lambdaFactor; - } - inline double getlambdaUpperBound() const { - return lambdaUpperBound; - } - inline double getlambdaLowerBound() const { - return lambdaLowerBound; - } - inline std::string getVerbosityLM() const { - return verbosityLMTranslator(verbosityLM); - } - inline std::string getLogFile() const { - return logFile; - } - inline bool getDiagonalDamping() const { - return diagonalDamping; - } - - inline void setlambdaInitial(double value) { - lambdaInitial = value; - } - inline void setlambdaFactor(double value) { - lambdaFactor = value; - } - inline void setlambdaUpperBound(double value) { - lambdaUpperBound = value; - } - inline void setlambdaLowerBound(double value) { - lambdaLowerBound = value; - } - inline void setVerbosityLM(const std::string &s) { - verbosityLM = verbosityLMTranslator(s); - } - inline void setLogFile(const std::string &s) { - logFile = s; - } - inline void setDiagonalDamping(bool flag) { - diagonalDamping = flag; - } - inline void setUseFixedLambdaFactor(bool flag) { - useFixedLambdaFactor_ = flag; - } + /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. + /// @{ + bool getDiagonalDamping() const { return diagonalDamping; } + double getlambdaFactor() const { return lambdaFactor; } + double getlambdaInitial() const { return lambdaInitial; } + double getlambdaLowerBound() const { return lambdaLowerBound; } + double getlambdaUpperBound() const { return lambdaUpperBound; } + std::string getLogFile() const { return logFile; } + std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} + void setDiagonalDamping(bool flag) { diagonalDamping = flag; } + void setlambdaFactor(double value) { lambdaFactor = value; } + void setlambdaInitial(double value) { lambdaInitial = value; } + void setlambdaLowerBound(double value) { lambdaLowerBound = value; } + void setlambdaUpperBound(double value) { lambdaUpperBound = value; } + void setLogFile(const std::string& s) { logFile = s; } + void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} + void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} + // @} }; /** @@ -125,13 +139,12 @@ class GTSAM_EXPORT LevenbergMarquardtState: public NonlinearOptimizerState { public: double lambda; - int totalNumberInnerIterations; // The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) boost::posix_time::ptime startTime; - VectorValues hessianDiagonal; //only update hessianDiagonal when reuse_diagonal_ = false + int totalNumberInnerIterations; //< The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) + VectorValues hessianDiagonal; //< we only update hessianDiagonal when reuseDiagonal = false + bool reuseDiagonal; ///< an additional option in Ceres for diagonalDamping - LevenbergMarquardtState() { - initTime(); - } + LevenbergMarquardtState() {} // used in LM constructor but immediately overwritten void initTime() { startTime = boost::posix_time::microsec_clock::universal_time(); @@ -145,7 +158,7 @@ protected: const Values& initialValues, const LevenbergMarquardtParams& params, unsigned int iterations = 0) : NonlinearOptimizerState(graph, initialValues, iterations), lambda( - params.lambdaInitial), totalNumberInnerIterations(0) { + params.lambdaInitial), totalNumberInnerIterations(0),reuseDiagonal(false) { initTime(); } @@ -175,12 +188,12 @@ public: * @param initialValues The initial variable assignments * @param params The optimization parameters */ - LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, - const Values& initialValues, const LevenbergMarquardtParams& params = - LevenbergMarquardtParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_( - graph, initialValues, params_) { - } + LevenbergMarquardtOptimizer( + const NonlinearFactorGraph& graph, const Values& initialValues, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) + : NonlinearOptimizer(graph), + params_(ensureHasOrdering(params, graph)), + state_(graph, initialValues, params_) {} /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -189,9 +202,11 @@ public: * @param graph The nonlinear factor graph to optimize * @param initialValues The initial variable assignments */ - LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, - const Values& initialValues, const Ordering& ordering) : - NonlinearOptimizer(graph) { + LevenbergMarquardtOptimizer( + const NonlinearFactorGraph& graph, const Values& initialValues, + const Ordering& ordering, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) + : NonlinearOptimizer(graph), params_(params) { params_.ordering = ordering; state_ = LevenbergMarquardtState(graph, initialValues, params_); } diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 676b3fb85..3ae8d4c98 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -146,7 +146,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(factor_); diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 107ec7d3f..6d7196ff5 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -1,7 +1,18 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -16,36 +27,49 @@ using namespace std; namespace gtsam { -/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering - * Can be moved to NonlinearFactorGraph.h if desired */ -VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) { +/** + * @brief Return the gradient vector of a nonlinear factor graph + * @param nfg the graph + * @param values a linearization point + * Can be moved to NonlinearFactorGraph.h if desired + */ +static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, + const Values &values) { // Linearize graph GaussianFactorGraph::shared_ptr linear = nfg.linearize(values); return linear->gradientAtZero(); } -double NonlinearConjugateGradientOptimizer::System::error(const State &state) const { +double NonlinearConjugateGradientOptimizer::System::error( + const State &state) const { return graph_.error(state); } -NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const { +NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient( + const State &state) const { return gradientInPlace(graph_, state); } -NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { + +NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance( + const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; step *= alpha; return current.retract(step); } void NonlinearConjugateGradientOptimizer::iterate() { - int dummy ; - boost::tie(state_.values, dummy) = nonlinearConjugateGradient(System(graph_), state_.values, params_, true /* single iterations */); + int dummy; + boost::tie(state_.values, dummy) = nonlinearConjugateGradient( + System(graph_), state_.values, params_, true /* single iterations */); ++state_.iterations; state_.error = graph_.error(state_.values); } const Values& NonlinearConjugateGradientOptimizer::optimize() { - boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient(System(graph_), state_.values, params_, false /* up to convergent */); + // Optimize until convergence + System system(graph_); + boost::tie(state_.values, state_.iterations) = // + nonlinearConjugateGradient(system, state_.values, params_, false); state_.error = graph_.error(state_.values); return state_.values; } diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 1ad8fa2ab..04d4734a4 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -1,8 +1,19 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief + * @file NonlinearConjugateGradientOptimizer.h + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG * @author Yong-Dian Jian - * @date Jun 11, 2012 + * @date June 11, 2012 */ #pragma once @@ -13,15 +24,18 @@ namespace gtsam { -/** An implementation of the nonlinear cg method using the template below */ -class GTSAM_EXPORT NonlinearConjugateGradientState : public NonlinearOptimizerState { +/** An implementation of the nonlinear CG method using the template below */ +class GTSAM_EXPORT NonlinearConjugateGradientState: public NonlinearOptimizerState { public: typedef NonlinearOptimizerState Base; - NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, const Values& values) - : Base(graph, values) {} + NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, + const Values& values) : + Base(graph, values) { + } }; -class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer { +class GTSAM_EXPORT NonlinearConjugateGradientOptimizer: public NonlinearOptimizer { + /* a class for the nonlinearConjugateGradient template */ class System { public: @@ -33,37 +47,49 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz const NonlinearFactorGraph &graph_; public: - System(const NonlinearFactorGraph &graph): graph_(graph) {} - double error(const State &state) const ; - Gradient gradient(const State &state) const ; - State advance(const State ¤t, const double alpha, const Gradient &g) const ; + System(const NonlinearFactorGraph &graph) : + graph_(graph) { + } + double error(const State &state) const; + Gradient gradient(const State &state) const; + State advance(const State ¤t, const double alpha, + const Gradient &g) const; }; public: + typedef NonlinearOptimizer Base; - typedef NonlinearConjugateGradientState States; typedef NonlinearOptimizerParams Parameters; typedef boost::shared_ptr shared_ptr; protected: - States state_; + NonlinearConjugateGradientState state_; Parameters params_; public: - NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const Parameters& params = Parameters()) - : Base(graph), state_(graph, initialValues), params_(params) {} + /// Constructor + NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const Parameters& params = Parameters()) : + Base(graph), state_(graph, initialValues), params_(params) { + } + + /// Destructor + virtual ~NonlinearConjugateGradientOptimizer() { + } - virtual ~NonlinearConjugateGradientOptimizer() {} virtual void iterate(); - virtual const Values& optimize (); - virtual const NonlinearOptimizerState& _state() const { return state_; } - virtual const NonlinearOptimizerParams& _params() const { return params_; } + virtual const Values& optimize(); + virtual const NonlinearOptimizerState& _state() const { + return state_; + } + virtual const NonlinearOptimizerParams& _params() const { + return params_; + } }; /** Implement the golden-section line search algorithm */ -template +template double lineSearch(const S &system, const V currentValues, const W &gradient) { /* normalize it such that it becomes a unit vector */ @@ -71,37 +97,40 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { // perform the golden section search algorithm to decide the the optimal step size // detail refer to http://en.wikipedia.org/wiki/Golden_section_search - const double phi = 0.5*(1.0+std::sqrt(5.0)), resphi = 2.0 - phi, tau = 1e-5; - double minStep = -1.0/g, maxStep = 0, - newStep = minStep + (maxStep-minStep) / (phi+1.0) ; + const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau = + 1e-5; + double minStep = -1.0 / g, maxStep = 0, newStep = minStep + + (maxStep - minStep) / (phi + 1.0); V newValues = system.advance(currentValues, newStep, gradient); double newError = system.error(newValues); while (true) { - const bool flag = (maxStep - newStep > newStep - minStep) ? true : false ; - const double testStep = flag ? - newStep + resphi * (maxStep - newStep) : newStep - resphi * (newStep - minStep); + const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; + const double testStep = + flag ? newStep + resphi * (maxStep - newStep) : + newStep - resphi * (newStep - minStep); - if ( (maxStep- minStep) < tau * (std::fabs(testStep) + std::fabs(newStep)) ) { - return 0.5*(minStep+maxStep); + if ((maxStep - minStep) + < tau * (std::fabs(testStep) + std::fabs(newStep))) { + return 0.5 * (minStep + maxStep); } const V testValues = system.advance(currentValues, testStep, gradient); const double testError = system.error(testValues); // update the working range - if ( testError >= newError ) { - if ( flag ) maxStep = testStep; - else minStep = testStep; - } - else { - if ( flag ) { + if (testError >= newError) { + if (flag) + maxStep = testStep; + else + minStep = testStep; + } else { + if (flag) { minStep = newStep; newStep = testStep; newError = testError; - } - else { + } else { maxStep = newStep; newStep = testStep; newError = testError; @@ -112,7 +141,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { } /** - * Implement the nonlinear conjugate gradient method using the Polak-Ribieve formula suggested in + * Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in * http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method. * * The S (system) class requires three member functions: error(state), gradient(state) and @@ -120,8 +149,10 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { * * The last parameter is a switch between gradient-descent and conjugate gradient */ -template -boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { +template +boost::tuple nonlinearConjugateGradient(const S &system, + const V &initial, const NonlinearOptimizerParams ¶ms, + const bool singleIteration, const bool gradientDescent = false) { // GTSAM_CONCEPT_MANIFOLD_TYPE(V); @@ -129,57 +160,67 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &initia // check if we're already close enough double currentError = system.error(initial); - if(currentError <= params.errorTol) { - if (params.verbosity >= NonlinearOptimizerParams::ERROR){ - std::cout << "Exiting, as error = " << currentError << " < " << params.errorTol << std::endl; + if (currentError <= params.errorTol) { + if (params.verbosity >= NonlinearOptimizerParams::ERROR) { + std::cout << "Exiting, as error = " << currentError << " < " + << params.errorTol << std::endl; } return boost::tie(initial, iteration); } V currentValues = initial; - typename S::Gradient currentGradient = system.gradient(currentValues), prevGradient, - direction = currentGradient; + typename S::Gradient currentGradient = system.gradient(currentValues), + prevGradient, direction = currentGradient; /* do one step of gradient descent */ - V prevValues = currentValues; double prevError = currentError; + V prevValues = currentValues; + double prevError = currentError; double alpha = lineSearch(system, currentValues, direction); currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "Initial error: " << currentError << std::endl; // Iterative loop do { - if ( gradientDescent == true) { + if (gradientDescent == true) { direction = system.gradient(currentValues); - } - else { + } else { prevGradient = currentGradient; currentGradient = system.gradient(currentValues); - const double beta = std::max(0.0, currentGradient.dot(currentGradient-prevGradient)/currentGradient.dot(currentGradient)); - direction = currentGradient + (beta*direction); + // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 + const double beta = std::max(0.0, + currentGradient.dot(currentGradient - prevGradient) + / prevGradient.dot(prevGradient)); + direction = currentGradient + (beta * direction); } alpha = lineSearch(system, currentValues, direction); - prevValues = currentValues; prevError = currentError; + prevValues = currentValues; + prevError = currentError; currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl; - } while( ++iteration < params.maxIterations && - !singleIteration && - !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity)); + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; + } while (++iteration < params.maxIterations && !singleIteration + && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, + params.errorTol, prevError, currentError, params.verbosity)); // Printing if verbose - if (params.verbosity >= NonlinearOptimizerParams::ERROR && iteration >= params.maxIterations) - std::cout << "nonlinearConjugateGradient: Terminating because reached maximum iterations" << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR + && iteration >= params.maxIterations) + std::cout + << "nonlinearConjugateGradient: Terminating because reached maximum iterations" + << std::endl; return boost::tie(currentValues, iteration); } -} +} // \ namespace gtsam diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 6e348fb58..86fb34fe0 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -21,21 +21,14 @@ #include #include +#include + #include #include #include namespace gtsam { -/** - * Template default compare function that assumes a testable T - */ -template -bool compare(const T& a, const T& b) { - GTSAM_CONCEPT_TESTABLE_TYPE(T); - return a.equals(b); -} - /** * An equality factor that forces either one variable to a constant, * or a set of variables to be equal to each other. @@ -76,7 +69,9 @@ public: /** * Function that compares two values */ - bool (*compare_)(const T& a, const T& b); + typedef boost::function CompareFunction; + CompareFunction compare_; +// bool (*compare_)(const T& a, const T& b); /** default constructor - only for serialization */ NonlinearEquality() { @@ -92,7 +87,7 @@ public: * Constructor - forces exact evaluation */ NonlinearEquality(Key j, const T& feasible, - bool (*_compare)(const T&, const T&) = compare) : + const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { @@ -102,7 +97,7 @@ public: * Constructor - allows inexact evaluation */ NonlinearEquality(Key j, const T& feasible, double error_gain, - bool (*_compare)(const T&, const T&) = compare) : + const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { @@ -122,7 +117,7 @@ public: /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This* e = dynamic_cast(&f); - return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) + return e && Base::equals(f) && traits::Equals(feasible_,e->feasible_, tol) && std::abs(error_gain_ - e->error_gain_) < tol; } @@ -185,7 +180,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -273,7 +268,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -337,7 +332,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 5d9f176b3..5b46742e7 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -112,7 +112,7 @@ public: * when the constraint is *NOT* fulfilled. * @return true if the constraint is active */ - virtual bool active(const Values& c) const { return true; } + virtual bool active(const Values& /*c*/) const { return true; } /** linearize to a GaussianFactor */ virtual boost::shared_ptr @@ -247,7 +247,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(noiseModel_); @@ -325,7 +325,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -401,7 +401,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -478,7 +478,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -559,7 +559,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -644,7 +644,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } @@ -733,7 +733,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 99a58a989..b81332f5e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -17,22 +17,24 @@ * @author Christian Potthast */ -#include -#include -#include #include #include -#include -#include #include -#include #include #include +#include +#include +#include +#include // for GTSAM_USE_TBB #ifdef GTSAM_USE_TBB # include #endif +#include +#include +#include + using namespace std; namespace gtsam { @@ -47,11 +49,12 @@ double NonlinearFactorGraph::probPrime(const Values& c) const { /* ************************************************************************* */ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const { - cout << str << "size: " << size() << endl; + cout << str << "size: " << size() << endl << endl; for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; - ss << "factor " << i << ": "; + ss << "Factor " << i << ": "; if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); + cout << endl; } } @@ -62,85 +65,51 @@ bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol) /* ************************************************************************* */ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, - const GraphvizFormatting& graphvizFormatting, + const GraphvizFormatting& formatting, const KeyFormatter& keyFormatter) const { stm << "graph {\n"; - stm << " size=\"" << graphvizFormatting.figureWidthInches << "," << - graphvizFormatting.figureHeightInches << "\";\n\n"; + stm << " size=\"" << formatting.figureWidthInches << "," << + formatting.figureHeightInches << "\";\n\n"; - FastSet keys = this->keys(); + KeySet keys = this->keys(); // Local utility function to extract x and y coordinates struct { boost::optional operator()( const Value& value, const GraphvizFormatting& graphvizFormatting) { - if(const Pose2* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = 0.0; break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = 0.0; break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = 0.0; break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = 0.0; break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); - } else if(const Pose3* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = p->z(); break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = p->z(); break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); - } else if(const Point3* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = p->z(); break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = p->z(); break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); + Vector3 t; + if (const GenericValue* p = dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t = p->value().translation().vector(); + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t = p->value().vector(); } else { return boost::none; } + double x, y; + switch (graphvizFormatting.paperHorizontalAxis) { + case GraphvizFormatting::X: x = t.x(); break; + case GraphvizFormatting::Y: x = t.y(); break; + case GraphvizFormatting::Z: x = t.z(); break; + case GraphvizFormatting::NEGX: x = -t.x(); break; + case GraphvizFormatting::NEGY: x = -t.y(); break; + case GraphvizFormatting::NEGZ: x = -t.z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + switch (graphvizFormatting.paperVerticalAxis) { + case GraphvizFormatting::X: y = t.x(); break; + case GraphvizFormatting::Y: y = t.y(); break; + case GraphvizFormatting::Z: y = t.z(); break; + case GraphvizFormatting::NEGX: y = -t.x(); break; + case GraphvizFormatting::NEGY: y = -t.y(); break; + case GraphvizFormatting::NEGZ: y = -t.z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + return Point2(x,y); }} getXY; // Find bounds @@ -148,7 +117,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, double minY = numeric_limits::infinity(), maxY = -numeric_limits::infinity(); BOOST_FOREACH(Key key, keys) { if(values.exists(key)) { - boost::optional xy = getXY(values.at(key), graphvizFormatting); + boost::optional xy = getXY(values.at(key), formatting); if(xy) { if(xy->x() < minX) minX = xy->x(); @@ -163,33 +132,22 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } // Create nodes for each variable in the graph - bool firstTimePoses = true; - Key lastKey; - BOOST_FOREACH(Key key, keys) { + BOOST_FOREACH(Key key, keys){ // Label the node with the label from the KeyFormatter stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if(values.exists(key)) { - boost::optional xy = getXY(values.at(key), graphvizFormatting); + boost::optional xy = getXY(values.at(key), formatting); if(xy) - stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\""; + stm << ", pos=\"" << formatting.scale*(xy->x() - minX) << "," << formatting.scale*(xy->y() - minY) << "!\""; } stm << "];\n"; - - if (firstTimePoses) { - lastKey = key; - firstTimePoses = false; - } else { - stm << " var" << key << "--" << "var" << lastKey << ";\n"; - lastKey = key; - } } stm << "\n"; - - if(graphvizFormatting.mergeSimilarFactors) { + if (formatting.mergeSimilarFactors) { // Remove duplicate factors - FastSet > structure; - BOOST_FOREACH(const sharedFactor& factor, *this) { + std::set > structure; + BOOST_FOREACH(const sharedFactor& factor, *this){ if(factor) { vector factorKeys = factor->keys(); std::sort(factorKeys.begin(), factorKeys.end()); @@ -199,57 +157,65 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections size_t i = 0; - BOOST_FOREACH(const vector& factorKeys, structure) { + BOOST_FOREACH(const vector& factorKeys, structure){ // Make each factor a dot stm << " factor" << i << "[label=\"\", shape=point"; { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; + map::const_iterator pos = formatting.factorPositions.find(i); + if(pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," + << formatting.scale*(pos->second.y() - minY) << "!\""; } stm << "];\n"; // Make factor-variable connections BOOST_FOREACH(Key key, factorKeys) { - stm << " var" << key << "--" << "factor" << i << ";\n"; } + stm << " var" << key << "--" << "factor" << i << ";\n"; + } ++ i; } } else { // Create factors and variable connections for(size_t i = 0; i < this->size(); ++i) { - if(graphvizFormatting.plotFactorPoints){ - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point"; - { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; - } - stm << "];\n"; + const NonlinearFactor::shared_ptr& factor = this->at(i); + if(formatting.plotFactorPoints) { + const FastVector& keys = factor->keys(); + if (formatting.binaryEdges && keys.size()==2) { + stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n"; + } else { + // Make each factor a dot + stm << " factor" << i << "[label=\"\", shape=point"; + { + map::const_iterator pos = formatting.factorPositions.find(i); + if(pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," + << formatting.scale*(pos->second.y() - minY) << "!\""; + } + stm << "];\n"; - // Make factor-variable connections - if(graphvizFormatting.connectKeysToFactor && this->at(i)) { - BOOST_FOREACH(Key key, *this->at(i)) { - stm << " var" << key << "--" << "factor" << i << ";\n"; + // Make factor-variable connections + if(formatting.connectKeysToFactor && factor) { + BOOST_FOREACH(Key key, *factor) { + stm << " var" << key << "--" << "factor" << i << ";\n"; + } + } + } } - } - - } else { - if(this->at(i)) { - Key k; - bool firstTime = true; - BOOST_FOREACH(Key key, *this->at(i)) { - if(firstTime){ - k = key; - firstTime = false; - continue; + if(factor) { + Key k; + bool firstTime = true; + BOOST_FOREACH(Key key, *this->at(i)) { + if(firstTime) { + k = key; + firstTime = false; + continue; + } + stm << " var" << key << "--" << "var" << k << ";\n"; + k = key; + } } - stm << " var" << key << "--" << "var" << k << ";\n"; - k = key; - } - } } } } @@ -270,8 +236,8 @@ double NonlinearFactorGraph::error(const Values& c) const { } /* ************************************************************************* */ -FastSet NonlinearFactorGraph::keys() const { - FastSet keys; +KeySet NonlinearFactorGraph::keys() const { + KeySet keys; BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) keys.insert(factor->begin(), factor->end()); @@ -312,23 +278,26 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const namespace { #ifdef GTSAM_USE_TBB - struct _LinearizeOneFactor { - const NonlinearFactorGraph& graph; - const Values& linearizationPoint; - GaussianFactorGraph& result; - _LinearizeOneFactor(const NonlinearFactorGraph& graph, const Values& linearizationPoint, GaussianFactorGraph& result) : - graph(graph), linearizationPoint(linearizationPoint), result(result) {} - void operator()(const tbb::blocked_range& r) const - { - for(size_t i = r.begin(); i != r.end(); ++i) - { - if(graph[i]) - result[i] = graph[i]->linearize(linearizationPoint); - else - result[i] = GaussianFactor::shared_ptr(); - } +class _LinearizeOneFactor { + const NonlinearFactorGraph& nonlinearGraph_; + const Values& linearizationPoint_; + GaussianFactorGraph& result_; +public: + // Create functor with constant parameters + _LinearizeOneFactor(const NonlinearFactorGraph& graph, + const Values& linearizationPoint, GaussianFactorGraph& result) : + nonlinearGraph_(graph), linearizationPoint_(linearizationPoint), result_(result) { + } + // Operator that linearizes a given range of the factors + void operator()(const tbb::blocked_range& blocked_range) const { + for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) { + if (nonlinearGraph_[i]) + result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_); + else + result_[i] = GaussianFactor::shared_ptr(); } - }; + } +}; #endif } @@ -357,7 +326,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li if(factor) { (*linearFG) += factor->linearize(linearizationPoint); } else - (*linearFG) += GaussianFactor::shared_ptr(); + (*linearFG) += GaussianFactor::shared_ptr(); } #endif diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 28f72d57d..ecd3b9b56 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -32,6 +32,10 @@ namespace gtsam { class Ordering; class GaussianFactorGraph; class SymbolicFactorGraph; + template + class Expression; + template + class ExpressionFactor; /** * Formatting options when saving in GraphViz format using @@ -47,6 +51,7 @@ namespace gtsam { bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity bool plotFactorPoints; ///< Plots each factor as a dot between the variables bool connectKeysToFactor; ///< Draw a line from each key within a factor to the dot of the factor + bool binaryEdges; ///< just use non-dotted edges for binary factors std::map factorPositions; ///< (optional for each factor) Manually specify factor "dot" positions. /// Default constructor sets up robot coordinates. Paper horizontal is robot Y, /// paper vertical is robot X. Default figure size of 5x5 in. @@ -54,7 +59,7 @@ namespace gtsam { paperHorizontalAxis(Y), paperVerticalAxis(X), figureWidthInches(5), figureHeightInches(5), scale(1), mergeSimilarFactors(false), plotFactorPoints(true), - connectKeysToFactor(true) {} + connectKeysToFactor(true), binaryEdges(true) {} }; @@ -101,7 +106,7 @@ namespace gtsam { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** return keys as an ordered set - ordering is by key value */ - FastSet keys() const; + 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; @@ -150,12 +155,24 @@ namespace gtsam { */ NonlinearFactorGraph rekey(const std::map& rekey_mapping) const; + /** + * Directly add ExpressionFactor that implements |h(x)-z|^2_R + * @param h expression that implements measurement function + * @param z measurement + * @param R model + */ + template + void addExpressionFactor(const SharedNoiseModel& R, const T& z, + const Expression& h) { + push_back(boost::make_shared >(R, z, h)); + } + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactorGraph", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 00746d9b7..77d26d361 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -68,6 +68,7 @@ void NonlinearOptimizer::defaultOptimize() { // Do next iteration currentError = this->error(); this->iterate(); + tictoc_finishedIteration(); // Maybe show output if(params.verbosity >= NonlinearOptimizerParams::VALUES) this->values().print("newValues"); diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 372bced8e..e3926aa64 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -210,8 +210,9 @@ namespace gtsam { } /* ************************************************************************* */ - FastList Values::keys() const { - FastList result; + KeyVector Values::keys() const { + KeyVector result; + result.reserve(size()); for(const_iterator key_value = begin(); key_value != end(); ++key_value) result.push_back(key_value->key); return result; diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 73d0711be..d3c114657 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -26,14 +26,9 @@ #include #include -#include #include - -#include -#include #include #include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -43,7 +38,6 @@ #pragma GCC diagnostic pop #endif #include -#include #include #include @@ -297,7 +291,7 @@ namespace gtsam { * Returns a set of keys in the config * Note: by construction, the list is ordered */ - KeyList keys() const; + KeyVector keys() const; /** Replace all keys and variables */ Values& operator=(const Values& rhs); @@ -404,7 +398,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(values_); } diff --git a/gtsam/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h index ab6703f3a..47f61b8b1 100644 --- a/gtsam/nonlinear/expressionTesting.h +++ b/gtsam/nonlinear/expressionTesting.h @@ -21,9 +21,9 @@ #include #include +#include #include #include -#include #include #include @@ -32,47 +32,6 @@ namespace gtsam { -/** - * Linearize a nonlinear factor using numerical differentiation - * The benefit of this method is that it does not need to know what types are - * involved to evaluate the factor. If all the machinery of gtsam is working - * correctly, we should get the correct numerical derivatives out the other side. - */ -JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, - const Values& values, double delta = 1e-5) { - - // We will fill a map of Jacobians - std::map jacobians; - - // Get size - Eigen::VectorXd e = factor.whitenedError(values); - const size_t rows = e.size(); - - // Loop over all variables - VectorValues dX = values.zeroVectors(); - BOOST_FOREACH(Key key, factor) { - // Compute central differences using the values struct. - const size_t cols = dX.dim(key); - Matrix J = Matrix::Zero(rows, cols); - for (size_t col = 0; col < cols; ++col) { - Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = delta; - dX[key] = dx; - Values eval_values = values.retract(dX); - Eigen::VectorXd left = factor.whitenedError(eval_values); - dx[col] = -delta; - dX[key] = dx; - eval_values = values.retract(dX); - Eigen::VectorXd right = factor.whitenedError(eval_values); - J.col(col) = (left - right) * (1.0 / (2.0 * delta)); - } - jacobians[key] = J; - } - - // Next step...return JacobianFactor - return JacobianFactor(jacobians, -e); -} - namespace internal { // CPPUnitLite-style test for linearization of a factor void testFactorJacobians(TestResult& result_, const std::string& name_, @@ -88,7 +47,7 @@ void testFactorJacobians(TestResult& result_, const std::string& name_, // Check cast result and then equality CHECK(actual); - EXPECT( assert_equal(expected, *actual, tolerance)); + EXPECT(assert_equal(expected, *actual, tolerance)); } } @@ -112,7 +71,7 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_, expression.value(values), expression); testFactorJacobians(result_, name_, f, values, nd_step, tolerance); } -} +} // namespace internal } // namespace gtsam /// \brief Check the Jacobians produced by an expression against finite differences. diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index a549517e5..2f46d6d74 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -18,6 +18,12 @@ Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } +// Generics +template +Expression compose(const Expression& t1, const Expression& t2) { + return Expression(traits::Compose, t1, t2); +} + typedef Expression double_; typedef Expression Vector3_; diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h new file mode 100644 index 000000000..442b29382 --- /dev/null +++ b/gtsam/nonlinear/factorTesting.h @@ -0,0 +1,68 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file factorTesting.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Evaluate derivatives of a nonlinear factor numerically + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Linearize a nonlinear factor using numerical differentiation + * The benefit of this method is that it does not need to know what types are + * involved to evaluate the factor. If all the machinery of gtsam is working + * correctly, we should get the correct numerical derivatives out the other side. + */ +JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, double delta = 1e-5) { + + // We will fill a vector of key/Jacobians pairs (a map would sort) + std::vector > jacobians; + + // Get size + Eigen::VectorXd e = factor.whitenedError(values); + const size_t rows = e.size(); + + // Loop over all variables + VectorValues dX = values.zeroVectors(); + BOOST_FOREACH(Key key, factor) { + // Compute central differences using the values struct. + const size_t cols = dX.dim(key); + Matrix J = Matrix::Zero(rows, cols); + for (size_t col = 0; col < cols; ++col) { + Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + dx[col] = delta; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = factor.whitenedError(eval_values); + dx[col] = -delta; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = factor.whitenedError(eval_values); + J.col(col) = (left - right) * (1.0 / (2.0 * delta)); + } + jacobians.push_back(std::make_pair(key,J)); + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h similarity index 92% rename from gtsam/nonlinear/CallRecord.h rename to gtsam/nonlinear/internal/CallRecord.h index 159a841e5..90aa8a8be 100644 --- a/gtsam/nonlinear/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -20,18 +20,9 @@ #pragma once -#include -#include -#include - -#include +#include namespace gtsam { - -class JacobianMap; -// forward declaration - -//----------------------------------------------------------------------------- namespace internal { /** @@ -64,8 +55,6 @@ struct ConvertToVirtualFunctionSupportedMatrixType { } }; -} // namespace internal - /** * The CallRecord is an abstract base class for the any class that stores * the Jacobians of applying a function with respect to each of its arguments, @@ -94,9 +83,8 @@ struct CallRecord { inline void reverseAD2(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { _reverseAD3( - internal::ConvertToVirtualFunctionSupportedMatrixType< - (Derived::RowsAtCompileTime > 5)>::convert(dFdT), - jacobians); + ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); } // This overload supports matrices with both rows and columns dynamically sized. @@ -140,7 +128,6 @@ private: */ const int CallRecordMaxVirtualStaticRows = 5; -namespace internal { /** * The CallRecordImplementor implements the CallRecord interface for a Derived class by * delegating to its corresponding (templated) non-virtual methods. @@ -193,5 +180,4 @@ private: }; } // namespace internal - } // gtsam diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h new file mode 100644 index 000000000..45817a3f9 --- /dev/null +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -0,0 +1,166 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ExecutionTrace.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief Execution trace for expressions + */ + +#pragma once +#include // Configuration from CMake +#include +#include +#include + +#include + +#include + +namespace gtsam { +namespace internal { + +template struct CallRecord; + +/// Storage type for the execution trace. +/// It enforces the proper alignment in a portable way. +/// Provide a traceSize() sized array of this type to traceExecution as traceStorage. +static const unsigned TraceAlignment = 16; +typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + // block makes HUGE difference + jacobians(key).block( + 0, 0) += dTdA; + } +}; + +/// Handle Leaf Case for Dynamic Matrix type (slower) +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + jacobians(key) += dTdA; + } +}; + +/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians +template +void handleLeafCase(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + UseBlockIf< + Derived::RowsAtCompileTime != Eigen::Dynamic + && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( + dTdA, jacobians, key); +} + +/** + * The ExecutionTrace class records a tree-structured expression's execution. + * + * The class looks a bit complicated but it is so for performance. + * It is a tagged union that obviates the need to create + * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead + * the key for the leaf is stored in the space normally used to store a + * CallRecord*. Nothing is stored for a Constant. + * + * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: + * Trace(Function) -> + * BinaryRecord with two traces in it + * trace1(Function) -> + * UnaryRecord with one trace in it + * trace1(Function) -> + * BinaryRecord with two traces in it + * trace1(Leaf) + * trace2(Constant) + * trace2(Leaf) + * Hence, there are three Record structs, written to memory by traceExecution + */ +template +class ExecutionTrace { + static const int Dim = traits::dimension; + enum { + Constant, Leaf, Function + } kind; + union { + Key key; + CallRecord* ptr; + } content; +public: + /// Pointer always starts out as a Constant + ExecutionTrace() : + kind(Constant) { + } + /// Change pointer to a Leaf Record + void setLeaf(Key key) { + kind = Leaf; + content.key = key; + } + /// Take ownership of pointer to a Function Record + void setFunction(CallRecord* record) { + kind = Function; + content.ptr = record; + } + /// Print + void print(const std::string& indent = "") const { + if (kind == Constant) + std::cout << indent << "Constant" << std::endl; + else if (kind == Leaf) + std::cout << indent << "Leaf, key = " << content.key << std::endl; + else if (kind == Function) { + std::cout << indent << "Function" << std::endl; + content.ptr->print(indent + " "); + } + } + /// Return record pointer, quite unsafe, used only for testing + template + boost::optional record() { + if (kind != Function) + return boost::none; + else { + Record* p = dynamic_cast(content.ptr); + return p ? boost::optional(p) : boost::none; + } + } + /** + * *** This is the main entry point for reverse AD, called from Expression *** + * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) + */ + typedef Eigen::Matrix JacobianTT; + void startReverseAD1(JacobianMap& jacobians) const { + if (kind == Leaf) { + // This branch will only be called on trivial Leaf expressions, i.e. Priors + static const JacobianTT I = JacobianTT::Identity(); + handleLeafCase(I, jacobians, content.key); + } else if (kind == Function) + // This is the more typical entry point, starting the AD pipeline + // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. + content.ptr->startReverseAD2(jacobians); + } + // Either add to Jacobians (Leaf) or propagate (Function) + template + void reverseAD1(const Eigen::MatrixBase & dTdA, + JacobianMap& jacobians) const { + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) + content.ptr->reverseAD2(dTdA, jacobians); + } + + /// Define type so we can apply it as a meta-function + typedef ExecutionTrace type; +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h new file mode 100644 index 000000000..e7aa34db0 --- /dev/null +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -0,0 +1,516 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ExpressionNode.h + * @date May 10, 2015 + * @author Frank Dellaert + * @author Paul Furgale + * @brief ExpressionNode class + */ + +#pragma once + +#include +#include +#include + +#include // operator typeid +#include +#include + +class ExpressionFactorBinaryTest; +// Forward declare for testing + +namespace gtsam { +namespace internal { + +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { + // right now only word sized types are supported. + // Easy to extend if needed, + // by somehow inferring the unsigned integer of same size + BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); + size_t & uiValue = reinterpret_cast(value); + size_t misAlignment = uiValue % requiredAlignment; + if (misAlignment) { + uiValue += requiredAlignment - misAlignment; + } + return value; +} +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { + return upAlign(value, requiredAlignment); +} + +//----------------------------------------------------------------------------- + +/** + * Expression node. The superclass for objects that do the heavy lifting + * An Expression has a pointer to an ExpressionNode underneath + * allowing Expressions to have polymorphic behaviour even though they + * are passed by value. This is the same way boost::function works. + * http://loki-lib.sourceforge.net/html/a00652.html + */ +template +class ExpressionNode { + +protected: + + size_t traceSize_; + + /// Constructor, traceSize is size of the execution trace of expression rooted here + ExpressionNode(size_t traceSize = 0) : + traceSize_(traceSize) { + } + +public: + + /// Destructor + virtual ~ExpressionNode() { + } + + /// Streaming + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, + const ExpressionNode& node) { + os << "Expression of type " << typeid(T).name(); + if (node.traceSize_ > 0) + os << ", trace size = " << node.traceSize_; + os << "\n"; + return os; + } + + /// Return keys that play in this expression as a set + virtual std::set keys() const { + std::set keys; + return keys; + } + + /// Return dimensions for each argument, as a map + virtual void dims(std::map& map) const { + } + + // Return size needed for memory buffer in traceExecution + size_t traceSize() const { + return traceSize_; + } + + /// Return value + virtual T value(const Values& values) const = 0; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const = 0; +}; + +//----------------------------------------------------------------------------- +/// Constant Expression +template +class ConstantExpression: public ExpressionNode { + + /// The constant value + T constant_; + + /// Constructor with a value, yielding a constant + ConstantExpression(const T& value) : + constant_(value) { + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~ConstantExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + return constant_; + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + return constant_; + } +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value +template +class LeafExpression: public ExpressionNode { + typedef T value_type; + + /// The key into values + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~LeafExpression() { + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + map[key_] = traits::dimension; + } + + /// Return value + virtual T value(const Values& values) const { + return values.at(key_); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + trace.setLeaf(key_); + return values.at(key_); + } + +}; + +//----------------------------------------------------------------------------- +/// meta-function to generate fixed-size JacobianTA type +template +struct Jacobian { + typedef Eigen::Matrix::dimension, traits::dimension> type; +}; + +// Eigen format for printing Jacobians +static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); + +//----------------------------------------------------------------------------- +/// Unary Function Expression +template +class UnaryExpression: public ExpressionNode { + + typedef typename Expression::template UnaryFunction::type Function; + boost::shared_ptr > expression1_; + Function function_; + + /// Constructor with a unary function f, and input argument e1 + UnaryExpression(Function f, const Expression& e1) : + expression1_(e1.root()), function_(f) { + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~UnaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + return function_(expression1_->value(values), boost::none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + return expression1_->keys(); + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "UnaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process + void startReverseAD4(JacobianMap& jacobians) const { + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 + // which calls the correctly sized CallRecord::reverseAD3, which in turn + // calls reverseAD4 below. + trace1.reverseAD1(dTdA1, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + } + }; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + + // Create the record at the start of the traceStorage and advance the pointer + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + + // Record the traces for all arguments + // After this, the traceStorage pointer is set to after what was written + // Write an Expression execution trace in record->trace + // Iff Constant or Leaf, this will not write to traceStorage, only to trace. + // Iff the expression is functional, write all Records in traceStorage buffer + // Return value of type T is recorded in record->value + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + + // ptr is never modified by traceExecution, but if traceExecution has + // written in the buffer, the next caller expects we advance the pointer + ptr += expression1_->traceSize(); + trace.setFunction(record); + + return function_(record->value1, record->dTdA1); + } +}; + +//----------------------------------------------------------------------------- +/// Binary Expression +template +class BinaryExpression: public ExpressionNode { + + typedef typename Expression::template BinaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + Function function_; + + /// Constructor with a binary function f, and two input arguments + BinaryExpression(Function f, const Expression& e1, + const Expression& e2) : + expression1_(e1.root()), expression2_(e2.root()), function_(f) { + ExpressionNode::traceSize_ = // + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); + } + + friend class Expression ; + friend class ::ExpressionFactorBinaryTest; + +public: + + /// Destructor + virtual ~BinaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(expression1_->value(values), expression2_->value(values), + none, none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "BinaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize(); + trace.setFunction(record); + return function_(record->value1, record->value2, record->dTdA1, + record->dTdA2); + } +}; + +//----------------------------------------------------------------------------- +/// Ternary Expression +template +class TernaryExpression: public ExpressionNode { + + typedef typename Expression::template TernaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + boost::shared_ptr > expression3_; + Function function_; + + /// Constructor with a ternary function f, and two input arguments + TernaryExpression(Function f, const Expression& e1, + const Expression& e2, const Expression& e3) : + expression1_(e1.root()), expression2_(e2.root()), expression3_(e3.root()), // + function_(f) { + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + // + e1.traceSize() + e2.traceSize() + e3.traceSize(); + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~TernaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(expression1_->value(values), expression2_->value(values), + expression3_->value(values), none, none, none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + myKeys = expression3_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + expression3_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + A3 value3; + ExecutionTrace trace3; + typename Jacobian::type dTdA3; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "TernaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << dTdA3.format(kMatlabFormat) << std::endl; + trace3.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + trace3.reverseAD1(dTdA3, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + trace3.reverseAD1(dFdT * dTdA3, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + record->value3 = expression3_->traceExecution(values, record->trace3, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize() + + expression3_->traceSize(); + trace.setFunction(record); + return function_(record->value1, record->value2, record->value3, + record->dTdA1, record->dTdA2, record->dTdA3); + } +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/JacobianMap.h b/gtsam/nonlinear/internal/JacobianMap.h new file mode 100644 index 000000000..f4d2e9565 --- /dev/null +++ b/gtsam/nonlinear/internal/JacobianMap.h @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file JacobianMap.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief JacobianMap for returning derivatives from expressions + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { +namespace internal { + +// A JacobianMap is the primary mechanism by which derivatives are returned. +// Expressions are designed to write their derivatives into an already allocated +// Jacobian of the correct size, of type VerticalBlockMatrix. +// The JacobianMap provides a mapping from keys to the underlying blocks. +class JacobianMap { +private: + typedef FastVector Keys; + const FastVector& keys_; + VerticalBlockMatrix& Ab_; + +public: + /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab + JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { + } + + /// Access blocks of via key + VerticalBlockMatrix::Block operator()(Key key) { + Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key); + DenseIndex block = it - keys_.begin(); + return Ab_(block); + } +}; + +} // namespace internal +} // namespace gtsam + diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index f3858c818..3f477098a 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -265,9 +265,10 @@ TEST(AdaptAutoDiff, Snavely) { typedef AdaptAutoDiff Adaptor; Expression expression(Adaptor(), P, X); #ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero #else - EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression::Record), + expression.traceSize()); // Todo, should be zero #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 483b5ffa9..208f0b284 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -18,7 +18,7 @@ * @brief unit tests for CallRecord class */ -#include +#include #include #include @@ -32,7 +32,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > CallRecordMaxVirtualStaticRows){ + if(i > internal::CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; @@ -80,13 +80,13 @@ struct Record: public internal::CallRecordImplementor { } void print(const std::string& indent) const { } - void startReverseAD4(JacobianMap& jacobians) const { + void startReverseAD4(internal::JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, internal::JacobianMap& jacobians) const { cc.compTimeRows = SomeMatrix::RowsAtCompileTime; cc.compTimeCols = SomeMatrix::ColsAtCompileTime; cc.runTimeRows = dFdT.rows(); @@ -97,7 +97,7 @@ struct Record: public internal::CallRecordImplementor { friend struct internal::CallRecordImplementor; }; -JacobianMap & NJM= *static_cast(NULL); +internal::JacobianMap & NJM= *static_cast(NULL); /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp new file mode 100644 index 000000000..731f69816 --- /dev/null +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExecutionTrace.cpp + * @date May 11, 2015 + * @author Frank Dellaert + * @brief unit tests for Expression internals + */ + +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Constant +TEST(ExecutionTrace, construct) { + internal::ExecutionTrace trace; +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 80100af5e..75af5f634 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -42,7 +42,7 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ // Constant -TEST(Expression, constant) { +TEST(Expression, Constant) { Expression R(someR); Values values; Rot3 actual = R.value(values); @@ -68,24 +68,27 @@ TEST(Expression, Leaves) { Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); std::vector > points = createUnknowns(10, 'p', 1); - EXPECT(assert_equal(somePoint,points.back().value(values))); + EXPECT(assert_equal(somePoint, points.back().value(values))); } /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { +Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) { return Point2(); } double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } +Vector f3(const Point3& p, OptionalJacobian H) { + return p.vector(); +} Expression p(1); set expected = list_of(1); } -TEST(Expression, Unary0) { +TEST(Expression, Unary1) { using namespace unary; - Expression e(f0, p); + Expression e(f1, p); EXPECT(expected == e.keys()); } TEST(Expression, Unary2) { @@ -94,6 +97,12 @@ TEST(Expression, Unary2) { EXPECT(expected == e.keys()); } /* ************************************************************************* */ +// Unary(Leaf), dynamic +TEST(Expression, Unary3) { + using namespace unary; +// Expression e(f3, p); +} +/* ************************************************************************* */ //Nullary Method TEST(Expression, NullaryMethod) { @@ -118,7 +127,7 @@ TEST(Expression, NullaryMethod) { EXPECT(actual == sqrt(50)); Matrix expected(1, 3); expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); - EXPECT(assert_equal(expected,H[0])); + EXPECT(assert_equal(expected, H[0])); } /* ************************************************************************* */ // Binary(Leaf,Leaf) @@ -149,12 +158,12 @@ TEST(Expression, BinaryKeys) { TEST(Expression, BinaryDimensions) { map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // dimensions TEST(Expression, BinaryTraceSize) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Binary::Record); EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize()); } @@ -166,7 +175,8 @@ using namespace binary; Expression K(3); // Create expression tree -Expression projection(PinholeCamera::project_to_camera, p_cam); +Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; +Expression projection(f, p_cam); Expression uv_hat(uncalibrate, K, projection); } /* ************************************************************************* */ @@ -180,17 +190,26 @@ TEST(Expression, TreeKeys) { TEST(Expression, TreeDimensions) { map actual, expected = map_list_of(1, 6)(2, 3)(3, 5); tree::uv_hat.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // TraceSize TEST(Expression, TreeTraceSize) { - typedef UnaryExpression Unary; - typedef BinaryExpression Binary1; - typedef BinaryExpression Binary2; - size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record) - + sizeof(Binary2::Record); - EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize()); + typedef internal::BinaryExpression Binary1; + EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), + tree::p_cam.traceSize()); + + typedef internal::UnaryExpression Unary; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(), + tree::projection.traceSize()); + + EXPECT_LONGS_EQUAL(0, tree::K.traceSize()); + + typedef internal::BinaryExpression Binary2; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() + + tree::projection.traceSize(), tree::uv_hat.traceSize()); } /* ************************************************************************* */ @@ -234,7 +253,8 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, + OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 2bdc04d5b..6dc3e3d2f 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index faa285cd5..dfc14e1e6 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -26,6 +26,7 @@ #include #include // for operator += +#include #include using namespace boost::assign; #include @@ -308,12 +309,12 @@ TEST(Values, extract_keys) config.insert(key3, Pose2()); config.insert(key4, Pose2()); - FastList expected, actual; + KeyVector expected, actual; expected += key1, key2, key3, key4; actual = config.keys(); CHECK(actual.size() == expected.size()); - FastList::const_iterator itAct = actual.begin(), itExp = expected.begin(); + KeyVector::const_iterator itAct = actual.begin(), itExp = expected.begin(); for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { EXPECT(*itExp == *itAct); } diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index ae794db6a..b3fd76c67 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -109,7 +109,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("AntiFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(factor_); diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index d5740a6ab..c78b5a3bf 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -91,7 +91,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index ef02b5cb1..1b51224d2 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -113,7 +113,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measuredBearing_); diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 319c74cd5..bfd7d4e34 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -117,7 +117,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); @@ -149,7 +149,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("BetweenFactor", boost::serialization::base_object >(*this)); } diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 9cb1e3017..44a5ee5f2 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -84,7 +84,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); @@ -157,7 +157,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 0e4fb48cf..92a78279b 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -97,7 +97,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 9d4a8e6e5..da22225e5 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -173,7 +173,7 @@ public: Point3 _1T2 = E.direction().point3(); Point3 d1T2 = d * _1T2; Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) - pn = SimpleCamera::project_to_camera(dP2); + pn = PinholeBase::Project(dP2); } else { @@ -186,7 +186,7 @@ public: Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); Matrix23 Dpn_dP2; - pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); + pn = PinholeBase::Project(dP2, Dpn_dP2); if (DE) { Matrix DdP2_E(3, 5); diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 2f8dd601f..d2b042fed 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -20,218 +20,280 @@ #pragma once -#include -#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include #include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { - /** - * Non-linear factor for a constraint derived from a 2D measurement. - * The calibration is unknown here compared to GenericProjectionFactor - * @addtogroup SLAM - */ - template - class GeneralSFMFactor: public NoiseModelFactor2 { +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * The calibration is unknown here compared to GenericProjectionFactor + * @addtogroup SLAM + */ +template +class GeneralSFMFactor: public NoiseModelFactor2 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) - GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA); + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK); - static const int DimC = FixedDimension::value; - static const int DimL = FixedDimension::value; + static const int DimC = FixedDimension::value; + static const int DimL = FixedDimension::value; + typedef Eigen::Matrix JacobianC; + typedef Eigen::Matrix JacobianL; - protected: +protected: - Point2 measured_; ///< the 2D measurement + Point2 measured_; ///< the 2D measurement - public: +public: - typedef GeneralSFMFactor This; ///< typedef for this object - typedef NoiseModelFactor2 Base; ///< typedef for the base class + typedef GeneralSFMFactor This;///< typedef for this object + typedef NoiseModelFactor2 Base;///< typedef for the base class - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /** - * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation of the measurements - * @param cameraKey is the index of the camera - * @param landmarkKey is the index of the landmark - */ - GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : - Base(model, cameraKey, landmarkKey), measured_(measured) {} - - GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor - GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 - GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 - - virtual ~GeneralSFMFactor() {} ///< destructor - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter for printing out Symbols - */ - void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); - } - - /** - * equals - */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { - const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; - } - - /** h(x)-z */ - Vector evaluateError(const CAMERA& camera, const LANDMARK& point, - boost::optional H1=boost::none, boost::optional H2=boost::none) const { - - try { - Point2 reprojError(camera.project2(point,H1,H2) - measured_); - return reprojError.vector(); - } - catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, DimC); - if (H2) *H2 = zeros(2, DimL); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; - return zero(2); - } - } - - /** return the measured */ - inline const Point2 measured() const { - return measured_; - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } - }; - - template - struct traits > : Testable< - GeneralSFMFactor > { - }; + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; /** - * Non-linear factor for a constraint derived from a 2D measurement. - * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. + * Constructor + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation of the measurements + * @param cameraKey is the index of the camera + * @param landmarkKey is the index of the landmark */ - template - class GeneralSFMFactor2: public NoiseModelFactor3 { + GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : + Base(model, cameraKey, landmarkKey), measured_(measured) {} - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) - static const int DimK = FixedDimension::value; + GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor + GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 + GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 - protected: + virtual ~GeneralSFMFactor() {} ///< destructor - Point2 measured_; ///< the 2D measurement + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this)));} - public: + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter for printing out Symbols + */ + void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } - typedef GeneralSFMFactor2 This; - typedef PinholeCamera Camera; ///< typedef for camera type - typedef NoiseModelFactor3 Base; ///< typedef for the base class + /** + * equals + */ + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + } - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /** - * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation of the measurements - * @param poseKey is the index of the camera - * @param landmarkKey is the index of the landmark - * @param calibKey is the index of the calibration - */ - GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : - Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} - GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor - - virtual ~GeneralSFMFactor2() {} ///< destructor - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); + /** h(x)-z */ + Vector evaluateError(const CAMERA& camera, const LANDMARK& point, + boost::optional H1=boost::none, boost::optional H2=boost::none) const { + try { + Point2 reprojError(camera.project2(point,H1,H2) - measured_); + return reprojError.vector(); } - - /** - * equals - */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { - const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; - } - - /** h(x)-z */ - Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const - { - try { - Camera camera(pose3,calib); - Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); - return reprojError.vector(); - } - catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, 6); - if (H2) *H2 = zeros(2, 3); - if (H3) *H3 = zeros(2, DimK); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; - } + catch( CheiralityException& e) { + if (H1) *H1 = JacobianC::Zero(); + if (H2) *H2 = JacobianL::Zero(); + // TODO warn if verbose output asked for return zero(2); } + } - /** return the measured */ - inline const Point2 measured() const { - return measured_; + /// Linearize using fixed-size matrices + boost::shared_ptr linearize(const Values& values) const { + // Only linearize if the factor is active + if (!this->active(values)) return boost::shared_ptr(); + + const Key key1 = this->key1(), key2 = this->key2(); + JacobianC H1; + JacobianL H2; + Vector2 b; + try { + const CAMERA& camera = values.at(key1); + const LANDMARK& point = values.at(key2); + Point2 reprojError(camera.project2(point, H1, H2) - measured()); + b = -reprojError.vector(); + } catch (CheiralityException& e) { + H1.setZero(); + H2.setZero(); + b.setZero(); + // TODO warn if verbose output asked for } - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor3", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); + // Whiten the system if needed + const SharedNoiseModel& noiseModel = this->get_noiseModel(); + if (noiseModel && !noiseModel->isUnit()) { + // TODO: implement WhitenSystem for fixed size matrices and include + // above + H1 = noiseModel->Whiten(H1); + H2 = noiseModel->Whiten(H2); + b = noiseModel->Whiten(b); } - }; - template - struct traits > : Testable< - GeneralSFMFactor2 > { - }; + // Create new (unit) noiseModel, preserving constraints if applicable + SharedDiagonal model; + if (noiseModel && noiseModel->isConstrained()) { + model = boost::static_pointer_cast(noiseModel)->unit(); + } + + return boost::make_shared >(key1, H1, key2, H2, b, model); + } + + /** return the measured */ + inline const Point2 measured() const { + return measured_; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; + +template +struct traits > : Testable< + GeneralSFMFactor > { +}; + +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. + */ +template +class GeneralSFMFactor2: public NoiseModelFactor3 { + + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + static const int DimK = FixedDimension::value; + +protected: + + Point2 measured_; ///< the 2D measurement + +public: + + typedef GeneralSFMFactor2 This; + typedef PinholeCamera Camera;///< typedef for camera type + typedef NoiseModelFactor3 Base;///< typedef for the base class + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation of the measurements + * @param poseKey is the index of the camera + * @param landmarkKey is the index of the landmark + * @param calibKey is the index of the calibration + */ + GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : + Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} + GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor + + virtual ~GeneralSFMFactor2() {} ///< destructor + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this)));} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /** + * equals + */ + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + } + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const + { + try { + Camera camera(pose3,calib); + Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); + return reprojError.vector(); + } + catch( CheiralityException& e) { + if (H1) *H1 = zeros(2, 6); + if (H2) *H2 = zeros(2, 3); + if (H3) *H3 = zeros(2, DimK); + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) + << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; + } + return zero(2); + } + + /** return the measured */ + inline const Point2 measured() const { + return measured_; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; + +template +struct traits > : Testable< + GeneralSFMFactor2 > { +}; } //namespace diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 923edddb7..16560a43e 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianFactorQ.h * @date Oct 27, 2013 @@ -6,16 +17,18 @@ #pragma once -#include "JacobianSchurFactor.h" +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public JacobianSchurFactor { +class JacobianFactorQ: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; + typedef std::pair KeyMatrix; public: @@ -24,37 +37,45 @@ public: } /// Empty constructor with keys - JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - Matrix zeroMatrix = Matrix::Zero(0,D); + JacobianFactorQ(const FastVector& keys, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { + Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) - QF.push_back(KeyMatrix(key, zeroMatrix)); + QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + JacobianFactorQ(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix3& P, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : + Base() { size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); // Calculate pre-computed Jacobian matrices // TODO: can we do better ? - typedef std::pair KeyMatrix; - std::vector < KeyMatrix > QF; + std::vector QF; QF.reserve(m); // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); + for (size_t k = 0; k < FBlocks.size(); ++k) { + Key key = keys[k]; + QF.push_back( + KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k])); + } // Which is then passed to the normal JacobianFactor constructor - JacobianFactor::fillTerms(QF, Q * b, model); + JacobianFactor::fillTerms(QF, - Q * b, model); } }; +// end class JacobianFactorQ + +// traits +template struct traits > : public Testable< + JacobianFactorQ > { +}; } diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index ccd6e8756..77102c24b 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,41 +6,47 @@ */ #pragma once -#include +#include +#include +#include namespace gtsam { + +class GaussianBayesNet; + /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public JacobianSchurFactor { +class JacobianFactorQR: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, + JacobianFactorQR(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix3& P, + const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + Base() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); - size_t i = 0; - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { - gfg.add(pointKey, E.block(ZDim * i, 0), it.first, it.second, - b.segment(ZDim * i), model); - i += 1; + for (size_t k = 0; k < FBlocks.size(); ++k) { + Key key = keys[k]; + gfg.add(pointKey, E.block(ZDim * k, 0), key, FBlocks[k], + b.segment < ZDim > (ZDim * k), model); } //gfg.print("gfg"); // eliminate the point - GaussianBayesNet::shared_ptr bn; + boost::shared_ptr bn; GaussianFactorGraph::shared_ptr fg; - std::vector < Key > variables; + std::vector variables; variables.push_back(pointKey); boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); //fg->print("fg"); @@ -48,6 +54,6 @@ public: JacobianFactor::operator=(JacobianFactor(*fg)); } }; -// class +// end class JacobianFactorQR -}// gtsam +}// end namespace gtsam diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e0a5f4566..86636c38f 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,50 +5,66 @@ */ #pragma once -#include "gtsam/slam/JacobianSchurFactor.h" +#include namespace gtsam { /** - * JacobianFactor for Schur complement that uses Q noise model + * JacobianFactor for Schur complement */ template -class JacobianFactorSVD: public JacobianSchurFactor { +class JacobianFactorSVD: public RegularJacobianFactor { + + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 + typedef std::pair KeyMatrix; public: - typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 - typedef std::pair KeyMatrix2D; - typedef std::pair KeyMatrix; - /// Default constructor - JacobianFactorSVD() {} + JacobianFactorSVD() { + } /// Empty constructor with keys - JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - Matrix zeroMatrix = Matrix::Zero(0,D); + JacobianFactorSVD(const FastVector& keys, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { + Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) - QF.push_back(KeyMatrix(key, zeroMatrix)); + QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } - /// Constructor - JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + /** + * @brief Constructor + * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) + * and a reduced point derivative, Enull + * and creates a reduced-rank Jacobian factor on the CameraSet + * + * @Fblocks: + */ + JacobianFactorSVD(const FastVector& keys, + const std::vector& Fblocks, const Matrix& Enull, + const Vector& b, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { size_t numKeys = Enull.rows() / ZDim; - size_t j = 0, m2 = ZDim*numKeys-3; + size_t m2 = ZDim * numKeys - 3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); - // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); std::vector QF; QF.reserve(numKeys); - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); + for (size_t k = 0; k < Fblocks.size(); ++k) { + Key key = keys[k]; + QF.push_back( + KeyMatrix(key, + (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k])); + } JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h deleted file mode 100644 index 5d28bbada..000000000 --- a/gtsam/slam/JacobianSchurFactor.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * @file JacobianSchurFactor.h - * @brief Jacobianfactor that combines and eliminates points - * @date Oct 27, 2013 - * @uthor Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { -/** - * JacobianFactor for Schur complement that uses Q noise model - */ -template -class JacobianSchurFactor: public JacobianFactor { - -public: - - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - /** - * @brief double* Matrix-vector multiply, i.e. y = A*x - * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way - */ - Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax) : Ax; - } - - /** - * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e - * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way - */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const - { - Vector E = alpha * (model_ ? model_->whiten(e) : e); - // Just iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); - if (empty()) return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } - - // multiply with alpha - Ax *= alpha; - - // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; posnoiseModel_->print(" noise model: "); +} -void OrientedPlane3DirectionPrior::print(const string& s) const { +//*************************************************************************** +void OrientedPlane3DirectionPrior::print(const string& s, + const KeyFormatter& keyFormatter) const { cout << "Prior Factor on " << landmarkKey_ << "\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } //*************************************************************************** - bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, - double tol) const { + double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol); + return e != NULL && Base::equals(*e, tol) + && this->measured_p_.equals(e->measured_p_, tol); } //*************************************************************************** @@ -33,21 +40,21 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, boost::optional H) const { -if(H) { - Matrix H_p; - Unit3 n_hat_p = measured_p_.normal(); - Unit3 n_hat_q = plane.normal(); - Vector e = n_hat_p.error(n_hat_q,H_p); - H->resize(2,3); - H->block <2,2>(0,0) << H_p; - H->block <2,1>(0,2) << Matrix::Zero(2, 1); - return e; -} else { - Unit3 n_hat_p = measured_p_.normal(); - Unit3 n_hat_q = plane.normal(); - Vector e = n_hat_p.error(n_hat_q); - return e; -} + if (H) { + Matrix H_p; + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q, H_p); + H->resize(2, 3); + H->block<2, 2>(0, 0) << H_p; + H->block<2, 1>(0, 2) << Matrix::Zero(2, 1); + return e; + } else { + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q); + return e; + } } } diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 7827a5c2c..ab77ec612 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -7,57 +7,53 @@ #pragma once -#include -#include #include -#include -#include -#include +#include namespace gtsam { - /** +/** * Factor to measure a planar landmark from a given pose */ class OrientedPlane3Factor: public NoiseModelFactor2 { protected: - Symbol poseSymbol_; - Symbol landmarkSymbol_; + Key poseKey_; + Key landmarkKey_; Vector measured_coeffs_; OrientedPlane3 measured_p_; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; public: /// Constructor - OrientedPlane3Factor () - {} + OrientedPlane3Factor() { + } /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol - OrientedPlane3Factor (const Vector&z, const SharedGaussian& noiseModel, - const Symbol& pose, - const Symbol& landmark) - : Base (noiseModel, pose, landmark), - poseSymbol_ (pose), - landmarkSymbol_ (landmark), - measured_coeffs_ (z) - { - measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3)); + OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel, + const Key& pose, const Key& landmark) : + Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_( + z) { + measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); } /// print - void print(const std::string& s="PlaneFactor") const; + virtual void print(const std::string& s = "OrientedPlane3Factor", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// evaluateError virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const - { - OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2); + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, + H2); Vector err(3); - err << predicted_plane.error (measured_p_); + err << predicted_plane.error(measured_p_); return (err); - }; + } + ; }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior @@ -65,30 +61,30 @@ class OrientedPlane3DirectionPrior: public NoiseModelFactor1 { protected: OrientedPlane3 measured_p_; /// measured plane parameters Key landmarkKey_; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactor1 Base; public: typedef OrientedPlane3DirectionPrior This; /// Constructor - OrientedPlane3DirectionPrior () - {} + OrientedPlane3DirectionPrior() { + } /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol - OrientedPlane3DirectionPrior (Key key, const Vector&z, - const SharedGaussian& noiseModel) - : Base (noiseModel, key), - landmarkKey_ (key) - { - measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3)); + OrientedPlane3DirectionPrior(Key key, const Vector&z, + const SharedGaussian& noiseModel) : + Base(noiseModel, key), landmarkKey_(key) { + measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); } - /// print - void print(const std::string& s) const; - /** equals */ + /// print + virtual void print(const std::string& s = "OrientedPlane3DirectionPrior", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// equals virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; virtual Vector evaluateError(const OrientedPlane3& plane, - boost::optional H = boost::none) const; + boost::optional H = boost::none) const; }; } // gtsam diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index f7d42497e..94c19a9d0 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -88,7 +88,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 6689653aa..e9914955c 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -90,7 +90,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index eee14f9f2..a738d4cf0 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -57,6 +57,11 @@ namespace gtsam { Base(model, key), prior_(prior) { } + /** Convenience constructor that takes a full covariance argument */ + PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : + Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { + } + /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( @@ -94,7 +99,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 52e28beaf..1056527d1 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -178,7 +178,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 216b9320e..d45dd5ce0 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -90,7 +90,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); @@ -181,7 +181,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 6a70d58b4..90ceeafc8 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -122,7 +122,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor3", boost::serialization::base_object(*this)); } diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h deleted file mode 100644 index 8c3df87cf..000000000 --- a/gtsam/slam/RegularHessianFactor.h +++ /dev/null @@ -1,178 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file RegularHessianFactor.h - * @brief HessianFactor class with constant sized blcoks - * @author Richard Roberts - * @date Dec 8, 2010 - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -template -class RegularHessianFactor: public HessianFactor { - -private: - - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix VectorD; - -public: - - /** Construct an n-way factor. Gs contains the upper-triangle blocks of the - * quadratic term (the Hessian matrix) provided in row-order, gs the pieces - * of the linear vector term, and f the constant term. - */ - RegularHessianFactor(const std::vector& js, - const std::vector& Gs, const std::vector& gs, double f) : - HessianFactor(js, Gs, gs, f) { - } - - /** Constructor with an arbitrary number of keys and with the augmented information matrix - * specified as a block matrix. */ - template - RegularHessianFactor(const KEYS& keys, - const SymmetricBlockMatrix& augmentedInformation) : - HessianFactor(keys, augmentedInformation) { - } - - // Scratch space for multiplyHessianAdd - typedef Eigen::Matrix DVector; - mutable std::vector y; - - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{ - throw std::runtime_error( - "RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead"); - } - - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { - // Create a vector of temporary y values, corresponding to rows i - y.resize(size()); - BOOST_FOREACH(DVector & yi, y) - yi.setZero(); - - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - // Accessing the VectorValues one by one is expensive - // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after - DVector xj(D); - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - Key key = keys_[j]; - const double* xj = x + key * D; - DenseIndex i = 0; - for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); - // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); - // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); - } - - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { - Key key = keys_[i]; - DMap(yvalues + key * D) += alpha * y[i]; - } - } - - void multiplyHessianAdd(double alpha, const double* x, double* yvalues, - std::vector offsets) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - // Create a vector of temporary y values, corresponding to rows i - std::vector y; - y.reserve(size()); - for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); - - // Accessing the VectorValues one by one is expensive - // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - DenseIndex i = 0; - for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - } - - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) - DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += - alpha * y[i]; - } - - /** Return the diagonal of the Hessian for this factor (raw memory version) */ - virtual void hessianDiagonal(double* d) const { - - // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - const Matrix& B = info_(pos, pos).selfadjointView(); - //DMap(d + 9 * j) += B.diagonal(); - DMap(d + D * j) += B.diagonal(); - } - } - - /* ************************************************************************* */ - // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n - virtual void gradientAtZero(double* d) const { - - // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - VectorD dj = -info_(pos,size()).knownOffDiagonal(); - //DMap(d + 9 * j) += dj; - DMap(d + D * j) += dj; - } - } - -}; - -} - diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 75b2d44ba..c713eff72 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -7,19 +7,18 @@ #pragma once -#include +#include #include #include #include -#include -#include +#include namespace gtsam { /** * RegularImplicitSchurFactor */ -template // +template class RegularImplicitSchurFactor: public GaussianFactor { public: @@ -28,14 +27,20 @@ public: protected: - typedef Eigen::Matrix Matrix2D; ///< type of an F block - typedef Eigen::Matrix MatrixDD; ///< camera hessian - typedef std::pair KeyMatrix2D; ///< named F block + // This factor is closely related to a CameraSet + typedef CameraSet Set; - std::vector Fblocks_; ///< All 2*D F blocks (one for each camera) - Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) - Matrix E_; ///< The 2m*3 E Jacobian with respect to the point - Vector b_; ///< 2m-dimensional RHS vector + typedef typename CAMERA::Measurement Z; + static const int D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + typedef Eigen::Matrix MatrixZD; ///< type of an F block + typedef Eigen::Matrix MatrixDD; ///< camera hessian + + const std::vector FBlocks_; ///< All ZDim*D F blocks (one for each camera) + const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) + const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point + const Vector b_; ///< 2m-dimensional RHS vector public: @@ -43,64 +48,60 @@ public: RegularImplicitSchurFactor() { } - /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b - RegularImplicitSchurFactor(const std::vector& Fblocks, const Matrix& E, - const Matrix3& P, const Vector& b) : - Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { - initKeys(); - } - - /// initialize keys from Fblocks - void initKeys() { - keys_.reserve(Fblocks_.size()); - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) - keys_.push_back(it.first); + /// Construct from blocks of F, E, inv(E'*E), and RHS vector b + RegularImplicitSchurFactor(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix& P, + const Vector& b) : + GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { } /// Destructor virtual ~RegularImplicitSchurFactor() { } - // Write access, only use for construction! - - inline std::vector& Fblocks() { - return Fblocks_; + std::vector& FBlocks() const { + return FBlocks_; } - inline Matrix3& PointCovariance() { - return PointCovariance_; - } - - inline Matrix& E() { + const Matrix& E() const { return E_; } - inline Vector& b() { + const Vector& b() const { return b_; } - /// Get matrix P - inline const Matrix3& getPointCovariance() const { + const Matrix& getPointCovariance() const { return PointCovariance_; } /// print - void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); - std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; - std::cout << " E_ \n" << E_ << std::endl; - std::cout << " b_ \n" << b_.transpose() << std::endl; + for (size_t pos = 0; pos < size(); ++pos) { + std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl; + } + std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl; + std::cout << "E:\n" << E_ << std::endl; + std::cout << "b:\n" << b_.transpose() << std::endl; } /// equals bool equals(const GaussianFactor& lf, double tol) const { - if (!dynamic_cast(&lf)) - return false; - else { + const This* f = dynamic_cast(&lf); + if (!f) return false; + for (size_t k = 0; k < FBlocks_.size(); ++k) { + if (keys_[k] != f->keys_[k]) + return false; + if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol)) + return false; } + return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) + && equal_with_abs_tol(E_, f->E_, tol) + && equal_with_abs_tol(b_, f->b_, tol); } /// Degrees of freedom of camera @@ -108,24 +109,37 @@ public: return D; } + virtual void updateHessian(const FastVector& keys, + SymmetricBlockMatrix* info) const { + throw std::runtime_error( + "RegularImplicitSchurFactor::updateHessian non implemented"); + } virtual Matrix augmentedJacobian() const { throw std::runtime_error( "RegularImplicitSchurFactor::augmentedJacobian non implemented"); return Matrix(); } virtual std::pair jacobian() const { - throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented"); + throw std::runtime_error( + "RegularImplicitSchurFactor::jacobian non implemented"); return std::make_pair(Matrix(), Vector()); } + + /// *Compute* full augmented information matrix virtual Matrix augmentedInformation() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::augmentedInformation non implemented"); - return Matrix(); + + // Do the Schur complement + SymmetricBlockMatrix augmentedHessian = // + Set::SchurComplement(FBlocks_, E_, b_); + return augmentedHessian.matrix(); } + + /// *Compute* full information matrix virtual Matrix information() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::information non implemented"); - return Matrix(); + Matrix augmented = augmentedInformation(); + int m = this->keys_.size(); + size_t M = D * m; + return augmented.block(0, 0, M, M); } /// Return the diagonal of the Hessian for this factor @@ -133,17 +147,17 @@ public: // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); VectorValues d; - for (size_t pos = 0; pos < size(); ++pos) { // for each camera - Key j = keys_[pos]; + for (size_t k = 0; k < size(); ++k) { // for each camera + Key j = keys_[k]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x 2) * (2 x 3) - const Matrix2D& Fj = Fblocks_[pos].second; - Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); + // D x 3 = (D x ZDim) * (ZDim x 3) + const MatrixZD& Fj = FBlocks_[k]; + Eigen::Matrix FtE = Fj.transpose() + * E_.block(ZDim * k, 0); Eigen::Matrix dj; - for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian // Vector column_k_Fj = Fj.col(k); dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj); // Vector column_k_FtE = FtE.row(k); @@ -169,13 +183,13 @@ public: Key j = keys_[pos]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x 2) * (2 x 3) - const Matrix2D& Fj = Fblocks_[pos].second; + // D x 3 = (D x ZDim) * (ZDim x 3) + const MatrixZD& Fj = FBlocks_[pos]; Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); + * E_.block(ZDim * pos, 0); DVector dj; - for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian dj(k) = Fj.col(k).squaredNorm(); // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); @@ -190,38 +204,41 @@ public: // F'*(I - E*P*E')*F for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; - // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) - const Matrix2D& Fj = Fblocks_[pos].second; + // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) + const MatrixZD& Fj = FBlocks_[pos]; // Eigen::Matrix FtE = Fj.transpose() - // * E_.block<2, 3>(2 * pos, 0); + // * E_.block(ZDim * pos, 0); // blocks[j] = Fj.transpose() * Fj // - FtE * PointCovariance_ * FtE.transpose(); - const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0); - blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); + const Matrix23& Ej = E_.block(ZDim * pos, 0); + blocks[j] = Fj.transpose() + * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( - // static const Eigen::Matrix I2 = eye(2); + // static const Eigen::Matrix I2 = eye(ZDim); // Matrix2 Q = // - // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); + // I2 - E_.block(ZDim * pos, 0) * PointCovariance_ * E_.block(ZDim * pos, 0).transpose(); // blocks[j] = Fj.transpose() * Q * Fj; } return blocks; } virtual GaussianFactor::shared_ptr clone() const { - return boost::make_shared >(Fblocks_, - PointCovariance_, E_, b_); - throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented"); + return boost::make_shared >(keys_, + FBlocks_, PointCovariance_, E_, b_); + throw std::runtime_error( + "RegularImplicitSchurFactor::clone non implemented"); } virtual bool empty() const { return false; } virtual GaussianFactor::shared_ptr negate() const { - return boost::make_shared >(Fblocks_, - PointCovariance_, E_, b_); - throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented"); + return boost::make_shared >(keys_, + FBlocks_, PointCovariance_, E_, b_); + throw std::runtime_error( + "RegularImplicitSchurFactor::negate non implemented"); } // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing @@ -239,22 +256,24 @@ public: typedef std::vector Error2s; /** - * @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b) + * @brief Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b) */ void projectError2(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * (e1-2*b) = (3*2m)*2m + // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2)); + d1 += E_.block(ZDim * k, 0).transpose() + * (e1[k] - ZDim * b_.segment(k * ZDim)); // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2; + e2[k] = e1[k] - ZDim * b_.segment(k * ZDim) + - E_.block(ZDim * k, 0) * d2; } /* @@ -274,7 +293,7 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]); + e1[k] = FBlocks_[k] * x.at(keys_[k]); projectError2(e1, e2); double result = 0; @@ -296,7 +315,7 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment(k * ZDim); projectError(e1, e2); double result = 0; @@ -309,21 +328,21 @@ public: /** * @brief Calculate corrected error Q*e = (I - E*P*E')*e */ - void projectError(const Error2s& e1, Error2s& e2) const { + void projectError(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * e1 = (3*2m)*2m - Vector3 d1; - d1.setZero(); - for (size_t k = 0; k < size(); k++) - d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + // d1 = E.transpose() * e1 = (3*2m)*2m + Vector3 d1; + d1.setZero(); + for (size_t k = 0; k < size(); k++) + d1 += E_.block(ZDim * k, 0).transpose() * e1[k]; - // d2 = E.transpose() * e1 = (3*2m)*2m - Vector3 d2 = PointCovariance_ * d1; + // d2 = E.transpose() * e1 = (3*2m)*2m + Vector3 d2 = PointCovariance_ * d1; - // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] - for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; - } + // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] + for (size_t k = 0; k < size(); k++) + e2[k] = e1[k] - E_.block(ZDim * k, 0) * d2; + } /// Scratch space for multiplyHessianAdd mutable Error2s e1, e2; @@ -344,19 +363,17 @@ public: e2.resize(size()); // e1 = F * x = (2m*dm)*dm - size_t k = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { - Key key = it.first; - e1[k++] = it.second * ConstDMap(x + D * key); + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + e1[k] = FBlocks_[k] * ConstDMap(x + D * key); } projectError(e1, e2); // y += F.transpose()*e2 = (2d*2m)*2m - k = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { - Key key = it.first; - DMap(y + D * key) += it.second.transpose() * alpha * e2[k++]; + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k]; } } @@ -377,7 +394,7 @@ public: // e1 = F * x = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]); + e1[k] = FBlocks_[k] * x.at(keys_[k]); projectError(e1, e2); @@ -389,8 +406,8 @@ public: Vector& yi = it.first->second; // Create the value as a zero vector if it does not exist. if (it.second) - yi = Vector::Zero(Fblocks_[k].second.cols()); - yi += Fblocks_[k].second.transpose() * alpha * e2[k]; + yi = Vector::Zero(FBlocks_[k].cols()); + yi += FBlocks_[k].transpose() * alpha * e2[k]; } } @@ -400,9 +417,9 @@ public: void multiplyHessianDummy(double alpha, const VectorValues& x, VectorValues& y) const { - BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) { + for (size_t k = 0; k < size(); ++k) { static const Vector empty; - Key key = Fi.first; + Key key = keys_[k]; std::pair it = y.tryInsert(key, empty); Vector& yi = it.first->second; yi = x.at(key); @@ -417,14 +434,14 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment < 2 > (2 * k); + e1[k] = b_.segment(ZDim * k); projectError(e1, e2); // g = F.transpose()*e2 VectorValues g; for (size_t k = 0; k < size(); ++k) { Key key = keys_[k]; - g.insert(key, -Fblocks_[k].second.transpose() * e2[k]); + g.insert(key, -FBlocks_[k].transpose() * e2[k]); } // return it @@ -444,23 +461,34 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment < 2 > (2 * k); + e1[k] = b_.segment(ZDim * k); projectError(e1, e2); for (size_t k = 0; k < size(); ++k) { // for each camera in the factor Key j = keys_[k]; - DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k]; + DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k]; } } /// Gradient wrt a key at any values Vector gradient(Key key, const VectorValues& x) const { - throw std::runtime_error("gradient for RegularImplicitSchurFactor is not implemented yet"); + throw std::runtime_error( + "gradient for RegularImplicitSchurFactor is not implemented yet"); } - }; -// RegularImplicitSchurFactor +// end class RegularImplicitSchurFactor + +template +const int RegularImplicitSchurFactor::D; + +template +const int RegularImplicitSchurFactor::ZDim; + +// traits +template struct traits > : public Testable< + RegularImplicitSchurFactor > { +}; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 44d6902fa..b147c2721 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -22,111 +22,134 @@ #include #include #include -#include #include -#include // for Cheirality exception -#include -#include -#include +#include +#include #include #include #include namespace gtsam { -/// Base class with no internal point, completely functional -template + +/** + * @brief Base class for smart factors + * This base class has no internal point, but it has a measurement, noise model + * and an optional sensor pose. + * This class mainly computes the derivatives and returns them as a variety of factors. + * The methods take a Cameras argument, which should behave like PinholeCamera, and + * the value of a point, which is kept in the base class. + */ +template class SmartFactorBase: public NonlinearFactor { +private: + typedef NonlinearFactor Base; + typedef SmartFactorBase This; + typedef typename CAMERA::Measurement Z; + + /** + * As of Feb 22, 2015, the noise model is the same for all measurements and + * is isotropic. This allows for moving most calculations of Schur complement + * etc to be moved to CameraSet very easily, and also agrees pragmatically + * with what is normally done. + */ + SharedIsotropic noiseModel_; + protected: + /** + * 2D measurement and noise model for each of the m views + * We keep a copy of measurements for I/O and computing the error. + * The order is kept the same as the keys that we use to create the factor. + */ + std::vector measured_; - // Keep a copy of measurement and calibration for I/O - std::vector measured_; ///< 2D measurement for each of the m views - std::vector noise_; ///< noise model used - ///< (important that the order is the same as the keys that we use to create the factor) + /// @name Pose of the camera in the body frame + const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + /// @} - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + static const int Dim = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension - static const int ZDim = traits::dimension; ///< Measurement dimension - - /// Definitions for blocks of F - typedef Eigen::Matrix Matrix2D; // F - typedef Eigen::Matrix MatrixD2; // F' - typedef std::pair KeyMatrix2D; // Fblocks - typedef Eigen::Matrix MatrixDD; // camera hessian block + // Definitions for block matrices used internally + typedef Eigen::Matrix MatrixD2; // F' + typedef Eigen::Matrix MatrixDD; // camera hessian block typedef Eigen::Matrix Matrix23; - typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix VectorD; typedef Eigen::Matrix Matrix2; - /// shorthand for base class type - typedef NonlinearFactor Base; - - /// shorthand for this class - typedef SmartFactorBase This; - + // check that noise model is isotropic and the same + // TODO, this is hacky, we should just do this via constructor, not add + void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) { + if (!sharedNoiseModel) + return; + SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< + noiseModel::Isotropic>(sharedNoiseModel); + if (!sharedIsotropic) + throw std::runtime_error("SmartFactorBase: needs isotropic"); + if (!noiseModel_) + noiseModel_ = sharedIsotropic; + else if (!sharedIsotropic->equals(*noiseModel_)) + throw std::runtime_error( + "SmartFactorBase: cannot add measurements with different noise model"); + } public: - + // Definitions for blocks of F, externally visible + typedef Eigen::Matrix MatrixZD; // F EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - typedef CAMERA Camera; - typedef std::vector Cameras; + /// We use the new CameraSte data structure to refer to a set of cameras + typedef CameraSet Cameras; - /** - * Constructor - * @param body_P_sensor is the transform from sensor to body frame (default identity) - */ - SmartFactorBase(boost::optional body_P_sensor = boost::none) : - body_P_sensor_(body_P_sensor) { - } + /// Constructor + SmartFactorBase(boost::optional body_P_sensor = boost::none) : + body_P_sensor_(body_P_sensor){} - /** Virtual destructor */ + /// Virtual destructor, subclasses from NonlinearFactor virtual ~SmartFactorBase() { } /** - * add a new measurement and pose key + * Add a new measurement and pose key * @param measured_i is the 2m dimensional projection of a single landmark * @param poseKey is the index corresponding to the camera observing the landmark - * @param noise_i is the measurement noise + * @param sharedNoiseModel is the measurement noise */ - void add(const Z& measured_i, const Key& poseKey_i, - const SharedNoiseModel& noise_i) { + void add(const Z& measured_i, const Key& cameraKey_i, + const SharedNoiseModel& sharedNoiseModel) { this->measured_.push_back(measured_i); - this->keys_.push_back(poseKey_i); - this->noise_.push_back(noise_i); + this->keys_.push_back(cameraKey_i); + maybeSetNoiseModel(sharedNoiseModel); } /** - * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises + * Add a bunch of measurements, together with the camera keys and noises */ - // **************************************************************************************************** - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& cameraKeys, std::vector& noises) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(poseKeys.at(i)); - this->noise_.push_back(noises.at(i)); + this->keys_.push_back(cameraKeys.at(i)); + maybeSetNoiseModel(noises.at(i)); } } /** - * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them + * Add a bunch of measurements and use the same noise model for all of them */ - // **************************************************************************************************** - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& cameraKeys, const SharedNoiseModel& noise) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(poseKeys.at(i)); - this->noise_.push_back(noise); + this->keys_.push_back(cameraKeys.at(i)); + maybeSetNoiseModel(noise); } } @@ -134,24 +157,31 @@ public: * Adds an entire SfM_track (collection of cameras observing a single point). * The noise is assumed to be the same for all measurements */ - // **************************************************************************************************** template void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { this->measured_.push_back(trackToAdd.measurements[k].second); this->keys_.push_back(trackToAdd.measurements[k].first); - this->noise_.push_back(noise); + maybeSetNoiseModel(noise); } } + /// get the dimension (number of rows!) of the factor + virtual size_t dim() const { + return ZDim * this->measured_.size(); + } + /** return the measurements */ const std::vector& measured() const { return measured_; } - /** return the noise model */ - const std::vector& noise() const { - return noise_; + /// Collect all cameras: important that in key order + virtual Cameras cameras(const Values& values) const { + Cameras cameras; + BOOST_FOREACH(const Key& k, this->keys_) + cameras.push_back(values.at(k)); + return cameras; } /** @@ -164,11 +194,11 @@ public: std::cout << s << "SmartFactorBase, z = \n"; for (size_t k = 0; k < measured_.size(); ++k) { std::cout << "measurement, p = " << measured_[k] << "\t"; - noise_[k]->print("noise model = "); + noiseModel_->print("noise model = "); } - if (this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - Base::print("", keyFormatter); + if(body_P_sensor_) + body_P_sensor_->print("body_P_sensor_:\n"); + print("", keyFormatter); } /// equals @@ -181,511 +211,210 @@ public: areMeasurementsEqual = false; break; } - return e && Base::equals(p, tol) && areMeasurementsEqual - && ((!body_P_sensor_ && !e->body_P_sensor_) - || (body_P_sensor_ && e->body_P_sensor_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + return e && Base::equals(p, tol) && areMeasurementsEqual; } - // **************************************************************************************************** -// /// Calculate vector of re-projection errors, before applying noise model - Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - - Vector b = zero(ZDim * cameras.size()); - - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z e(camera.project(point) - zi); - b[ZDim * i] = e.x(); - b[ZDim * i + 1] = e.y(); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); + /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives + template + Vector unwhitenedError(const Cameras& cameras, const POINT& point, + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { + Vector ue = cameras.reprojectionError(point, measured_, Fs, E); + if(body_P_sensor_){ + for(size_t i=0; i < Fs->size(); i++){ + Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); + Matrix J(6, 6); + Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + Fs->at(i) = Fs->at(i) * J; } - i += 1; } - - return b; + return ue; } - // **************************************************************************************************** /** - * Calculate the error of the factor. + * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] + * Noise model applied + */ + template + Vector whitenedError(const Cameras& cameras, const POINT& point) const { + Vector e = cameras.reprojectionError(point, measured_); + if (noiseModel_) + noiseModel_->whitenInPlace(e); + return e; + } + + /** Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. - * This is different from reprojectionError(cameras,point) as each point is whitened + * Will be used in "error(Values)" function required by NonlinearFactor base class */ + template double totalReprojectionError(const Cameras& cameras, - const Point3& point) const { - - double overallError = 0; - - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z reprojectionError(camera.project(point) - zi); - overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; - } - return overallError; + const POINT& point) const { + Vector e = whitenedError(cameras, point); + return 0.5 * e.dot(e); } - // **************************************************************************************************** - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, - const Point3& point) const { - - int numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - Vector b = zero(2 * numKeys); - - Matrix Ei(ZDim, 3); - for (size_t i = 0; i < this->measured_.size(); i++) { - try { - cameras[i].project(point, boost::none, Ei, boost::none); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - this->noise_.at(i)->WhitenSystem(Ei, b); - E.block(ZDim * i, 0) = Ei; - } - - // Matrix PointCov; - PointCov.noalias() = (E.transpose() * E).inverse(); + /// Computes Point Covariance P from E + static Matrix PointCov(Matrix& E) { + return (E.transpose() * E).inverse(); } - // **************************************************************************************************** - /// Compute F, E only (called below in both vanilla and SVD versions) - /// Given a Point3, assumes dimensionality is 3 - double computeJacobians(std::vector& Fblocks, Matrix& E, - Vector& b, const Cameras& cameras, const Point3& point) const { - - size_t numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - b = zero(ZDim * numKeys); - double f = 0; - - Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D); - for (size_t i = 0; i < this->measured_.size(); i++) { - - Vector bi; - try { - bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); - if(body_P_sensor_){ - Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); - Matrix J(6, 6); - Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); - Fi = Fi * J; - } - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); - - f += bi.squaredNorm(); - if (D == 6) { // optimize only camera pose - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); - } else { - Hcam.block(0, 0) = Fi; // ZDim x 6 block for the cameras - Hcam.block(0, 6) = Hcali; // ZDim x nrCal block for the cameras - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); - } - E.block(ZDim * i, 0) = Ei; - subInsert(b, bi, ZDim * i); - } - return f; + /** + * Compute F, E, and b (called below in both vanilla and SVD versions), where + * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives + * with respect to the point. The value of cameras/point are passed as parameters. + * TODO: Kill this obsolete method + */ + template + void computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras, const POINT& point) const { + // Project into Camera set and calculate derivatives + // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar) + // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z| + // = |A*dx - (z-h(x_bar))| + b = -unwhitenedError(cameras, point, Fblocks, E); } - // **************************************************************************************************** - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, Matrix& E, - Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, - double lambda = 0.0, bool diagonalDamping = false) const { - - double f = computeJacobians(Fblocks, E, b, cameras, point); - - // Point covariance inv(E'*E) - Matrix3 EtE = E.transpose() * E; - - if (diagonalDamping) { // diagonal of the hessian - EtE(0, 0) += lambda * EtE(0, 0); - EtE(1, 1) += lambda * EtE(1, 1); - EtE(2, 2) += lambda * EtE(2, 2); - }else{ - EtE(0, 0) += lambda; - EtE(1, 1) += lambda; - EtE(2, 2) += lambda; - } - - PointCov.noalias() = (EtE).inverse(); - - return f; - } - - // **************************************************************************************************** - // TODO, there should not be a Matrix version, really - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const Point3& point, - const double lambda = 0.0) const { - - size_t numKeys = this->keys_.size(); - std::vector Fblocks; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, - lambda); - F = zeros(This::ZDim * numKeys, D * numKeys); - - for (size_t i = 0; i < this->keys_.size(); ++i) { - F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - } - return f; - } - - // **************************************************************************************************** /// SVD version - double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, - Vector& b, const Cameras& cameras, const Point3& point, double lambda = - 0.0, bool diagonalDamping = false) const { + template + void computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, + Vector& b, const Cameras& cameras, const POINT& point) const { Matrix E; - Matrix3 PointCov; // useless - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); // diagonalDamping should have no effect (only on PointCov) + computeJacobians(Fblocks, E, b, cameras, point); + + static const int N = FixedDimension::value; // 2 (Unit3) or 3 (Point3) // Do SVD on A Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); - // Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3); - size_t numKeys = this->keys_.size(); - Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns - - return f; + size_t m = this->keys_.size(); + Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns } - // **************************************************************************************************** - /// Matrix version of SVD - // TODO, there should not be a Matrix version, really - double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, - const Cameras& cameras, const Point3& point) const { - - int numKeys = this->keys_.size(); - std::vector Fblocks; - double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - F.resize(ZDim * numKeys, D * numKeys); - F.setZero(); - - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - - return f; - } - - // **************************************************************************************************** - /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + /// Linearize to a Hessianfactor + boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { - int numKeys = this->keys_.size(); - - std::vector Fblocks; + std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); + computeJacobians(Fblocks, E, b, cameras, point); -//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix -#ifdef HESSIAN_BLOCKS - // Create structures for Hessian Factors - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + // build augmented hessian + SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, + b); - sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs); - // schurComplement(Fblocks, E, PointCov, b, Gs, gs); - - //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); - //std::vector < Vector > gs2(gs.begin(), gs.end()); - - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, f); -#else // we create directly a SymmetricBlockMatrix - size_t n1 = D * numKeys + 1; - std::vector dims(numKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, D); - dims.back() = 1; - - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) - sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - augmentedHessian(numKeys, numKeys)(0, 0) = f; - return boost::make_shared >(this->keys_, + return boost::make_shared >(keys_, augmentedHessian); -#endif } - // **************************************************************************************************** - // slow version - works on full (sparse) matrices - void schurComplement(const std::vector& Fblocks, const Matrix& E, - const Matrix& PointCov, const Vector& b, - /*output ->*/std::vector& Gs, std::vector& gs) const { - // Schur complement trick - // Gs = F' * F - F' * E * inv(E'*E) * E' * F - // gs = F' * (b - E * inv(E'*E) * E' * b) - // This version uses full matrices - - int numKeys = this->keys_.size(); - - /// Compute Full F ???? - Matrix F = zeros(ZDim * numKeys, D * numKeys); - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - - Matrix H(D * numKeys, D * numKeys); - Vector gs_vector; - - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); - - // Populate Gs and gs - int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment(i1D); - for (DenseIndex i2 = 0; i2 < numKeys; i2++) { - if (i2 >= i1) { - Gs.at(GsCount2) = H.block(i1D, i2 * D); - GsCount2++; - } - } - } - } - - // **************************************************************************************************** - void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - - // D = (Dx2) * (2) - // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian(i1, i1) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); - - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian(i1, i2) = -Fi1.transpose() - * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - } - - // **************************************************************************************************** - void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, - /*output ->*/std::vector& Gs, std::vector& gs) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); - - int GsIndex = 0; - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - // GsIndex points to the upper triangular blocks - // 0 1 2 3 4 - // X 5 6 7 8 - // X X 9 10 11 - // X X X 12 13 - // X X X X 14 - const Matrix2D& Fi1 = Fblocks.at(i1).second; - - const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - - { // for i1 = i2 - // D = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b - -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - Gs.at(GsIndex) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); - GsIndex++; - } - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - Gs.at(GsIndex) = -Fi1.transpose() - * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - GsIndex++; - } - } // end of for over cameras - } - - // **************************************************************************************************** + /** + * Add the contribution of the smart factor to a pre-allocated Hessian, + * using sparse linear algebra. More efficient than the creation of the + * Hessian without preallocation of the SymmetricBlockMatrix + */ void updateAugmentedHessian(const Cameras& cameras, const Point3& point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian, const FastVector allKeys) const { - - // int numKeys = this->keys_.size(); - - std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + std::vector Fblocks; + computeJacobians(Fblocks, E, b, cameras, point); + Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, + augmentedHessian); } - // **************************************************************************************************** - void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, - const double f, const FastVector allKeys, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - MatrixDD matrixBlock; - typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix - - FastMap KeySlotMap; - for (size_t slot=0; slot < allKeys.size(); slot++) - KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); // cameras observing current point - size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - - // D = (DxZDim) * (ZDim) - // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) - // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) - // Key cameraKey_i1 = this->keys_[i1]; - DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]]; - - // information vector - store previous vector - // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); - // add contribution of current factor - augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() - + Fi1.transpose() * b.segment(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - // main block diagonal - store previous block - matrixBlock = augmentedHessian(aug_i1, aug_i1); - // add contribution of current factor - augmentedHessian(aug_i1, aug_i1) = matrixBlock + - ( Fi1.transpose() * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1) ); - - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - //Key cameraKey_i2 = this->keys_[i2]; - DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; - - // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) - // off diagonal block - store previous block - // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); - // add contribution of current factor - augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() - - Fi1.transpose() * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - - augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; + /// Whiten the Jacobians computed by computeJacobians using noiseModel_ + void whitenJacobians(std::vector& F, Matrix& E, Vector& b) const { + noiseModel_->WhitenSystem(E, b); + // TODO make WhitenInPlace work with any dense matrix type + for (size_t i = 0; i < F.size(); i++) + F[i] = noiseModel_->Whiten(F[i]); } - // **************************************************************************************************** - boost::shared_ptr > createRegularImplicitSchurFactor( + /// Return Jacobians as RegularImplicitSchurFactor with raw access + boost::shared_ptr > // + createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point, + double lambda = 0.0, bool diagonalDamping = false) const { + Matrix E; + Vector b; + std::vector F; + computeJacobians(F, E, b, cameras, point); + whitenJacobians(F, E, b); + Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); + return boost::make_shared >(keys_, F, E, + P, b); + } + + /** + * Return Jacobians as JacobianFactorQ + */ + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - typename boost::shared_ptr > f( - new RegularImplicitSchurFactor()); - computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), - cameras, point, lambda, diagonalDamping); - f->initKeys(); - return f; - } - - // **************************************************************************************************** - boost::shared_ptr > createJacobianQFactor( - const Cameras& cameras, const Point3& point, double lambda = 0.0, - bool diagonalDamping = false) const { - std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, b); + std::vector F; + computeJacobians(F, E, b, cameras, point); + const size_t M = b.size(); + Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); + SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); + return boost::make_shared >(keys_, F, E, P, b, n); } - // **************************************************************************************************** + /** + * Return Jacobians as JacobianFactorSVD + * TODO lambda is currently ignored + */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { - size_t numKeys = this->keys_.size(); - std::vector Fblocks; + size_t m = this->keys_.size(); + std::vector F; Vector b; - Matrix Enull(ZDim*numKeys, ZDim*numKeys-3); - computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); + const size_t M = ZDim * m; + Matrix E0(M, M - 3); + computeJacobiansSVD(F, E0, b, cameras, point); + SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3, + noiseModel_->sigma()); + return boost::make_shared >(keys_, F, E0, b, n); + } + + /// Create BIG block-diagonal matrix F from Fblocks + static void FillDiagonalF(const std::vector& Fblocks, Matrix& F) { + size_t m = Fblocks.size(); + F.resize(ZDim * m, Dim * m); + F.setZero(); + for (size_t i = 0; i < m; ++i) + F.block(ZDim * i, Dim * i) = Fblocks.at(i); + } + + + Pose3 body_P_sensor() const{ + if(body_P_sensor_) + return *body_P_sensor_; + else + return Pose3(); // if unspecified, the transformation is the identity } private: - /// Serialization function +/// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; +// end class SmartFactorBase -template -const int SmartFactorBase::ZDim; +// Definitions need to avoid link errors (above are only declarations) +template const int SmartFactorBase::Dim; +template const int SmartFactorBase::ZDim; } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 2bfcbfaa0..d6b549acb 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -32,110 +31,135 @@ namespace gtsam { -/** - * Structure for storing some state memory, used to speed up optimization - * @addtogroup SLAM - */ -class SmartProjectionFactorState { +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; -protected: +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + +/* + * Parameters for the smart projection factors + */ +class GTSAM_EXPORT SmartProjectionParams { public: - SmartProjectionFactorState() { - } - // Hessian representation (after Schur complement) - bool calculatedHessian; - Matrix H; - Vector gs_vector; - std::vector Gs; - std::vector gs; - double f; -}; + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor -enum LinearizationMode { - HESSIAN, JACOBIAN_SVD, JACOBIAN_Q + /// @name Parameters governing the triangulation + /// @{ + mutable TriangulationParameters triangulation; + const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + /// @} + + /// @name Parameters governing how triangulation result is treated + /// @{ + const bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + const bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + + // Constructor + SmartProjectionParams(LinearizationMode linMode = HESSIAN, + DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false) : + linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( + 1e-5), throwCheirality(throwCheirality), verboseCheirality( + verboseCheirality) { + } + + virtual ~SmartProjectionParams() { + } + + void print(const std::string& str) const { + std::cout << "linearizationMode: " << linearizationMode << "\n"; + std::cout << " degeneracyMode: " << degeneracyMode << "\n"; + std::cout << triangulation << std::endl; + } + + LinearizationMode getLinearizationMode() const { + return linearizationMode; + } + DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + TriangulationParameters getTriangulationParameters() const { + return triangulation; + } + bool getVerboseCheirality() const { + return verboseCheirality; + } + bool getThrowCheirality() const { + return throwCheirality; + } + void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + void setRankTolerance(double rankTol) { + triangulation.rankTolerance = rankTol; + } + void setEnableEPI(bool enableEPI) { + triangulation.enableEPI = enableEPI; + } + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold) { + triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold) { + triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } }; /** - * SmartProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? + * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ -template -class SmartProjectionFactor: public SmartFactorBase, D> { +template +class SmartProjectionFactor: public SmartFactorBase { + +public: + +private: + typedef SmartFactorBase Base; + typedef SmartProjectionFactor This; + typedef SmartProjectionFactor SmartProjectionCameraFactor; + protected: - // Some triangulation parameters - const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ - const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate + /// @name Parameters + /// @{ + const SmartProjectionParams params_; + /// @} + + /// @name Caching triangulation + /// @{ + mutable TriangulationResult result_; ///< result from triangulateSafe mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses - - const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases - - const bool enableEPI_; ///< if set to true, will refine triangulation using LM - - const double linearizationThreshold_; ///< threshold to decide whether to re-linearize - mutable std::vector cameraPosesLinearization_; ///< current linearization poses - - mutable Point3 point_; ///< Current estimate of the 3D point - - mutable bool degenerate_; - mutable bool cheiralityException_; - - // verbosity handling for Cheirality Exceptions - const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) - - boost::shared_ptr state_; - - /// shorthand for smart projection factor state variable - typedef boost::shared_ptr SmartFactorStatePtr; - - /// shorthand for base class type - typedef SmartFactorBase, D> Base; - - double landmarkDistanceThreshold_; // if the landmark is triangulated at a - // distance larger than that the factor is considered degenerate - - double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the - // average reprojection error is smaller than this threshold after triangulation, - // and the factor is disregarded if the error is large - - /// shorthand for this class - typedef SmartProjectionFactor This; - - static const int ZDim = 2; ///< Measurement dimension + /// @} public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - typedef PinholeCamera Camera; - typedef std::vector Cameras; + /// shorthand for a set of cameras + typedef CameraSet Cameras; /** * Constructor - * @param rankTol tolerance used to check if point triangulation is degenerate - * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) - * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, - * otherwise the factor is simply neglected - * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from sensor to body frame (default identity) + * @param body_P_sensor pose of the camera in the body frame + * @param params internal parameters of the smart factors */ - SmartProjectionFactor(const double rankTol, const double linThreshold, - const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, - double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : - Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( - 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( - linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state), - landmarkDistanceThreshold_(landmarkDistanceThreshold), - dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { + SmartProjectionFactor( + const boost::optional body_P_sensor = boost::none, + const SmartProjectionParams& params = SmartProjectionParams()) : + Base(body_P_sensor), params_(params), // + result_(TriangulationResult::Degenerate()) { } /** Virtual destructor */ @@ -149,24 +173,33 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "SmartProjectionFactor, z = \n"; - std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; - std::cout << "degenerate_ = " << degenerate_ << std::endl; - std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; + std::cout << s << "SmartProjectionFactor\n"; + std::cout << "linearizationMode:\n" << params_.linearizationMode + << std::endl; + std::cout << "triangulationParameters:\n" << params_.triangulation + << std::endl; + std::cout << "result:\n" << result_ << std::endl; Base::print("", keyFormatter); } - /// Check if the new linearization point_ is the same as the one used for previous triangulation + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && params_.linearizationMode == e->params_.linearizationMode + && Base::equals(p, tol); + } + + /// Check if the new linearization point is the same as the one used for previous triangulation bool decideIfTriangulate(const Cameras& cameras) const { - // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate + // several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate // Note that this is not yet "selecting linearization", that will come later, and we only check if the - // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_ + // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point size_t m = cameras.size(); bool retriangulate = false; - // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + // if we do not have a previous linearization point or the new linearization point includes more poses if (cameraPosesTriangulation_.empty() || cameras.size() != cameraPosesTriangulation_.size()) retriangulate = true; @@ -174,7 +207,7 @@ public: if (!retriangulate) { for (size_t i = 0; i < cameras.size(); i++) { if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], - retriangulationThreshold_)) { + params_.retriangulationThreshold)) { retriangulate = true; // at least two poses are different, hence we retriangulate break; } @@ -189,141 +222,39 @@ public: cameraPosesTriangulation_.push_back(cameras[i].pose()); } - return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation - } - - /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization - bool decideIfLinearize(const Cameras& cameras) const { - // "selective linearization" - // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose - // (we only care about the "rigidity" of the poses, not about their absolute pose) - - if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize - return true; - - // if we do not have a previous linearization point_ or the new linearization point_ includes more poses - if (cameraPosesLinearization_.empty() - || (cameras.size() != cameraPosesLinearization_.size())) - return true; - - Pose3 firstCameraPose, firstCameraPoseOld; - for (size_t i = 0; i < cameras.size(); i++) { - - if (i == 0) { // we store the initial pose, this is useful for selective re-linearization - firstCameraPose = cameras[i].pose(); - firstCameraPoseOld = cameraPosesLinearization_[i]; - continue; - } - - // we compare the poses in the frame of the first pose - Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); - Pose3 localCameraPoseOld = firstCameraPoseOld.between( - cameraPosesLinearization_[i]); - if (!localCameraPose.equals(localCameraPoseOld, - this->linearizationThreshold_)) - return true; // at least two "relative" poses are different, hence we re-linearize - } - return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize + return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation } /// triangulateSafe - size_t triangulateSafe(const Values& values) const { - return triangulateSafe(this->cameras(values)); - } - - /// triangulateSafe - size_t triangulateSafe(const Cameras& cameras) const { + TriangulationResult triangulateSafe(const Cameras& cameras) const { size_t m = cameras.size(); - if (m < 2) { // if we have a single pose the corresponding factor is uninformative - degenerate_ = true; - return m; - } + if (m < 2) // if we have a single pose the corresponding factor is uninformative + return TriangulationResult::Degenerate(); + bool retriangulate = decideIfTriangulate(cameras); - - if (retriangulate) { - // We triangulate the 3D position of the landmark - try { - // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - point_ = triangulatePoint3(cameras, this->measured_, - rankTolerance_, enableEPI_); - degenerate_ = false; - cheiralityException_ = false; - - // Check landmark distance and reprojection errors to avoid outliers - double totalReprojError = 0.0; - size_t i=0; - BOOST_FOREACH(const Camera& camera, cameras) { - Point3 cameraTranslation = camera.pose().translation(); - // we discard smart factors corresponding to points that are far away - if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ - degenerate_ = true; - break; - } - const Point2& zi = this->measured_.at(i); - try { - Point2 reprojectionError(camera.project(point_) - zi); - totalReprojError += reprojectionError.vector().norm(); - } catch (CheiralityException) { - cheiralityException_ = true; - } - i += 1; - } - // we discard smart factors that have large reprojection error - if(dynamicOutlierRejectionThreshold_ > 0 && - totalReprojError/m > dynamicOutlierRejectionThreshold_) - degenerate_ = true; - - } catch (TriangulationUnderconstrainedException&) { - // if TriangulationUnderconstrainedException can be - // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before - // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) - // in the second case we want to use a rotation-only smart factor - degenerate_ = true; - cheiralityException_ = false; - } catch (TriangulationCheiralityException&) { - // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers - // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint - cheiralityException_ = true; - } - } - return m; + if (retriangulate) + result_ = gtsam::triangulateSafe(cameras, this->measured_, + params_.triangulation); + return result_; } /// triangulate bool triangulateForLinearize(const Cameras& cameras) const { - - bool isDebug = false; - size_t nrCameras = this->triangulateSafe(cameras); - - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - if (isDebug) { - std::cout << "createRegularImplicitSchurFactor: degenerate configuration" - << std::endl; - } - return false; - } else { - - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - } - return true; - } + triangulateSafe(cameras); // imperative, might reset result_ + return (result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( - const Cameras& cameras, const double lambda = 0.0) const { + boost::shared_ptr > createHessianFactor( + const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = + false) const { - bool isDebug = false; size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors - std::vector < Key > js; - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + std::vector js; + std::vector Gs(numKeys * (numKeys + 1) / 2); + std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) { std::cout @@ -332,278 +263,198 @@ public: exit(1); } - this->triangulateSafe(cameras); + triangulateSafe(cameras); - if (numKeys < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - // std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) - m = zeros(D, D); + if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { + // failed: return"empty" Hessian + BOOST_FOREACH(Matrix& m, Gs) + m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(D); - return boost::make_shared >(this->keys_, Gs, gs, - 0.0); + v = zero(Base::Dim); + return boost::make_shared >(this->keys_, + Gs, gs, 0.0); } - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - } - - bool doLinearize = this->decideIfLinearize(cameras); - - if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize - for (size_t i = 0; i < cameras.size(); i++) - this->cameraPosesLinearization_[i] = cameras[i].pose(); - - if (!doLinearize) { // return the previous Hessian factor - std::cout << "=============================" << std::endl; - std::cout << "doLinearize " << doLinearize << std::endl; - std::cout << "this->linearizationThreshold_ " - << this->linearizationThreshold_ << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; - std::cout - << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" - << std::endl; - exit(1); - return boost::make_shared >(this->keys_, - this->state_->Gs, this->state_->gs, this->state_->f); - } - - // ================================================================== - Matrix F, E; - Matrix3 PointCov; + // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). + std::vector Fblocks; + Matrix E; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); - // Schur complement trick - // Frank says: should be possible to do this more efficiently? - // And we care, as in grouped factors this is called repeatedly - Matrix H(D * numKeys, D * numKeys); - Vector gs_vector; + // Whiten using noise model + Base::whitenJacobians(Fblocks, E, b); - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); - if (isDebug) - std::cout << "gs_vector size " << gs_vector.size() << std::endl; + // build augmented hessian + SymmetricBlockMatrix augmentedHessian = // + Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); - // Populate Gs and gs - int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment < D > (i1D); - for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { - if (i2 >= i1) { - Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); - GsCount2++; - } - } - } - // ================================================================== - if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables - this->state_->Gs = Gs; - this->state_->gs = gs; - this->state_->f = f; - } - return boost::make_shared >(this->keys_, Gs, gs, f); + return boost::make_shared >(this->keys_, + augmentedHessian); } // create factor - boost::shared_ptr > createRegularImplicitSchurFactor( + boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createRegularImplicitSchurFactor(cameras, point_, lambda); + return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda); else - return boost::shared_ptr >(); + // failed: return empty + return boost::shared_ptr >(); } /// create factor - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createJacobianQFactor(cameras, point_, lambda); + return Base::createJacobianQFactor(cameras, *result_, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + // failed: return empty + return boost::make_shared >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { - Cameras myCameras; - // TODO triangulate twice ?? - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); - if (nonDegenerate) - return createJacobianQFactor(myCameras, lambda); - else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return createJacobianQFactor(this->cameras(values), lambda); } /// different (faster) way to compute Jacobian factor - boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, - double lambda) const { + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createJacobianSVDFactor(cameras, point_, lambda); + return Base::createJacobianSVDFactor(cameras, *result_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + // failed: return empty + return boost::make_shared >(this->keys_); } - /// Returns true if nonDegenerate - bool computeCamerasAndTriangulate(const Values& values, - Cameras& myCameras) const { - Values valuesFactor; + /// linearize to a Hessianfactor + virtual boost::shared_ptr > linearizeToHessian( + const Values& values, double lambda = 0.0) const { + return createHessianFactor(this->cameras(values), lambda); + } - // Select only the cameras - BOOST_FOREACH(const Key key, this->keys_) - valuesFactor.insert(key, values.at(key)); + /// linearize to an Implicit Schur factor + virtual boost::shared_ptr > linearizeToImplicit( + const Values& values, double lambda = 0.0) const { + return createRegularImplicitSchurFactor(this->cameras(values), lambda); + } - myCameras = this->cameras(valuesFactor); - size_t nrCameras = this->triangulateSafe(myCameras); + /// linearize to a JacobianfactorQ + virtual boost::shared_ptr > linearizeToJacobian( + const Values& values, double lambda = 0.0) const { + return createJacobianQFactor(this->cameras(values), lambda); + } - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) - return false; - - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - - if (this->degenerate_) { - std::cout << "SmartProjectionFactor: this is not ready" << std::endl; - std::cout << "this->cheiralityException_ " << this->cheiralityException_ - << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Cameras& cameras, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + switch (params_.linearizationMode) { + case HESSIAN: + return createHessianFactor(cameras, lambda); + case IMPLICIT_SCHUR: + return createRegularImplicitSchurFactor(cameras, lambda); + case JACOBIAN_SVD: + return createJacobianSVDFactor(cameras, lambda); + case JACOBIAN_Q: + return createJacobianQFactor(cameras, lambda); + default: + throw std::runtime_error("SmartFactorlinearize: unknown mode"); } - return true; } - /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Values& values, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + Cameras cameras = this->cameras(values); + return linearizeDamped(cameras, lambda); + } + + /// linearize + virtual boost::shared_ptr linearize( + const Values& values) const { + return linearizeDamped(values); + } + + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const { + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + cameras.project2(*result_, boost::none, E); return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); - } - - /// Version that takes values, and creates the point - bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); - if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); - return nonDegenerate; + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateAndComputeE(E, cameras); } /// Compute F, E only (called below in both vanilla and SVD versions) /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate - double computeJacobians(std::vector& Fblocks, - Matrix& E, Vector& b, const Cameras& cameras) const { + void computeJacobiansWithTriangulatedPoint( + std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras) const { - if (this->degenerate_) { - std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; - std::cout << "point " << point_ << std::endl; - std::cout - << "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used" - << std::endl; - if (D > 6) { - std::cout - << "Management of degeneracy is not yet ready when one also optimizes for the calibration " - << std::endl; - } - - int numKeys = this->keys_.size(); - E = zeros(2 * numKeys, 2); - b = zero(2 * numKeys); - double f = 0; - for (size_t i = 0; i < this->measured_.size(); i++) { - if (i == 0) { // first pose - this->point_ = cameras[i].backprojectPointAtInfinity( - this->measured_.at(i)); - // 3D parametrization of point at infinity: [px py 1] - } - Matrix Fi, Ei; - Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei) - - this->measured_.at(i)).vector(); - - this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); - f += bi.squaredNorm(); - Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); - E.block < 2, 2 > (2 * i, 0) = Ei; - subInsert(b, bi, 2 * i); - } - return f; + if (!result_) { + // Handle degeneracy + // TODO check flag whether we should do this + Unit3 backProjected = cameras[0].backprojectPointAtInfinity( + this->measured_.at(0)); + Base::computeJacobians(Fblocks, E, b, cameras, backProjected); } else { - // nondegenerate: just return Base version - return Base::computeJacobians(Fblocks, E, b, cameras, point_); - } // end else + // valid result: just return Base version + Base::computeJacobians(Fblocks, E, b, cameras, *result_); + } } - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, - const double lambda = 0.0) const { - - double f = computeJacobians(Fblocks, E, b, cameras); - - // Point covariance inv(E'*E) - PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); - - return f; + /// Version that takes values, and creates the point + bool triangulateAndComputeJacobians( + std::vector& Fblocks, Matrix& E, Vector& b, + const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + return nonDegenerate; } /// takes values - bool computeJacobiansSVD(std::vector& Fblocks, - Matrix& Enull, Vector& b, const Values& values) const { - typename Base::Cameras myCameras; - double good = computeCamerasAndTriangulate(values, myCameras); - if (good) - computeJacobiansSVD(Fblocks, Enull, b, myCameras); - return true; - } - - /// SVD version - double computeJacobiansSVD(std::vector& Fblocks, - Matrix& Enull, Vector& b, const Cameras& cameras) const { - return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); - } - - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - // TODO should there be a lambda? - double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, - const Cameras& cameras) const { - return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); - } - - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); - } - - /// Calculate vector of re-projection errors, before applying noise model - /// Assumes triangulation was done and degeneracy handled - Vector reprojectionError(const Cameras& cameras) const { - return Base::reprojectionError(cameras, point_); - } - - /// Calculate vector of re-projection errors, before applying noise model - Vector reprojectionError(const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + bool triangulateAndComputeJacobiansSVD( + std::vector& Fblocks, Matrix& Enull, Vector& b, + const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - return reprojectionError(myCameras); + Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); + return nonDegenerate; + } + + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionErrorAfterTriangulation(const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + return Base::unwhitenedError(cameras, *result_); else - return zero(myCameras.size() * 2); + return zero(cameras.size() * 2); } /** @@ -615,86 +466,57 @@ public: double totalReprojectionError(const Cameras& cameras, boost::optional externalPoint = boost::none) const { - size_t nrCameras; - if (externalPoint) { - nrCameras = this->keys_.size(); - point_ = *externalPoint; - degenerate_ = false; - cheiralityException_ = false; - } else { - nrCameras = this->triangulateSafe(cameras); - } + if (externalPoint) + result_ = TriangulationResult(*externalPoint); + else + result_ = triangulateSafe(cameras); - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { + if (result_) + // All good, just use version in base class + return Base::totalReprojectionError(cameras, *result_); + else if (params_.degeneracyMode == HANDLE_INFINITY) { + // Otherwise, manage the exceptions with rotation-only factors + const Point2& z0 = this->measured_.at(0); + Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0); + return Base::totalReprojectionError(cameras, backprojected); + } else // if we don't want to manage the exceptions we discard the factor - // std::cout << "In error evaluation: exception" << std::endl; return 0.0; - } + } - if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors - std::cout - << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!" - << std::endl; - this->degenerate_ = true; - } - - if (this->degenerate_) { - // return 0.0; // TODO: this maybe should be zero? - std::cout - << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!" - << std::endl; - size_t i = 0; - double overallError = 0; - BOOST_FOREACH(const Camera& camera, cameras) { - const Point2& zi = this->measured_.at(i); - if (i == 0) // first pose - this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity - Point2 reprojectionError( - camera.projectPointAtInfinity(this->point_) - zi); - overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); - i += 1; - } - return overallError; - } else { - // Just use version in base class - return Base::totalReprojectionError(cameras, point_); + /// Calculate total reprojection error + virtual double error(const Values& values) const { + if (this->active(values)) { + return totalReprojectionError(Base::cameras(values)); + } else { // else of active flag + return 0.0; } } - /// Cameras are computed in derived class - virtual Cameras cameras(const Values& values) const = 0; - /** return the landmark */ - boost::optional point() const { - return point_; + TriangulationResult point() const { + return result_; } /** COMPUTE the landmark */ - boost::optional point(const Values& values) const { - triangulateSafe(values); - return point_; + TriangulationResult point(const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateSafe(cameras); + } + + /// Is result valid? + bool isValid() const { + return result_; } /** return the degenerate state */ - inline bool isDegenerate() const { - return (cheiralityException_ || degenerate_); + bool isDegenerate() const { + return result_.degenerate(); } /** return the cheirality status flag */ - inline bool isPointBehindCamera() const { - return cheiralityException_; - } - /** return cheirality verbosity */ - inline bool verboseCheirality() const { - return verboseCheirality_; - } - - /** return flag for throwing cheirality exceptions */ - inline bool throwCheirality() const { - return throwCheirality_; + bool isPointBehindCamera() const { + return result_.behindCamera(); } private: @@ -704,12 +526,16 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(throwCheirality_); - ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality); + ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality); } +} +; + +/// traits +template +struct traits > : public Testable< + SmartProjectionFactor > { }; -template -const int SmartProjectionFactor::ZDim; - } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 0801d141f..93a4449f5 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -37,88 +37,38 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor< + PinholePose > { + +private: + typedef PinholePose Camera; + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactor This; + protected: - LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) - - std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) + boost::shared_ptr K_; ///< calibration object (one for all cameras) public: - /// shorthand for base class type - typedef SmartProjectionFactor Base; - - /// shorthand for this class - typedef SmartProjectionPoseFactor This; - /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /** * Constructor - * @param rankTol tolerance used to check if point triangulation is degenerate - * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) - * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, - * otherwise the factor is simply neglected - * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from sensor to body frame (default identity) + * @param K (fixed) calibration, assumed to be the same for all cameras + * @param body_P_sensor pose of the camera in the body frame + * @param params internal parameters of the smart factors */ - SmartProjectionPoseFactor(const double rankTol = 1, - const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, - landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} + SmartProjectionPoseFactor(const boost::shared_ptr K, + const boost::optional body_P_sensor = boost::none, + const SmartProjectionParams& params = SmartProjectionParams()) : + Base(body_P_sensor, params), K_(K) { + } /** Virtual destructor */ - virtual ~SmartProjectionPoseFactor() {} - - /** - * add a new measurement and pose key - * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKey is key corresponding to the camera observing the same landmark - * @param noise_i is the measurement noise - * @param K_i is the (known) camera calibration - */ - void add(const Point2 measured_i, const Key poseKey_i, - const SharedNoiseModel noise_i, - const boost::shared_ptr K_i) { - Base::add(measured_i, poseKey_i, noise_i); - K_all_.push_back(K_i); - } - - /** - * Variant of the previous one in which we include a set of measurements - * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noises vector of measurement noises - * @param Ks vector of calibration objects - */ - void add(std::vector measurements, std::vector poseKeys, - std::vector noises, - std::vector > Ks) { - Base::add(measurements, poseKeys, noises); - for (size_t i = 0; i < measurements.size(); i++) { - K_all_.push_back(Ks.at(i)); - } - } - - /** - * Variant of the previous one in which we include a set of measurements with the same noise and calibration - * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noise measurement noise (same for all measurements) - * @param K the (known) camera calibration (same for all measurements) - */ - void add(std::vector measurements, std::vector poseKeys, - const SharedNoiseModel noise, const boost::shared_ptr K) { - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements.at(i), poseKeys.at(i), noise); - K_all_.push_back(K); - } + virtual ~SmartProjectionPoseFactor() { } /** @@ -129,64 +79,15 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "SmartProjectionPoseFactor, z = \n "; - BOOST_FOREACH(const boost::shared_ptr& K, K_all_) - K->print("calibration = "); Base::print("", keyFormatter); } /// equals virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); - return e && Base::equals(p, tol); } - /// get the dimension of the factor - virtual size_t dim() const { - return 6 * this->keys_.size(); - } - - /** - * Collect all cameras involved in this factor - * @param values Values structure which must contain camera poses corresponding - * to keys involved in this factor - * @return vector of Values - */ - typename Base::Cameras cameras(const Values& values) const { - typename Base::Cameras cameras; - size_t i=0; - BOOST_FOREACH(const Key& k, this->keys_) { - Pose3 pose = values.at(k); - if(Base::body_P_sensor_) - pose = pose.compose(*(Base::body_P_sensor_)); - - typename Base::Camera camera(pose, *K_all_[i++]); - cameras.push_back(camera); - } - return cameras; - } - - /** - * Linearize to Gaussian Factor - * @param values Values structure which must contain camera poses for this factor - * @return - */ - virtual boost::shared_ptr linearize( - const Values& values) const { - // depending on flag set on construction we may linearize to different linear factors - switch(linearizeTo_){ - case JACOBIAN_SVD : - return this->createJacobianSVDFactor(cameras(values), 0.0); - break; - case JACOBIAN_Q : - return this->createJacobianQFactor(cameras(values), 0.0); - break; - default: - return this->createHessianFactor(cameras(values)); - break; - } - } - /** * error calculates the error of the factor. */ @@ -198,9 +99,28 @@ public: } } - /** return the calibration object */ - inline const std::vector > calibration() const { - return K_all_; + /** return calibration shared pointers */ + inline const boost::shared_ptr calibration() const { + return K_; + } + + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses corresponding + * to keys involved in this factor + * @return vector of Values + */ + typename Base::Cameras cameras(const Values& values) const { + typename Base::Cameras cameras; + BOOST_FOREACH(const Key& k, this->keys_) { + Pose3 pose = values.at(k); + if (Base::body_P_sensor_) + pose = pose.compose(*(Base::body_P_sensor_)); + + Camera camera(pose, K_); + cameras.push_back(camera); + } + return cameras; } private: @@ -208,15 +128,18 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); + ar & BOOST_SERIALIZATION_NVP(K_); } -}; // end of class declaration +}; +// end of class declaration /// traits -template -struct traits > : public Testable > {}; +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { +}; } // \ namespace gtsam diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 7b21e044f..578422eaf 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -169,7 +169,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index b7f6a20dc..b94eafba7 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -16,8 +16,7 @@ */ #include -#include -#include +#include #include namespace gtsam { @@ -27,18 +26,24 @@ namespace gtsam { * The calibration and pose are assumed known. * @addtogroup SLAM */ -template +template class TriangulationFactor: public NoiseModelFactor1 { public: - /// Camera type - typedef PinholeCamera Camera; + /// CAMERA type + typedef CAMERA Camera; protected: + /// shorthand for base class type + typedef NoiseModelFactor1 Base; + + /// shorthand for this class + typedef TriangulationFactor This; + // Keep a copy of measurement and calibration for I/O - const Camera camera_; ///< Camera in which this landmark was seen + const CAMERA camera_; ///< CAMERA in which this landmark was seen const Point2 measured_; ///< 2D measurement // verbosity handling for Cheirality Exceptions @@ -47,12 +52,6 @@ protected: public: - /// shorthand for base class type - typedef NoiseModelFactor1 Base; - - /// shorthand for this class - typedef TriangulationFactor This; - /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -70,7 +69,7 @@ public: * @param throwCheirality determines whether Cheirality exceptions are rethrown * @param verboseCheirality determines whether exceptions are printed for Cheirality */ - TriangulationFactor(const Camera& camera, const Point2& measured, + TriangulationFactor(const CAMERA& camera, const Point2& measured, const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false, bool verboseCheirality = false) : Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_( @@ -114,7 +113,7 @@ public: Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); + Point2 error(camera_.project2(point, boost::none, H2) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) @@ -154,7 +153,7 @@ public: // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector(); + b = -(camera_.project2(point, boost::none, A) - measured_).vector(); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); @@ -184,7 +183,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(camera_); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index d3a7c1e84..5ad1ff2c0 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -15,19 +15,34 @@ * @brief utility functions for loading datasets */ -#include -#include #include +#include +#include +#include #include -#include -#include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include +#include +#include +#include +#include +#include #include -#include -#include +#include +#include using namespace std; namespace fs = boost::filesystem; @@ -64,8 +79,8 @@ string findExampleDataFile(const string& name) { throw invalid_argument( "gtsam::findExampleDataFile could not find a matching file in\n" - SOURCE_TREE_DATASET_DIR " or\n" - INSTALLED_DATASET_DIR " named\n" + + GTSAM_SOURCE_TREE_DATASET_DIR " or\n" + GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name + ".graph, or " + name + ".txt"); } @@ -670,6 +685,7 @@ bool readBundler(const string& filename, SfM_data &data) { float u, v; is >> cam_idx >> point_idx >> u >> v; track.measurements.push_back(make_pair(cam_idx, Point2(u, -v))); + track.siftIndices.push_back(make_pair(cam_idx, point_idx)); } data.tracks.push_back(track); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 3a0f8edb7..54e27229c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,16 +18,21 @@ #pragma once -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include // for pair +#include #include +#include // for pair +#include namespace gtsam { @@ -137,11 +142,15 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfM_Measurement; +/// SfM_Track +typedef std::pair SIFT_Index; + /// Define the structure for the 3D points struct SfM_Track { Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) + std::vector siftIndices; size_t number_measurements() const { return measurements.size(); } diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index f7314c73f..fac2cf03d 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -8,6 +8,7 @@ #pragma once #include +#include #include #include #include @@ -27,6 +28,7 @@ inline Point2_ transform_to(const Pose2_& x, const Point2_& p) { // 3D Geometry typedef Expression Point3_; +typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; @@ -39,33 +41,52 @@ inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { typedef Expression Cal3_S2_; typedef Expression Cal3Bundler_; +/// Expression version of PinholeBase::Project inline Point2_ project(const Point3_& p_cam) { - return Point2_(PinholeCamera::project_to_camera, p_cam); + Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; + return Point2_(f, p_cam); } -template -Point2 project4(const CAMERA& camera, const Point3& p, - OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) { +inline Point2_ project(const Unit3_& p_cam) { + Point2 (*f)(const Unit3&, OptionalJacobian<2, 2>) = &PinholeBase::Project; + return Point2_(f, p_cam); +} + +namespace internal { +// Helper template for project2 expression below +template +Point2 project4(const CAMERA& camera, const POINT& p, + OptionalJacobian<2, CAMERA::dimension> Dcam, + OptionalJacobian<2, FixedDimension::value> Dpoint) { return camera.project2(p, Dcam, Dpoint); } - -template -Point2_ project2(const Expression& camera_, const Point3_& p_) { - return Point2_(project4, camera_, p_); } +template +Point2_ project2(const Expression& camera_, + const Expression& p_) { + return Point2_(internal::project4, camera_, p_); +} + +namespace internal { +// Helper template for project3 expression below +template inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, - OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, + OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } - -inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { - return Point2_(project6, x, p, K); } -template -Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { - return Point2_(K, &CAL::uncalibrate, xy_hat); +template +inline Point2_ project3(const Pose3_& x, const Expression& p, + const Expression& K) { + return Point2_(internal::project6, x, p, K); +} + +template +Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { + return Point2_(K, &CALIBRATION::uncalibrate, xy_hat); } } // \namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h new file mode 100644 index 000000000..8e83ec503 --- /dev/null +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartFactorScenarios.h + * @brief Scenarios for testSmart*.cpp + * @author Frank Dellaert + * @date Feb 2015 + */ + +#pragma once +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// three landmarks ~5 meters infront of camera +Point3 landmark1(5, 0.5, 1.2); +Point3 landmark2(5, -0.5, 1.2); +Point3 landmark3(3, 0, 3.0); +Point3 landmark4(10, 0.5, 1.2); +Point3 landmark5(10, -0.5, 1.2); + +// First camera pose, looking along X-axis, 1 meter above ground plane (x-y) +Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); +// Second camera 1 meter to the right of first camera +Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +// Third camera 1 meter above the first camera +Pose3 pose_above = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); + +// Create a noise unit2 for the pixel error +static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + +static double fov = 60; // degrees +static size_t w = 640, h = 480; + +/* ************************************************************************* */ +// default Cal3_S2 cameras +namespace vanilla { +typedef PinholeCamera Camera; +typedef SmartProjectionFactor SmartFactor; +static Cal3_S2 K(fov, w, h); +static Cal3_S2 K2(1500, 1200, 0, w, h); +Camera level_camera(level_pose, K2); +Camera level_camera_right(pose_right, K2); +Point2 level_uv = level_camera.project(landmark1); +Point2 level_uv_right = level_camera_right.project(landmark1); +Camera cam1(level_pose, K2); +Camera cam2(pose_right, K2); +Camera cam3(pose_above, K2); +typedef GeneralSFMFactor SFMFactor; +SmartProjectionParams params; +} + +/* ************************************************************************* */ +// default Cal3_S2 poses +namespace vanillaPose { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +Camera level_camera(level_pose, sharedK); +Camera level_camera_right(pose_right, sharedK); +Camera cam1(level_pose, sharedK); +Camera cam2(pose_right, sharedK); +Camera cam3(pose_above, sharedK); +} + +/* ************************************************************************* */ +// default Cal3_S2 poses +namespace vanillaPose2 { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); +Camera level_camera(level_pose, sharedK2); +Camera level_camera_right(pose_right, sharedK2); +Camera cam1(level_pose, sharedK2); +Camera cam2(pose_right, sharedK2); +Camera cam3(pose_above, sharedK2); +} + +/* *************************************************************************/ +// Cal3Bundler cameras +namespace bundler { +typedef PinholeCamera Camera; +typedef SmartProjectionFactor SmartFactor; +static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); +Camera level_camera(level_pose, K); +Camera level_camera_right(pose_right, K); +Point2 level_uv = level_camera.project(landmark1); +Point2 level_uv_right = level_camera_right.project(landmark1); +Pose3 pose1 = level_pose; +Camera cam1(level_pose, K); +Camera cam2(pose_right, K); +Camera cam3(pose_above, K); +typedef GeneralSFMFactor SFMFactor; +} +/* *************************************************************************/ +// Cal3Bundler poses +namespace bundlerPose { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static boost::shared_ptr sharedBundlerK( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +Camera level_camera(level_pose, sharedBundlerK); +Camera level_camera_right(pose_right, sharedBundlerK); +Camera cam1(level_pose, sharedBundlerK); +Camera cam2(pose_right, sharedBundlerK); +Camera cam3(pose_above, sharedBundlerK); +} +/* *************************************************************************/ + +template +CAMERA perturbCameraPose(const CAMERA& camera) { + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 cameraPose = camera.pose(); + Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); + return CAMERA(perturbedCameraPose, camera.calibration()); +} + +template +void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2, + const CAMERA& cam3, Point3 landmark, vector& measurements_cam) { + Point2 cam1_uv1 = cam1.project(landmark); + Point2 cam2_uv1 = cam2.project(landmark); + Point2 cam3_uv1 = cam3.project(landmark); + measurements_cam.push_back(cam1_uv1); + measurements_cam.push_back(cam2_uv1); + measurements_cam.push_back(cam3_uv1); +} + +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index d6a5ffa8c..0406c3d27 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -15,16 +15,16 @@ * @author Richard Roberts, Luca Carlone */ -#include - -#include - -#include -#include -#include #include #include +#include +#include +#include + +#include + +#include using namespace gtsam::symbol_shorthand; using namespace std; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index e0e26ecff..3bcc3eccd 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -160,7 +160,7 @@ TEST (EssentialMatrixFactor2, factor) { // Check evaluation Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); - const Point2 pi = SimpleCamera::project_to_camera(P2); + const Point2 pi = PinholeBase::Project(P2); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 240b19a46..a8f85301e 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -19,18 +19,20 @@ #include #include #include +#include +#include +#include #include -#include #include #include #include -#include -#include +#include #include +#include #include #include -using namespace boost; +using namespace boost::assign; #include #include @@ -49,7 +51,8 @@ typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: - void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { + void addMeasurement(int i, int j, const Point2& z, + const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, X(i), L(j))); } @@ -65,98 +68,99 @@ public: }; -static double getGaussian() -{ - double S,V1,V2; - // Use Box-Muller method to create gauss noise from uniform noise - do - { - double U1 = rand() / (double)(RAND_MAX); - double U2 = rand() / (double)(RAND_MAX); - V1 = 2 * U1 - 1; /* V1=[-1,1] */ - V2 = 2 * U2 - 1; /* V2=[-1,1] */ - S = V1 * V1 + V2 * V2; - } while(S>=1); - return sqrt(-2.0f * (double)log(S) / S) * V1; +static double getGaussian() { + double S, V1, V2; + // Use Box-Muller method to create gauss noise from uniform noise + do { + double U1 = rand() / (double) (RAND_MAX); + double U2 = rand() / (double) (RAND_MAX); + V1 = 2 * U1 - 1; /* V1=[-1,1] */ + V2 = 2 * U2 - 1; /* V2=[-1,1] */ + S = V1 * V1 + V2 * V2; + } while (S >= 1); + return sqrt(-2.f * (double) log(S) / S) * V1; } static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); +static const double baseline = 5.; /* ************************************************************************* */ -TEST( GeneralSFMFactor, equals ) -{ - // Create two identical factors and make sure they're equal - Point2 z(323.,240.); - const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); - const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); +static vector genPoint3() { + const double z = 5; + vector landmarks; + landmarks.push_back(Point3(-1., -1., z)); + landmarks.push_back(Point3(-1., 1., z)); + landmarks.push_back(Point3(1., 1., z)); + landmarks.push_back(Point3(1., -1., z)); + landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-2., -2., 2 * z)); + landmarks.push_back(Point3(-2., 2., 2 * z)); + landmarks.push_back(Point3(2., 2., 2 * z)); + landmarks.push_back(Point3(2., -2., 2 * z)); + return landmarks; +} - boost::shared_ptr - factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); +static vector genCameraDefaultCalibration() { + vector X; + X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)))); + return X; +} + +static vector genCameraVariableCalibration() { + const Cal3_S2 K(640, 480, 0.1, 320, 240); + vector X; + X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K)); + return X; +} + +static boost::shared_ptr getOrdering( + const vector& cameras, const vector& landmarks) { + boost::shared_ptr ordering(new Ordering); + for (size_t i = 0; i < landmarks.size(); ++i) + ordering->push_back(L(i)); + for (size_t i = 0; i < cameras.size(); ++i) + ordering->push_back(X(i)); + return ordering; +} + +/* ************************************************************************* */ +TEST( GeneralSFMFactor, equals ) { + // Create two identical factors and make sure they're equal + Point2 z(323., 240.); + const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); + const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); + boost::shared_ptr factor1( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + + boost::shared_ptr factor2( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { - Point2 z(3.,0.); - const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); + Point2 z(3., 0.); + const SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + Projection factor(z, sigma, X(1), L(1)); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; - Point3 t1(0,0,-6); - Pose3 x1(R,t1); + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor->unwhitenedError(values))); + Point3 l1; + values.insert(L(1), l1); + EXPECT( + assert_equal(((Vector ) Vector2(-3., 0.)), + factor.unwhitenedError(values))); } -static const double baseline = 5.0 ; - -/* ************************************************************************* */ -static vector genPoint3() { - const double z = 5; - vector landmarks ; - landmarks.push_back(Point3 (-1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); - landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); - return landmarks ; -} - -static vector genCameraDefaultCalibration() { - vector X ; - X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); - X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); - return X ; -} - -static vector genCameraVariableCalibration() { - const Cal3_S2 K(640,480,0.01,320,240); - vector X ; - X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); - X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); - return X ; -} - -static boost::shared_ptr getOrdering(const vector& cameras, const vector& landmarks) { - boost::shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; - return ordering ; -} - - /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_defaultK ) { @@ -165,32 +169,32 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -202,38 +206,37 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - if ( i == 0 ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; - } - else { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + if (i == 0) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); + } else { + values.insert(L(i), landmarks[i]); } } graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -246,35 +249,34 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; - graph.addMeasurement(j, i, pt, sigma1); + for (size_t i = 0; i < cameras.size(); ++i) { + for (size_t j = 0; j < landmarks.size(); ++j) { + Point2 z = cameras[i].project(landmarks[j]); + graph.addMeasurement(i, j, z, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); - values.insert(L(i), pt) ; + for (size_t j = 0; j < landmarks.size(); ++j) { + Point3 pt(landmarks[j].x() + noise * getGaussian(), + landmarks[j].y() + noise * getGaussian(), + landmarks[j].z() + noise * getGaussian()); + values.insert(L(j), pt); } - for ( size_t i = 0 ; i < cameras.size() ; ++i ) + for (size_t i = 0; i < cameras.size(); ++i) graph.addCameraConstraint(i, cameras[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -288,50 +290,45 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) { - const double - rot_noise = 1e-5, - trans_noise = 1e-3, - focal_noise = 1, - skew_noise = 1e-5; - if ( i == 0 ) { - values.insert(X(i), cameras[i]) ; - } - else { + for (size_t i = 0; i < cameras.size(); ++i) { + const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, + skew_noise = 1e-5; + if (i == 0) { + values.insert(X(i), cameras[i]); + } else { - Vector delta = (Vector(11) << - rot_noise, rot_noise, rot_noise, // rotation - trans_noise, trans_noise, trans_noise, // translation - focal_noise, focal_noise, // f_x, f_y - skew_noise, // s - trans_noise, trans_noise // ux, uy + Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation + trans_noise, trans_noise, trans_noise, // translation + focal_noise, focal_noise, // f_x, f_y + skew_noise, // s + trans_noise, trans_noise // ux, uy ).finished(); - values.insert(X(i), cameras[i].retract(delta)) ; + values.insert(X(i), cameras[i].retract(delta)); } } - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + values.insert(L(i), landmarks[i]); } // fix X0 and all landmarks, allow only the cameras[1] to move graph.addCameraConstraint(0, cameras[0]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + for (size_t i = 0; i < landmarks.size(); ++i) graph.addPoint3Constraint(i, landmarks[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -344,38 +341,40 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } // Constrain position of system with the first camera constrained to the origin graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.))); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -386,17 +385,21 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.))); + graph.push_back( + PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.))); Values init; init.insert(X(0), GeneralCamera()); - init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.))); // The optimal value between the 2m range factor and 1m prior is 1.5m Values expected; expected.insert(X(0), GeneralCamera()); - expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0))); + expected.insert(X(1), Pose3(Rot3(), Point3(1.5, 0., 0.))); LevenbergMarquardtParams params; params.absoluteErrorTol = 1e-9; @@ -410,16 +413,23 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.push_back(PriorFactor(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0))); - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back( + PriorFactor(X(0), CalibratedCamera(), + noiseModel::Isotropic::Sigma(6, 1.))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.))); + graph.push_back( + PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.))); Values init; init.insert(X(0), CalibratedCamera()); - init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.))); Values expected; - expected.insert(X(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); + expected.insert(X(0), + CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0))); LevenbergMarquardtParams params; @@ -431,5 +441,95 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +// Frank created these tests after switching to a custom LinearizedFactor +TEST(GeneralSFMFactor, BinaryJacobianFactor) { + Point2 measurement(3., -1.); + + // Create Values + Values values; + Rot3 R; + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); + values.insert(X(1), GeneralCamera(x1)); + Point3 l1; + values.insert(L(1), l1); + + vector models; + { + // Create various noise-models to test all cases + using namespace noiseModel; + Rot2 R = Rot2::fromAngle(0.3); + Matrix2 cov = R.matrix() * R.matrix().transpose(); + models += SharedNoiseModel(), Unit::Create(2), // + Isotropic::Sigma(2, 0.5), Constrained::All(2), Gaussian::Covariance(cov); + } + + // Now loop over all these noise models + BOOST_FOREACH(SharedNoiseModel model, models) { + Projection factor(measurement, model, X(1), L(1)); + + // Test linearize + GaussianFactor::shared_ptr expected = // + factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = factor.linearize(values); + EXPECT(assert_equal(*expected, *actual, 1e-9)); + + // Test methods that rely on updateHessian + if (model && !model->isConstrained()) { + // Construct HessianFactor from single JacobianFactor + HessianFactor expectedHessian(*expected), actualHessian(*actual); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-9)); + + // Convert back + JacobianFactor actualJacobian(actualHessian); + // Note we do not expect the actualJacobian to match *expected + // Just that they have the same information on the variable. + EXPECT( + assert_equal(expected->augmentedInformation(), + actualJacobian.augmentedInformation(), 1e-9)); + + // Construct from GaussianFactorGraph + GaussianFactorGraph gfg1; + gfg1 += expected; + GaussianFactorGraph gfg2; + gfg2 += actual; + HessianFactor hessian1(gfg1), hessian2(gfg2); + EXPECT(assert_equal(hessian1, hessian2, 1e-9)); + } + } +} + +/* ************************************************************************* */ +// Do a thorough test of BinaryJacobianFactor +TEST( GeneralSFMFactor, BinaryJacobianFactor2 ) { + + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); + + Values values; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); + for (size_t j = 0; j < landmarks.size(); ++j) + values.insert(L(j), landmarks[j]); + + for (size_t i = 0; i < cameras.size(); ++i) { + for (size_t j = 0; j < landmarks.size(); ++j) { + Point2 z = cameras[i].project(landmarks[j]); + Projection::shared_ptr nonlinear = // + boost::make_shared(z, sigma1, X(i), L(j)); + GaussianFactor::shared_ptr factor = nonlinear->linearize(values); + HessianFactor hessian(*factor); + JacobianFactor jacobian(hessian); + EXPECT( + assert_equal(factor->augmentedInformation(), + jacobian.augmentedInformation(), 1e-9)); + } + } +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index df56f5260..f812cd308 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -49,7 +49,8 @@ typedef NonlinearEquality Point3Constraint; /* ************************************************************************* */ class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { + void addMeasurement(const int& i, const int& j, const Point2& z, + const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, X(i), L(j))); } @@ -65,97 +66,101 @@ public: }; -static double getGaussian() -{ - double S,V1,V2; - // Use Box-Muller method to create gauss noise from uniform noise - do - { - double U1 = rand() / (double)(RAND_MAX); - double U2 = rand() / (double)(RAND_MAX); - V1 = 2 * U1 - 1; /* V1=[-1,1] */ - V2 = 2 * U2 - 1; /* V2=[-1,1] */ - S = V1 * V1 + V2 * V2; - } while(S>=1); - return sqrt(-2.0f * (double)log(S) / S) * V1; +static double getGaussian() { + double S, V1, V2; + // Use Box-Muller method to create gauss noise from uniform noise + do { + double U1 = rand() / (double) (RAND_MAX); + double U2 = rand() / (double) (RAND_MAX); + V1 = 2 * U1 - 1; /* V1=[-1,1] */ + V2 = 2 * U2 - 1; /* V2=[-1,1] */ + S = V1 * V1 + V2 * V2; + } while (S >= 1); + return sqrt(-2.f * (double) log(S) / S) * V1; } static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); /* ************************************************************************* */ -TEST( GeneralSFMFactor_Cal3Bundler, equals ) -{ +TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Point2 z(323.,240.); - const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); + Point2 z(323., 240.); + const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor1( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - boost::shared_ptr - factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor2( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ TEST( GeneralSFMFactor_Cal3Bundler, error ) { - Point2 z(3.,0.); + Point2 z(3., 0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor(new Projection(z, sigma, X(1), L(1))); + boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; - Point3 t1(0,0,-6); - Pose3 x1(R,t1); + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values))); + Point3 l1; + values.insert(L(1), l1); + EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values))); } - -static const double baseline = 5.0 ; +static const double baseline = 5.; /* ************************************************************************* */ static vector genPoint3() { const double z = 5; - vector landmarks ; - landmarks.push_back(Point3 (-1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); - landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); - return landmarks ; + vector landmarks; + landmarks.push_back(Point3(-1., -1., z)); + landmarks.push_back(Point3(-1., 1., z)); + landmarks.push_back(Point3(1., 1., z)); + landmarks.push_back(Point3(1., -1., z)); + landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-2., -2., 2 * z)); + landmarks.push_back(Point3(-2., 2., 2 * z)); + landmarks.push_back(Point3(2., 2., 2 * z)); + landmarks.push_back(Point3(2., -2., 2 * z)); + return landmarks; } static vector genCameraDefaultCalibration() { - vector cameras ; - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); - return cameras ; + vector cameras; + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)))); + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)))); + return cameras; } static vector genCameraVariableCalibration() { - const Cal3Bundler K(500,1e-3,1e-3); - vector cameras ; - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); - return cameras ; + const Cal3Bundler K(500, 1e-3, 1e-3); + vector cameras; + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K)); + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K)); + return cameras; } -static boost::shared_ptr getOrdering(const std::vector& cameras, const std::vector& landmarks) { +static boost::shared_ptr getOrdering( + const std::vector& cameras, + const std::vector& landmarks) { boost::shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; - return ordering ; + for (size_t i = 0; i < landmarks.size(); ++i) + ordering->push_back(L(i)); + for (size_t i = 0; i < cameras.size(); ++i) + ordering->push_back(X(i)); + return ordering; } /* ************************************************************************* */ @@ -166,32 +171,32 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -203,38 +208,37 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - if ( i == 0 ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; - } - else { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + if (i == 0) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); + } else { + values.insert(L(i), landmarks[i]); } } graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -247,35 +251,35 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); - values.insert(L(i), pt) ; + values.insert(L(i), pt); } - for ( size_t i = 0 ; i < cameras.size() ; ++i ) + for (size_t i = 0; i < cameras.size(); ++i) graph.addCameraConstraint(i, cameras[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -289,46 +293,43 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) { - const double - rot_noise = 1e-5, trans_noise = 1e-3, - focal_noise = 1, distort_noise = 1e-3; - if ( i == 0 ) { - values.insert(X(i), cameras[i]) ; - } - else { + for (size_t i = 0; i < cameras.size(); ++i) { + const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, + distort_noise = 1e-3; + if (i == 0) { + values.insert(X(i), cameras[i]); + } else { - Vector delta = (Vector(9) << - rot_noise, rot_noise, rot_noise, // rotation - trans_noise, trans_noise, trans_noise, // translation - focal_noise, distort_noise, distort_noise // f, k1, k2 + Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation + trans_noise, trans_noise, trans_noise, // translation + focal_noise, distort_noise, distort_noise // f, k1, k2 ).finished(); - values.insert(X(i), cameras[i].retract(delta)) ; + values.insert(X(i), cameras[i].retract(delta)); } } - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + values.insert(L(i), landmarks[i]); } // fix X0 and all landmarks, allow only the cameras[1] to move graph.addCameraConstraint(0, cameras[0]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + for (size_t i = 0; i < landmarks.size(); ++i) graph.addPoint3Constraint(i, landmarks[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -341,43 +342,48 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } // Constrain position of system with the first camera constrained to the origin graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.))); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 2d1793417..7db71d8ce 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index fa16a0f91..4a7b4c3fe 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -16,21 +16,12 @@ * @brief Tests the OrientedPlane3Factor class */ -#include -#include #include -#include -#include -#include -#include #include -#include -#include -#include -#include #include -#include +#include #include + #include #include #include @@ -43,92 +34,100 @@ using namespace std; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) -TEST (OrientedPlane3, lm_translation_error) -{ +// ************************************************************************* +TEST (OrientedPlane3Factor, lm_translation_error) { // Tests one pose, two measurements of the landmark that differ in range only. // Normal along -x, 3m away - gtsam::Symbol lm_sym ('p', 0); - gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); - - gtsam::ISAM2 isam2; - gtsam::Values new_values; - gtsam::NonlinearFactorGraph new_graph; + Symbol lm_sym('p', 0); + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + + ISAM2 isam2; + Values new_values; + NonlinearFactorGraph new_graph; // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - gtsam::Symbol init_sym ('x', 0); - gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), - gtsam::Point3 (0.0, 0.0, 0.0)); - gtsam::Vector sigmas(6); + Symbol init_sym('x', 0); + Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Vector sigmas(6); sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; - gtsam::PriorFactor pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas (sigmas) ); - new_values.insert (init_sym, init_pose); - new_graph.add (pose_prior); - + PriorFactor pose_prior(init_sym, init_pose, + noiseModel::Diagonal::Sigmas(sigmas)); + new_values.insert(init_sym, init_pose); + new_graph.add(pose_prior); + // Add two landmark measurements, differing in range - new_values.insert (lm_sym, test_lm0); - gtsam::Vector sigmas3(3); + new_values.insert(lm_sym, test_lm0); + Vector sigmas3(3); sigmas3 << 0.1, 0.1, 0.1; - gtsam::Vector test_meas0_mean(4); - test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); - new_graph.add (test_meas0); - gtsam::Vector test_meas1_mean(4); - test_meas1_mean << -1.0, 0.0, 0.0, 1.0; - gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); - new_graph.add (test_meas1); - - // Optimize - gtsam::ISAM2Result result = isam2.update (new_graph, new_values); - gtsam::Values result_values = isam2.calculateEstimate (); - gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at(lm_sym); - - // Given two noisy measurements of equal weight, expect result between the two - gtsam::OrientedPlane3 expected_plane_landmark (-1.0, 0.0, 0.0, 2.0); - EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); -} - -TEST (OrientedPlane3, lm_rotation_error) -{ - // Tests one pose, two measurements of the landmark that differ in angle only. - // Normal along -x, 3m away - gtsam::Symbol lm_sym ('p', 0); - gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); - - gtsam::ISAM2 isam2; - gtsam::Values new_values; - gtsam::NonlinearFactorGraph new_graph; - - // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - gtsam::Symbol init_sym ('x', 0); - gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), - gtsam::Point3 (0.0, 0.0, 0.0)); - gtsam::PriorFactor pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas ((Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); - new_values.insert (init_sym, init_pose); - new_graph.add (pose_prior); - -// // Add two landmark measurements, differing in angle - new_values.insert (lm_sym, test_lm0); Vector test_meas0_mean(4); test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas(Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); - new_graph.add (test_meas0); + OrientedPlane3Factor test_meas0(test_meas0_mean, + noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); + new_graph.add(test_meas0); Vector test_meas1_mean(4); - test_meas1_mean << 0.0, -1.0, 0.0, 3.0; - gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); - new_graph.add (test_meas1); - + test_meas1_mean << -1.0, 0.0, 0.0, 1.0; + OrientedPlane3Factor test_meas1(test_meas1_mean, + noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); + new_graph.add(test_meas1); + // Optimize - gtsam::ISAM2Result result = isam2.update (new_graph, new_values); - gtsam::Values result_values = isam2.calculateEstimate (); - gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at(lm_sym); + ISAM2Result result = isam2.update(new_graph, new_values); + Values result_values = isam2.calculateEstimate(); + OrientedPlane3 optimized_plane_landmark = result_values.at( + lm_sym); // Given two noisy measurements of equal weight, expect result between the two - gtsam::OrientedPlane3 expected_plane_landmark (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3.0); - EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); + OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } // ************************************************************************* -TEST( OrientedPlane3DirectionPriorFactor, Constructor ) { +TEST (OrientedPlane3Factor, lm_rotation_error) { + // Tests one pose, two measurements of the landmark that differ in angle only. + // Normal along -x, 3m away + Symbol lm_sym('p', 0); + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + + ISAM2 isam2; + Values new_values; + NonlinearFactorGraph new_graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + Symbol init_sym('x', 0); + Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + PriorFactor pose_prior(init_sym, init_pose, + noiseModel::Diagonal::Sigmas( + (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); + new_values.insert(init_sym, init_pose); + new_graph.add(pose_prior); + +// // Add two landmark measurements, differing in angle + new_values.insert(lm_sym, test_lm0); + Vector test_meas0_mean(4); + test_meas0_mean << -1.0, 0.0, 0.0, 3.0; + OrientedPlane3Factor test_meas0(test_meas0_mean, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add(test_meas0); + Vector test_meas1_mean(4); + test_meas1_mean << 0.0, -1.0, 0.0, 3.0; + OrientedPlane3Factor test_meas1(test_meas1_mean, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add(test_meas1); + + // Optimize + ISAM2Result result = isam2.update(new_graph, new_values); + Values result_values = isam2.calculateEstimate(); + OrientedPlane3 optimized_plane_landmark = result_values.at( + lm_sym); + + // Given two noisy measurements of equal weight, expect result between the two + OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, + 0.0, 3.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); +} + +// ************************************************************************* +TEST( OrientedPlane3DirectionPrior, Constructor ) { // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, @@ -138,29 +137,30 @@ TEST( OrientedPlane3DirectionPriorFactor, Constructor ) { // Factor Key key(1); - SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas (Vector3(0.1, 0.1, 10.0)); + SharedGaussian model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 10.0)); OrientedPlane3DirectionPrior factor(key, planeOrientation, model); // Create a linearization point at the zero-error point Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0); - Vector theta2 = Vector4(0.0, 0.1, - 0.8, 10.0); - Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); - + Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0); + Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); OrientedPlane3 T1(theta1); OrientedPlane3 T2(theta2); OrientedPlane3 T3(theta3); - // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T1); + Matrix expectedH1 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T1); - Matrix expectedH2 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T2); - Matrix expectedH3 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T3); + Matrix expectedH3 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T3); // Use the factor to calculate the derivative Matrix actualH1, actualH2, actualH3; diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp deleted file mode 100644 index 8dfb753f4..000000000 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testRegularHessianFactor.cpp - * @author Frank Dellaert - * @date March 4, 2014 - */ - -#include -#include - -//#include -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost::assign; - -const double tol = 1e-5; - -/* ************************************************************************* */ -TEST(RegularHessianFactor, ConstructorNWay) -{ - Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished(); - Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished(); - Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished(); - - Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished(); - Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished(); - - Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished(); - - Vector g1 = (Vector(2) << -7, -9).finished(); - Vector g2 = (Vector(2) << -9, 1).finished(); - Vector g3 = (Vector(2) << 2, 3).finished(); - - double f = 10; - - std::vector js; - js.push_back(0); js.push_back(1); js.push_back(3); - std::vector Gs; - Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33); - std::vector gs; - gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); - RegularHessianFactor<2> factor(js, Gs, gs, f); - - // multiplyHessianAdd: - { - // brute force - Matrix AtA = factor.information(); - HessianFactor::const_iterator i1 = factor.begin(); - HessianFactor::const_iterator i2 = i1 + 1; - Vector X(6); X << 1,2,3,4,5,6; - Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696; - EXPECT(assert_equal(Y,AtA*X)); - - VectorValues x = map_list_of - (0, (Vector(2) << 1,2).finished()) - (1, (Vector(2) << 3,4).finished()) - (3, (Vector(2) << 5,6).finished()); - - VectorValues expected; - expected.insert(0, Y.segment<2>(0)); - expected.insert(1, Y.segment<2>(2)); - expected.insert(3, Y.segment<2>(4)); - - // RAW ACCESS - Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; - Vector fast_y = gtsam::zero(8); - double xvalues[8] = {1,2,3,4,0,0,5,6}; - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); - EXPECT(assert_equal(expected_y, fast_y)); - - // now, do it with non-zero y - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); - EXPECT(assert_equal(2*expected_y, fast_y)); - - // check some expressions - EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal())); - EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView())); - EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal())); - } -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 29eaf2ac1..77944da83 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -1,21 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * 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 testImplicitSchurFactor.cpp + * @file testRegularImplicitSchurFactor.cpp * @brief unit test implicit jacobian factors * @author Frank Dellaert * @date Oct 20, 2013 */ -//#include -#include -//#include #include -//#include "gtsam_unstable/slam/JacobianFactorQR.h" -#include "gtsam/slam/JacobianFactorQR.h" +#include +#include +#include +#include -#include #include #include #include +#include #include #include @@ -31,8 +41,8 @@ using namespace gtsam; const Matrix26 F0 = Matrix26::Ones(); const Matrix26 F1 = 2 * Matrix26::Ones(); const Matrix26 F3 = 3 * Matrix26::Ones(); -const vector > Fblocks = list_of > // - (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3)); +const vector FBlocks = list_of(F0)(F1)(F3); +const FastVector keys = list_of(0)(1)(3); // RHS and sigmas const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); @@ -43,7 +53,7 @@ TEST( regularImplicitSchurFactor, creation ) { E.block<2,2>(0, 0) = eye(2); E.block<2,3>(2, 0) = 2 * ones(2, 3); Matrix3 P = (E.transpose() * E).inverse(); - RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + RegularImplicitSchurFactor expected(keys, FBlocks, E, P, b); Matrix expectedP = expected.getPointCovariance(); EXPECT(assert_equal(expectedP, P)); } @@ -76,15 +86,15 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; // Calculate expected result F'*alpha*(I - E*P*E')*F*x - FastVector keys; - keys += 0,1,2,3; - Vector x = xvalues.vector(keys); + FastVector keys2; + keys2 += 0,1,2,3; + Vector x = xvalues.vector(keys2); Vector expected = zero(24); - RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); - EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8)); + RegularImplicitSchurFactor::multiplyHessianAdd(F, E, P, alpha, x, expected); + EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8)); // Create ImplicitSchurFactor - RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); + RegularImplicitSchurFactor implicitFactor(keys, FBlocks, E, P, b); VectorValues zero = 0 * yExpected;// quick way to get zero w right structure { // First Version @@ -114,32 +124,34 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { // Create JacobianFactor with same error const SharedDiagonal model; - JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model); + JacobianFactorQ<6, 2> jfQ(keys, FBlocks, E, P, b, model); - { // error - double expectedError = jf.error(xvalues); - double actualError = implicitFactor.errorJF(xvalues); - DOUBLES_EQUAL(expectedError,actualError,1e-7) + // error + double expectedError = 11875.083333333334; + { + EXPECT_DOUBLES_EQUAL(expectedError,jfQ.error(xvalues),1e-7) + EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.errorJF(xvalues),1e-7) + EXPECT_DOUBLES_EQUAL(11903.500000000007,implicitFactor.error(xvalues),1e-7) } - { // JacobianFactor with same error + { VectorValues yActual = zero; - jf.multiplyHessianAdd(alpha, xvalues, yActual); + jfQ.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(yExpected, yActual, 1e-8)); - jf.multiplyHessianAdd(alpha, xvalues, yActual); + jfQ.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); - jf.multiplyHessianAdd(-1, xvalues, yActual); + jfQ.multiplyHessianAdd(-1, xvalues, yActual); EXPECT(assert_equal(zero, yActual, 1e-8)); } { // check hessian Diagonal - VectorValues diagExpected = jf.hessianDiagonal(); + VectorValues diagExpected = jfQ.hessianDiagonal(); VectorValues diagActual = implicitFactor.hessianDiagonal(); EXPECT(assert_equal(diagExpected, diagActual, 1e-8)); } { // check hessian Block Diagonal - map BD = jf.hessianBlockDiagonal(); + map BD = jfQ.hessianBlockDiagonal(); map actualBD = implicitFactor.hessianBlockDiagonal(); LONGS_EQUAL(3,actualBD.size()); EXPECT(assert_equal(BD[0],actualBD[0])); @@ -149,40 +161,46 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { { // Raw memory Version std::fill(y, y + 24, 0);// zero y ! - jf.multiplyHessianAdd(alpha, xdata, y); + jfQ.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(expected, XMap(y), 1e-8)); - jf.multiplyHessianAdd(alpha, xdata, y); + jfQ.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); - jf.multiplyHessianAdd(-1, xdata, y); + jfQ.multiplyHessianAdd(-1, xdata, y); EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); } + VectorValues expectedVV; + expectedVV.insert(0,-3.5*ones(6)); + expectedVV.insert(1,10*ones(6)/3); + expectedVV.insert(3,-19.5*ones(6)); { // Check gradientAtZero - VectorValues expected = jf.gradientAtZero(); VectorValues actual = implicitFactor.gradientAtZero(); - EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8)); + EXPECT(assert_equal(expectedVV, implicitFactor.gradientAtZero(), 1e-8)); } // Create JacobianFactorQR - JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model); + JacobianFactorQR<6, 2> jfQR(keys, FBlocks, E, P, b, model); + EXPECT_DOUBLES_EQUAL(expectedError, jfQR.error(xvalues),1e-7) + EXPECT(assert_equal(expectedVV, jfQR.gradientAtZero(), 1e-8)); { const SharedDiagonal model; VectorValues yActual = zero; - jfq.multiplyHessianAdd(alpha, xvalues, yActual); + jfQR.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(yExpected, yActual, 1e-8)); - jfq.multiplyHessianAdd(alpha, xvalues, yActual); + jfQR.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); - jfq.multiplyHessianAdd(-1, xvalues, yActual); + jfQR.multiplyHessianAdd(-1, xvalues, yActual); EXPECT(assert_equal(zero, yActual, 1e-8)); } { // Raw memory Version std::fill(y, y + 24, 0);// zero y ! - jfq.multiplyHessianAdd(alpha, xdata, y); + jfQR.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(expected, XMap(y), 1e-8)); - jfq.multiplyHessianAdd(alpha, xdata, y); + jfQR.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); - jfq.multiplyHessianAdd(-1, xdata, y); + jfQR.multiplyHessianAdd(-1, xdata, y); EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); } delete [] y; @@ -206,7 +224,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) E.block<2,3>(2, 0) << 1,2,3,4,5,6; E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; Matrix3 P = (E.transpose() * E).inverse(); - RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b); + RegularImplicitSchurFactor factor(keys, FBlocks, E, P, b); // hessianDiagonal VectorValues expected; @@ -247,6 +265,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0])); EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1])); EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); + + // augmentedInformation (test just checks diagonals) + Matrix actualInfo = factor.augmentedInformation(); + EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12))); + + // information (test just checks diagonals) + Matrix actualInfo2 = factor.information(); + EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12))); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp new file mode 100644 index 000000000..373d482fe --- /dev/null +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartFactorBase.cpp + * @brief Unit tests for testSmartFactorBase Class + * @author Frank Dellaert + * @date Feb 2015 + */ + +#include "../SmartFactorBase.h" +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +#include +class PinholeFactor: public SmartFactorBase > { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Pinhole) { + PinholeFactor f; + f.add(Point2(), 1, SharedNoiseModel()); + f.add(Point2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 2, f.dim()); +} + +/* ************************************************************************* */ +#include +class StereoFactor: public SmartFactorBase { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Stereo) { + StereoFactor f; + f.add(StereoPoint2(), 1, SharedNoiseModel()); + f.add(StereoPoint2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 3, f.dim()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp new file mode 100644 index 000000000..533e16bec --- /dev/null +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -0,0 +1,856 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartProjectionCameraFactor.cpp + * @brief Unit tests for SmartProjectionCameraFactor Class + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + * @date Sept 2013 + */ + +#include "smartFactorScenarios.h" +#include +#include +#include +#include +#include + +using namespace boost::assign; + +static bool isDebugTest = false; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +static Key x1(1); +Symbol l1('l', 1), l2('l', 2), l3('l', 3); +Key c1 = 1, c2 = 2, c3 = 3; + +static Point2 measurement1(323.0, 240.0); + +static double rankTol = 1.0; + +template +PinholeCamera perturbCameraPoseAndCalibration( + const PinholeCamera& camera) { + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 cameraPose = camera.pose(); + Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); + typename gtsam::traits::TangentVector d; + d.setRandom(); + d *= 0.1; + CALIBRATION perturbedCalibration = camera.calibration().retract(d); + return PinholeCamera(perturbedCameraPose, perturbedCalibration); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, perturbCameraPose) { + using namespace vanilla; + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 perturbed_level_pose = level_pose.compose(noise_pose); + Camera actualCamera(perturbed_level_pose, K2); + + Camera expectedCamera = perturbCameraPose(level_camera); + CHECK(assert_equal(expectedCamera, actualCamera)); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor) { + using namespace vanilla; + SmartFactor::shared_ptr factor1(new SmartFactor()); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor2) { + using namespace vanilla; + params.setRankTolerance(rankTol); + SmartFactor factor1(boost::none, params); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor3) { + using namespace vanilla; + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, x1, unit2); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor4) { + using namespace vanilla; + params.setRankTolerance(rankTol); + SmartFactor factor1(boost::none, params); + factor1.add(measurement1, x1, unit2); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Equals ) { + using namespace vanilla; + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, x1, unit2); + + SmartFactor::shared_ptr factor2(new SmartFactor()); + factor2->add(measurement1, x1, unit2); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, noiseless ) { + using namespace vanilla; + + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); + CHECK( + assert_equal(zero(4), + factor1->reprojectionErrorAfterTriangulation(values), 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, noisy ) { + + using namespace vanilla; + + // Project one landmark into two cameras and add noise on first + Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2); + Point2 level_uv_right = level_camera_right.project(landmark1); + + Values values; + values.insert(c1, level_camera); + Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right); + values.insert(c2, perturbed_level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + + // Point is now uninitialized before a triangulation event + EXPECT(!factor1->point()); + + double expectedError = 58640; + double actualError1 = factor1->error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1); + + // Check triangulated point + CHECK(factor1->point()); + EXPECT( + assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4)); + + // Check whitened errors + Vector expected(4); + expected << -7, 235, 58, -242; + SmartFactor::Cameras cameras1 = factor1->cameras(values); + Point3 point1 = *factor1->point(); + Vector actual = factor1->whitenedError(cameras1, point1); + EXPECT(assert_equal(expected, actual, 1)); + + SmartFactor::shared_ptr factor2(new SmartFactor()); + vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + vector noises; + noises.push_back(unit2); + noises.push_back(unit2); + + vector views; + views.push_back(c1); + views.push_back(c2); + + factor2->add(measurements, views, noises); + + double actualError2 = factor2->error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { + + using namespace vanilla; + + // Project three landmarks into three cameras + vector measurements_cam1, measurements_cam2, measurements_cam3; + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create and fill smartfactors + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + smartFactor1->add(measurements_cam1, views, unit2); + smartFactor2->add(measurements_cam2, views, unit2); + smartFactor3->add(measurements_cam3, views, unit2); + + // Create factor graph and add priors on two cameras + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + // Create initial estimate + Values initial; + initial.insert(c1, cam1); + initial.insert(c2, cam2); + initial.insert(c3, perturbCameraPose(cam3)); + if (isDebugTest) + initial.at(c3).print("Smart: Pose3 before optimization: "); + + // Points are now uninitialized before a triangulation event + EXPECT(!smartFactor1->point()); + EXPECT(!smartFactor2->point()); + EXPECT(!smartFactor3->point()); + + EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1); + EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1); + EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1); + + // Error should trigger this and initialize the points, abort if not so + CHECK(smartFactor1->point()); + CHECK(smartFactor2->point()); + CHECK(smartFactor3->point()); + + EXPECT( + assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(), + 1e-4)); + EXPECT( + assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(), + 1e-4)); + EXPECT( + assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(), + 1e-4)); + + // Check whitened errors + Vector expected(6); + expected << 256, 29, -26, 29, -206, -202; + Point3 point1 = *smartFactor1->point(); + SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial); + Vector reprojectionError = cameras1.reprojectionError(point1, + measurements_cam1); + EXPECT(assert_equal(expected, reprojectionError, 1)); + Vector actual = smartFactor1->whitenedError(cameras1, point1); + EXPECT(assert_equal(expected, actual, 1)); + + // Optimize + LevenbergMarquardtParams lmParams; + if (isDebugTest) { + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + } + LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams); + Values result = optimizer.optimize(); + + EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-7)); + EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-7)); + EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-7)); + + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial); + // VectorValues delta = GFG->optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { + + using namespace vanilla; + + // Project three landmarks into three cameras + vector measurements_cam1, measurements_cam2, measurements_cam3; + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SfM_Track track1; + for (size_t i = 0; i < 3; ++i) { + SfM_Measurement measures; + measures.first = i + 1; // cameras are from 1 to 3 + measures.second = measurements_cam1.at(i); + track1.measurements.push_back(measures); + } + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(track1, unit2); + + SfM_Track track2; + for (size_t i = 0; i < 3; ++i) { + SfM_Measurement measures; + measures.first = i + 1; // cameras are from 1 to 3 + measures.second = measurements_cam2.at(i); + track2.measurements.push_back(measures); + } + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(track2, unit2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, unit2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + values.insert(c3, perturbCameraPose(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); + // VectorValues delta = GFG->optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { + + using namespace vanilla; + + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4, measurements_cam5; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, unit2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, unit2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, unit2); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor()); + smartFactor4->add(measurements_cam4, views, unit2); + + SmartFactor::shared_ptr smartFactor5(new SmartFactor()); + smartFactor5->add(measurements_cam5, views, unit2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.push_back(smartFactor5); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + values.insert(c3, perturbCameraPoseAndCalibration(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); + // VectorValues delta = GFG->optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 20)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, Cal3Bundler ) { + + using namespace bundler; + + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4, measurements_cam5; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, unit2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, unit2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, unit2); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor()); + smartFactor4->add(measurements_cam4, views, unit2); + + SmartFactor::shared_ptr smartFactor5(new SmartFactor()); + smartFactor5->add(measurements_cam5, views, unit2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(c3, perturbCameraPose(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1), 1e-4)); + EXPECT(assert_equal(cam2, result.at(c2), 1e-4)); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 1)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { + + using namespace bundler; + + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4, measurements_cam5; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, unit2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, unit2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, unit2); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor()); + smartFactor4->add(measurements_cam4, views, unit2); + + SmartFactor::shared_ptr smartFactor5(new SmartFactor()); + smartFactor5->add(measurements_cam5, views, unit2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(c3, perturbCameraPoseAndCalibration(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1), 1e-4)); + EXPECT(assert_equal(cam2, result.at(c2), 1e-4)); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, noiselessBundler ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + + double actualError = factor1->error(values); + + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-3); + + Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this) + if (factor1->point()) + oldPoint = *(factor1->point()); + + Point3 expectedPoint; + if (factor1->point(values)) + expectedPoint = *(factor1->point(values)); + + EXPECT(assert_equal(expectedPoint, landmark1, 1e-3)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + NonlinearFactorGraph smartGraph; + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + smartGraph.push_back(factor1); + double expectedError = factor1->error(values); + double expectedErrorGraph = smartGraph.error(values); + Point3 expectedPoint; + if (factor1->point()) + expectedPoint = *(factor1->point()); + // cout << "expectedPoint " << expectedPoint.vector() << endl; + + // COMMENTS: + // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as + // value in the generalGrap + NonlinearFactorGraph generalGraph; + SFMFactor sfm1(level_uv, unit2, c1, l1); + SFMFactor sfm2(level_uv_right, unit2, c2, l1); + generalGraph.push_back(sfm1); + generalGraph.push_back(sfm2); + values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation + Vector e1 = sfm1.evaluateError(values.at(c1), values.at(l1)); + Vector e2 = sfm2.evaluateError(values.at(c2), values.at(l1)); + double actualError = 0.5 + * (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2)); + double actualErrorGraph = generalGraph.error(values); + + DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7); + DOUBLES_EQUAL(expectedErrorGraph, expectedError, 1e-7); + DOUBLES_EQUAL(actualErrorGraph, actualError, 1e-7); + DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + NonlinearFactorGraph smartGraph; + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + smartGraph.push_back(factor1); + Matrix expectedHessian = smartGraph.linearize(values)->hessian().first; + Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second; + Point3 expectedPoint; + if (factor1->point()) + expectedPoint = *(factor1->point()); + + // COMMENTS: + // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as + // value in the generalGrap + NonlinearFactorGraph generalGraph; + SFMFactor sfm1(level_uv, unit2, c1, l1); + SFMFactor sfm2(level_uv_right, unit2, c2, l1); + generalGraph.push_back(sfm1); + generalGraph.push_back(sfm2); + values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation + Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first; + Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second; + Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18) + - actualFullHessian.block(0, 18, 18, 3) + * (actualFullHessian.block(18, 18, 3, 3)).inverse() + * actualFullHessian.block(18, 0, 3, 18); + Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1) + - actualFullHessian.block(0, 18, 18, 3) + * (actualFullHessian.block(18, 18, 3, 3)).inverse() + * actualFullInfoVector.block(18, 0, 3, 1); + + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7)); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7)); +} + +/* *************************************************************************/ +// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors +//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){ +// +// Values values; +// values.insert(c1, level_camera); +// values.insert(c2, level_camera_right); +// +// NonlinearFactorGraph smartGraph; +// SmartFactor::shared_ptr factor1(new SmartFactor()); +// factor1->add(level_uv, c1, unit2); +// factor1->add(level_uv_right, c2, unit2); +// smartGraph.push_back(factor1); +// GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values); +// +// Point3 expectedPoint; +// if(factor1->point()) +// expectedPoint = *(factor1->point()); +// +// // COMMENTS: +// // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as +// // value in the generalGrap +// NonlinearFactorGraph generalGraph; +// SFMFactor sfm1(level_uv, unit2, c1, l1); +// SFMFactor sfm2(level_uv_right, unit2, c2, l1); +// generalGraph.push_back(sfm1); +// generalGraph.push_back(sfm2); +// values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation +// GaussianFactorGraph::shared_ptr gfg = generalGraph.linearize(values); +// +// double alpha = 1.0; +// +// VectorValues yExpected, yActual, ytmp; +// VectorValues xtmp = map_list_of +// (c1, (Vec(9) << 0,0,0,0,0,0,0,0,0)) +// (c2, (Vec(9) << 0,0,0,0,0,0,0,0,0)) +// (l1, (Vec(3) << 5.5, 0.5, 1.2)); +// gfg ->multiplyHessianAdd(alpha, xtmp, ytmp); +// +// VectorValues x = map_list_of +// (c1, (Vec(9) << 1,2,3,4,5,6,7,8,9)) +// (c2, (Vec(9) << 11,12,13,14,15,16,17,18,19)) +// (l1, (Vec(3) << 5.5, 0.5, 1.2)); +// +// gfgSmart->multiplyHessianAdd(alpha, ytmp + x, yActual); +// gfg ->multiplyHessianAdd(alpha, x, yExpected); +// +// EXPECT(assert_equal(yActual,yExpected, 1e-7)); +//} +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + Matrix expectedE; + Vector expectedb; + + CameraSet cameras; + cameras.push_back(level_camera); + cameras.push_back(level_camera_right); + + factor1->error(values); // this is important to have a triangulation of the point + Point3 point; + if (factor1->point()) + point = *(factor1->point()); + vector Fblocks; + factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point); + + NonlinearFactorGraph generalGraph; + SFMFactor sfm1(level_uv, unit2, c1, l1); + SFMFactor sfm2(level_uv_right, unit2, c2, l1); + generalGraph.push_back(sfm1); + generalGraph.push_back(sfm2); + values.insert(l1, point); // note: we get rid of possible errors in the triangulation + Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first; + Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse(); + + Matrix3 expectedVinv = factor1->PointCov(expectedE); + EXPECT(assert_equal(expectedVinv, actualVinv, 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { + + using namespace bundler; + + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + double rankTol = 1; + bool useEPI = false; + + // Hessian version + SmartProjectionParams params; + params.setRankTolerance(rankTol); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(useEPI); + + SmartFactor::shared_ptr explicitFactor( + new SmartFactor(boost::none, params)); + explicitFactor->add(level_uv, c1, unit2); + explicitFactor->add(level_uv_right, c2, unit2); + + GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize( + values); + HessianFactor& hessianFactor = + dynamic_cast(*gaussianHessianFactor); + + // Implicit Schur version + params.setLinearizationMode(gtsam::IMPLICIT_SCHUR); + SmartFactor::shared_ptr implicitFactor( + new SmartFactor(boost::none, params)); + implicitFactor->add(level_uv, c1, unit2); + implicitFactor->add(level_uv_right, c2, unit2); + GaussianFactor::shared_ptr gaussianImplicitSchurFactor = + implicitFactor->linearize(values); + CHECK(gaussianImplicitSchurFactor); + typedef RegularImplicitSchurFactor Implicit9; + Implicit9& implicitSchurFactor = + dynamic_cast(*gaussianImplicitSchurFactor); + + VectorValues x = map_list_of(c1, + (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished())(c2, + (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished()); + + VectorValues yExpected, yActual; + double alpha = 1.0; + hessianFactor.multiplyHessianAdd(alpha, x, yActual); + implicitSchurFactor.multiplyHessianAdd(alpha, x, yExpected); + EXPECT(assert_equal(yActual, yExpected, 1e-7)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c2855f09b..72147ff35 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -10,290 +10,193 @@ * -------------------------------------------------------------------------- */ /** - * @file TestSmartProjectionPoseFactor.cpp + * @file testSmartProjectionPoseFactor.cpp * @brief Unit tests for ProjectionFactor Class * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira + * @author Frank Dellaert * @date Sept 2013 */ -#include "../SmartProjectionPoseFactor.h" - -#include -#include +#include "smartFactorScenarios.h" #include -#include +#include +#include +#include #include +#include #include -using namespace std; using namespace boost::assign; -using namespace gtsam; -static bool isDebugTest = false; - -// make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w=640,h=480; - -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); -static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - -static double rankTol = 1.0; -static double linThreshold = -1.0; -static bool manageDegeneracy = true; +static const double rankTol = 1.0; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(2)); +static const double sigma = 0.1; +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; // tests data -static Symbol x1('X', 1); -static Symbol x2('X', 2); -static Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); -static Key poseKey1(x1); static Point2 measurement1(323.0, 240.0); -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionPoseFactor SmartFactorBundler; - -void projectToMultipleCameras( - SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, - vector& measurements_cam){ - - Point2 cam1_uv1 = cam1.project(landmark); - Point2 cam2_uv1 = cam2.project(landmark); - Point2 cam3_uv1 = cam3.project(landmark); - measurements_cam.push_back(cam1_uv1); - measurements_cam.push_back(cam2_uv1); - measurements_cam.push_back(cam3_uv1); -} +LevenbergMarquardtParams lmParams; +// Make more verbose like so (in tests): +// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { - SmartFactor::shared_ptr factor1(new SmartFactor()); + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor2) { - SmartFactor factor1(rankTol, linThreshold); + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(sharedK, boost::none, params); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor3) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); + factor1->add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor4) { - SmartFactor factor1(rankTol, linThreshold); - factor1.add(measurement1, poseKey1, model, K); -} - -/* ************************************************************************* */ -TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { - bool manageDegeneracy = true; - bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); - factor1.add(measurement1, poseKey1, model, K); + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(sharedK, boost::none, params); + factor1.add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); + factor1->add(measurement1, x1, model); - SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, poseKey1, model, K); + SmartFactor::shared_ptr factor2(new SmartFactor(sharedK)); + factor2->add(measurement1, x1, model); CHECK(assert_equal(*factor1, *factor2)); } /* *************************************************************************/ -TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ - // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; +TEST( SmartProjectionPoseFactor, noiseless ) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera level_camera(level_pose, *K2); + using namespace vanillaPose; - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera level_camera_right(level_pose_right, *K2); + // Project two landmarks into two cameras + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); + SmartFactor factor(sharedK); + factor.add(level_uv, x1, model); + factor.add(level_uv_right, x2, model); - // 1. Project two landmarks into two cameras and triangulate - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); - Values values; - values.insert(x1, level_pose); - values.insert(x2, level_pose_right); - - SmartFactor factor1; - factor1.add(level_uv, x1, model, K); - factor1.add(level_uv_right, x2, model, K); - - double actualError = factor1.error(values); + double actualError = factor.error(values); double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactor::Cameras cameras = factor1.cameras(values); - double actualError2 = factor1.totalReprojectionError(cameras); + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); - // test vector of errors - //Vector actual = factor1.unwhitenedError(values); - //EXPECT(assert_equal(zero(4),actual,1e-8)); + // Calculate expected derivative for point (easiest to check) + boost::function f = // + boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); + + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point + boost::optional point = factor.point(); + CHECK(point); + + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using reprojectionError + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(zero(4), actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + vector Fblocks; + factor.computeJacobians(Fblocks, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, noisy ){ - // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; +TEST( SmartProjectionPoseFactor, noisy ) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera level_camera(level_pose, *K2); + using namespace vanillaPose; - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera level_camera_right(level_pose_right, *K2); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); - - // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2,0.2); - Point2 level_uv = level_camera.project(landmark) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark); + // Project two landmarks into two cameras + Point2 pixelError(0.2, 0.2); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); Values values; - values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - values.insert(x2, level_pose_right.compose(noise_pose)); + values.insert(x1, cam1.pose()); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + values.insert(x2, pose_right.compose(noise_pose)); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, x1, model, K); - factor1->add(level_uv_right, x2, model, K); + SmartFactor::shared_ptr factor(new SmartFactor((sharedK))); + factor->add(level_uv, x1, model); + factor->add(level_uv_right, x2, model); - double actualError1= factor1->error(values); + double actualError1 = factor->error(values); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartFactor::shared_ptr factor2(new SmartFactor((sharedK))); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) - Ks.push_back(K); - Ks.push_back(K); - - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); - factor2->add(measurements, views, noises, Ks); - - double actualError2= factor2->error(values); - + factor2->add(measurements, views, noises); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K2); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, K2); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, K2); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - -// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); -// VectorValues delta = GFG->optimize(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3),1e-8)); - if(isDebugTest) tictoc_print_(); -} - /* *************************************************************************/ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ + // make a realistic calibration matrix + double fov = 60; // degrees + size_t w=640,h=480; + + Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses @@ -330,19 +233,19 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ views.push_back(x2); views.push_back(x3); - double rankTol = 1; - double linThreshold = -1; - bool manageDegeneracy = false; - bool enableEPI = false; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartProjectionPoseFactor smartFactor1(K, sensor_to_body, params); + smartFactor1.add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartProjectionPoseFactor smartFactor2(K, sensor_to_body, params); + smartFactor2.add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartProjectionPoseFactor smartFactor3(K, sensor_to_body, params); + smartFactor3.add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -370,58 +273,37 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, bodyPose3*noise_pose); - LevenbergMarquardtParams params; + LevenbergMarquardtParams lmParams; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(bodyPose3,result.at(x3))); } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { + using namespace vanillaPose2; vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K); + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2)); + smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK2)); + smartFactor2->add(measurements_cam2, views, model); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK2)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -429,73 +311,227 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values groundTruth; + groundTruth.insert(x1, cam1.pose()); + groundTruth.insert(x2, cam2.pose()); + groundTruth.insert(x3, cam3.pose()); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); - if(isDebugTest) tictoc_print_(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianSVD ){ +TEST( SmartProjectionPoseFactor, Factors ) { - std::vector views; + using namespace vanillaPose; + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( + Pose3(R, Point3(1, 0, 0)), sharedK); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + vector views; + views.push_back(x1); + views.push_back(x2); + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(sharedK); + smartFactor1->add(measurements_cam1, views, model); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + CHECK(smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + { + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual, 1e-8)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); + } + + { + Matrix26 F1; + F1.setZero(); + F1(0, 1) = -100; + F1(0, 3) = -10; + F1(1, 0) = 100; + F1(1, 4) = -10; + Matrix26 F2; + F2.setZero(); + F2(0, 1) = -101; + F2(0, 3) = -10; + F2(0, 5) = -1; + F2(1, 0) = 100; + F2(1, 2) = 10; + F2(1, 4) = -10; + Matrix E(4, 3); + E.setZero(); + E(0, 0) = 10; + E(1, 1) = 10; + E(2, 0) = 10; + E(2, 2) = 1; + E(3, 1) = 10; + vector Fblocks = list_of(F1)(F2); + Vector b(4); + b.setZero(); + + // Create smart factors + FastVector keys; + keys.push_back(x1); + keys.push_back(x2); + + // createJacobianQFactor + SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); + Matrix3 P = (E.transpose() * E).inverse(); + JacobianFactorQ<6, 2> expectedQ(keys, Fblocks, E, P, b, n); + EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8)); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actualQ); + EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8)); + EXPECT(assert_equal(expectedQ, *actualQ)); + EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-8); + + // Whiten for RegularImplicitSchurFactor (does not have noise model) + model->WhitenSystem(E, b); + Matrix3 whiteP = (E.transpose() * E).inverse(); + Fblocks[0] = model->Whiten(Fblocks[0]); + Fblocks[1] = model->Whiten(Fblocks[1]); + + // createRegularImplicitSchurFactor + RegularImplicitSchurFactor expected(keys, Fblocks, E, whiteP, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); + } + + { + // createJacobianSVDFactor + Vector1 b; + b.setZero(); + double s = sigma * sin(M_PI_4); + SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); + JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); + + boost::shared_ptr actual = + smartFactor1->createJacobianSVDFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); + } +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { + + using namespace vanillaPose; + + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK)); + smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK)); + smartFactor2->add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -503,63 +539,127 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), Point3(0.1, -0.1, 1.9)), + values.at(x3))); - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, landmarkDistance ){ +TEST( SmartProjectionPoseFactor, jacobianSVD ) { + + using namespace vanillaPose; + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); + SmartFactor factor1(sharedK, boost::none, params); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, landmarkDistance ) { + + using namespace vanillaPose; double excludeLandmarksFutherThanDist = 2; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -567,75 +667,70 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { + + using namespace vanillaPose; double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + // add fourth landmark Point3 landmark4(5, -0.5, 1); - vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; - // 1. Project three landmarks into three cameras and triangulate + // Project 4 landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor4->add(measurements_cam4, views, model, K); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); + + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(sharedK, boost::none, params)); + smartFactor4->add(measurements_cam4, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -644,61 +739,52 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(cam3.pose(), result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianQ ){ +TEST( SmartProjectionPoseFactor, jacobianQ ) { - std::vector views; + using namespace vanillaPose; + + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_Q); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -706,565 +792,485 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ - // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { - std::vector views; + using namespace vanillaPose2; + + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - typedef GenericProjectionFactor ProjectionFactor; NonlinearFactorGraph graph; - // 1. Project three landmarks into three cameras and triangulate - graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); + // Project three landmarks into three cameras + graph.push_back( + ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2)); - graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2)); - graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, level_pose, noisePrior)); + graph.push_back(PriorFactor(x2, pose_right, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3* noise_pose); + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above * noise_pose); values.insert(L(1), landmark1); values.insert(L(2), landmark2); values.insert(L(3), landmark3); - if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + DOUBLES_EQUAL(48406055, graph.error(values), 1); + + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); Values result = optimizer.optimize(); - if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); + DOUBLES_EQUAL(0, graph.error(result), 1e-9); + + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, CheckHessian){ +TEST( SmartProjectionPoseFactor, CheckHessian) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); + using namespace vanillaPose; - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + // Two slightly different cameras + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - double rankTol = 10; + SmartProjectionParams params; + params.setRankTolerance(10); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views, model); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + boost::shared_ptr factor1 = smartFactor1->linearize(values); + boost::shared_ptr factor2 = smartFactor2->linearize(values); + boost::shared_ptr factor3 = smartFactor3->linearize(values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = factor1->information() + factor2->information() + + factor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize(values); + boost::shared_ptr GaussianGraph = graph.linearize( + values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = factor1->augmentedInformation() + + factor2->augmentedInformation() + factor3->augmentedInformation(); // Check Information vector - // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; +TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { + using namespace vanillaPose2; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); + // Two different cameras, at the same position, but different rotations + Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); + Camera cam2(pose2, sharedK2); + Camera cam3(pose3, sharedK2); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam2(pose2, *K2); + vector measurements_cam1, measurements_cam2; - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - double rankTol = 50; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, K2); + SmartProjectionParams params; + params.setRankTolerance(50); + params.setDegeneracyMode(gtsam::HANDLE_INFINITY); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K2); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedK2, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedK2, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2*noise_pose); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x1, cam1.pose()); + values.insert(x2, pose2 * noise_pose); + values.insert(x3, pose3 * noise_pose); - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; - // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; +TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { - std::vector views; + using namespace vanillaPose; + + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + // Two different cameras, at the same position, but different rotations + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - double rankTol = 10; + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; - // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) + // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior + EXPECT(assert_equal(Pose3(values.at(x3).rotation(), + Point3(0,0,1)), result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Hessian ){ - // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl; +TEST( SmartProjectionPoseFactor, Hessian ) { - std::vector views; + using namespace vanillaPose2; + + vector views; views.push_back(x1); views.push_back(x2); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1); vector measurements_cam1; measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1,views, model, K2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2)); + smartFactor1->add(measurements_cam1, views, model); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); - boost::shared_ptr hessianFactor = smartFactor1->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr factor = smartFactor1->linearize(values); // compute triangulation from linearization point // compute reprojection errors (sum squared) - // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) + // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotation ){ +TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; - std::vector views; + using namespace vanillaPose; + + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1, measurements_cam2, measurements_cam3; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); - smartFactorInstance->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(sharedK)); + smartFactorInstance->add(measurements_cam1, views, model); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); - // hessianFactor->print("Hessian factor \n"); + boost::shared_ptr factor = smartFactorInstance->linearize( + values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(pose_right)); + rotValues.insert(x3, poseDrift.compose(pose_above)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); - // hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr factorRot = smartFactorInstance->linearize( + rotValues); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(pose_right)); + tranValues.insert(x3, poseDrift2.compose(pose_above)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr factorRotTran = + smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ - // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { - std::vector views; + using namespace vanillaPose2; + + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - SimpleCamera cam3(pose3, *K2); - - Point3 landmark1(5, 0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; + // All cameras have the same pose so will be degenerate ! + Camera cam2(level_pose, sharedK2); + Camera cam3(level_pose, sharedK2); + vector measurements_cam1; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactor(new SmartFactor()); - smartFactor->add(measurements_cam1, views, model, K2); - + SmartFactor::shared_ptr smartFactor(new SmartFactor(sharedK2)); + smartFactor->add(measurements_cam1, views, model); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr factor = smartFactor->linearize(values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(level_pose)); + rotValues.insert(x3, poseDrift.compose(level_pose)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr factorRot = // + smartFactor->linearize(rotValues); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(level_pose)); + tranValues.insert(x3, poseDrift2.compose(level_pose)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr factorRotTran = smartFactor->linearize( + tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - SmartProjectionPoseFactor factor1(rankTol, linThreshold); - boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - factor1.add(measurement1, poseKey1, model, Kbundler); + using namespace bundlerPose; + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartFactor factor(sharedBundlerK, boost::none, params); + factor.add(measurement1, x1, model); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3Bundler ){ - // cout << " ************************ SmartProjectionPoseFactor: Cal3Bundler **********************" << endl; +TEST( SmartProjectionPoseFactor, Cal3Bundler ) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - PinholeCamera cam1(pose1, *Kbundler); + using namespace bundlerPose; - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - PinholeCamera cam2(pose2, *Kbundler); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - PinholeCamera cam3(pose3, *Kbundler); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); + // three landmarks ~5 meters in front of camera Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); - - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler()); - smartFactor1->add(measurements_cam1, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedBundlerK)); + smartFactor1->add(measurements_cam1, views, model); - SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler()); - smartFactor2->add(measurements_cam2, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedBundlerK)); + smartFactor2->add(measurements_cam2, views, model); - SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler()); - smartFactor3->add(measurements_cam3, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedBundlerK)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1272,136 +1278,123 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-6)); - if(isDebugTest) tictoc_print_(); - } + EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); +} /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { - std::vector views; + using namespace bundlerPose; + + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - PinholeCamera cam1(pose1, *Kbundler); + // Two different cameras + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedBundlerK); + Camera cam3(pose3, sharedBundlerK); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - PinholeCamera cam2(pose2, *Kbundler); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - PinholeCamera cam3(pose3, *Kbundler); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); + // landmark3 at 3 meters now Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedBundlerK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); - double rankTol = 10; + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedBundlerK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); - SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, Kbundler); - - SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, Kbundler); - - SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(sharedBundlerK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; - // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 6b79171df..2c3a64495 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -35,7 +36,7 @@ static const boost::shared_ptr sharedCal = // // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); -PinholeCamera camera1(pose1, *sharedCal); +SimpleCamera camera1(pose1, *sharedCal); // landmark ~5 meters infront of camera static const Point3 landmark(5, 0.5, 1.2); @@ -48,7 +49,7 @@ TEST( triangulation, TriangulationFactor ) { // Create the factor with a measurement that is 3 pixels off in x Key pointKey(1); SharedNoiseModel model; - typedef TriangulationFactor<> Factor; + typedef TriangulationFactor Factor; Factor factor(camera1, z1, model, pointKey); // Use the factor to calculate the Jacobians diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 5dda02350..4db265036 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -76,7 +76,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index cac711043..c4f09c71c 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -65,7 +65,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 4b89a4b60..0530af556 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -116,7 +116,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); } diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index 56850e991..f1e2b48c2 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -17,15 +17,17 @@ #pragma once -#include +#include +#include +#include +#include +#include + #include #include #include -#include -#include -#include -#include +#include namespace gtsam { @@ -37,8 +39,10 @@ namespace gtsam std::pair, boost::shared_ptr > EliminateSymbolic(const FactorGraph& factors, const Ordering& keys) { + gttic(EliminateSymbolic); + // Gather all keys - FastSet allKeys; + KeySet allKeys; BOOST_FOREACH(const boost::shared_ptr& factor, factors) { allKeys.insert(factor->begin(), factor->end()); } @@ -50,7 +54,7 @@ namespace gtsam } // Sort frontal keys - FastSet frontals(keys); + KeySet frontals(keys); const size_t nFrontals = keys.size(); // Build a key vector with the frontals followed by the separator diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index f30f88935..aca1ba043 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -144,7 +144,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; // IndexFactor diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 9813df331..0a5427ba3 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -111,7 +111,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index 49b14bc07..f2d891827 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -46,10 +46,10 @@ TEST( JunctionTree, constructor ) frontal1 = list_of(2)(3), frontal2 = list_of(0)(1), sep1, sep2 = list_of(2); - EXPECT(assert_container_equality(frontal1, actual.roots().front()->keys)); + EXPECT(assert_container_equality(frontal1, actual.roots().front()->orderedFrontalKeys)); //EXPECT(assert_equal(sep1, actual.roots().front()->separator)); LONGS_EQUAL(1, (long)actual.roots().front()->factors.size()); - EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->keys)); + EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->orderedFrontalKeys)); //EXPECT(assert_equal(sep2, actual.roots().front()->children.front()->separator)); LONGS_EQUAL(2, (long)actual.roots().front()->children.front()->factors.size()); EXPECT(assert_equal(*simpleChain[2], *actual.roots().front()->factors[0])); diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 89a97d51b..781b08d57 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -13,6 +13,10 @@ if("@GTSAM_USE_TBB@") list(APPEND GTSAM_INCLUDE_DIR "@TBB_INCLUDE_DIRS@") endif() +# Append Eigen include path, set in top-level CMakeLists.txt to either +# system-eigen, or GTSAM eigen path +list(APPEND GTSAM_INCLUDE_DIR "@GTSAM_EIGEN_INCLUDE_PREFIX@") + if("@GTSAM_USE_EIGEN_MKL@") list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@") endif() diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index b579364b6..37a4fe5a4 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -193,7 +193,7 @@ private: // add the belief factor for each neighbor variable to this star graph // also record the factor index for later modification - FastSet neighbors = star->keys(); + KeySet neighbors = star->keys(); neighbors.erase(key); CorrectedBeliefIndices correctedBeliefIndices; BOOST_FOREACH(Key neighbor, neighbors) { diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 818266d4c..942e1ab55 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -3,18 +3,15 @@ * @author Alex Cunningham */ -#include -#include - -#include - #include +#include +#include namespace gtsam { using namespace std; -static const Vector g = delta(3, 2, 9.81); +static const Vector kGravity = delta(3, 2, 9.81); /* ************************************************************************* */ double bound(double a, double min, double max) { @@ -24,28 +21,30 @@ double bound(double a, double min, double max) { } /* ************************************************************************* */ -PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, - double vx, double vy, double vz) -: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {} +PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, + double z, double vx, double vy, double vz) : + Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), + Velocity3(vx, vy, vz)) { +} /* ************************************************************************* */ -PoseRTV::PoseRTV(const Vector& rtv) -: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3)) -{ +PoseRTV::PoseRTV(const Vector& rtv) : + Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), + Velocity3(rtv.tail(3))) { } /* ************************************************************************* */ Vector PoseRTV::vector() const { Vector rtv(9); - rtv.head(3) = Rt_.rotation().xyz(); - rtv.segment(3,3) = Rt_.translation().vector(); - rtv.tail(3) = v_.vector(); + rtv.head(3) = rotation().xyz(); + rtv.segment(3,3) = translation().vector(); + rtv.tail(3) = velocity().vector(); return rtv; } /* ************************************************************************* */ bool PoseRTV::equals(const PoseRTV& other, double tol) const { - return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol); + return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol); } /* ************************************************************************* */ @@ -53,73 +52,7 @@ void PoseRTV::print(const string& s) const { cout << s << ":" << endl; gtsam::print((Vector)R().xyz(), " R:rpy"); t().print(" T"); - v_.print(" V"); -} - -/* ************************************************************************* */ -PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) { - if (H) CONCEPT_NOT_IMPLEMENTED; - Pose3 newPose = Pose3::Expmap(v.head<6>()); - Velocity3 newVel = Velocity3(v.tail<3>()); - return PoseRTV(newPose, newVel); -} - -/* ************************************************************************* */ -Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) { - if (H) CONCEPT_NOT_IMPLEMENTED; - Vector6 Lx = Pose3::Logmap(p.Rt_); - Vector3 Lv = p.v_.vector(); - return (Vector9() << Lx, Lv).finished(); -} - -/* ************************************************************************* */ -PoseRTV PoseRTV::retract(const Vector& v, - ChartJacobian Horigin, - ChartJacobian Hv) const { - if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED; - assert(v.size() == 9); - // First order approximation - Pose3 newPose = Rt_.retract(sub(v, 0, 6)); - Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9)); - return PoseRTV(newPose, newVel); -} - -/* ************************************************************************* */ -Vector PoseRTV::localCoordinates(const PoseRTV& p1, - ChartJacobian Horigin, - ChartJacobian Hp) const { - if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED; - const Pose3& x0 = pose(), &x1 = p1.pose(); - // First order approximation - Vector6 poseLogmap = x0.localCoordinates(x1); - Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector(); - return (Vector(9) << poseLogmap, lv).finished(); -} - -/* ************************************************************************* */ -PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } -PoseRTV PoseRTV::inverse(ChartJacobian H1) const { - if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); - return PoseRTV(Rt_.inverse(), - v_); -} - -/* ************************************************************************* */ -PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); } -PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1, - ChartJacobian H2) const { - if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5); - if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5); - return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_); -} - -/* ************************************************************************* */ -PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); } -PoseRTV PoseRTV::between(const PoseRTV& p, - ChartJacobian H1, - ChartJacobian H2) const { - if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5); - if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5); - return inverse().compose(p); + velocity().print(" V"); } /* ************************************************************************* */ @@ -187,7 +120,7 @@ PoseRTV PoseRTV::generalDynamics( Rot3 r2 = rotation().retract(gyro * dt); // Integrate Velocity Equations - Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g))); + Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity)); // Integrate Position Equations Point3 t2 = translationIntegration(r2, v2, dt); @@ -205,7 +138,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { // acceleration Vector3 accel = (v2-v1).vector() / dt; - imu.head<3>() = r2.transpose() * (accel - g); + imu.head<3>() = r2.transpose() * (accel - kGravity); // rotation rates // just using euler angles based on matlab code @@ -232,54 +165,40 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub } /* ************************************************************************* */ -double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } double PoseRTV::range(const PoseRTV& other, OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const { - if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5); - if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5); - return t().distance(other.t()); + Matrix36 D_t1_pose, D_t2_other; + const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); + const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); + Matrix13 D_d_t1, D_d_t2; + double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; + if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; + return d; } /* ************************************************************************* */ -PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) { - return global.transformed_from(transform); -} - PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, OptionalJacobian<9, 6> Dtrans) const { - // Note that we rotate the velocity - Matrix DVr, DTt; - Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt); - if (!Dglobal && !Dtrans) - return PoseRTV(trans.compose(pose()), newvel); // Pose3 transform is just compose - Matrix DTc, DGc; - Pose3 newpose = trans.compose(pose(), DTc, DGc); + Matrix6 D_newpose_trans, D_newpose_pose; + Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose); + + // Note that we rotate the velocity + Matrix3 D_newvel_R, D_newvel_v; + Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v); if (Dglobal) { - *Dglobal = zeros(9,9); - insertSub(*Dglobal, DGc, 0, 0); - - // Rotate velocity - insertSub(*Dglobal, eye(3,3), 6, 6); // FIXME: should this actually be an identity matrix? + Dglobal->setZero(); + Dglobal->topLeftCorner<6,6>() = D_newpose_pose; + Dglobal->bottomRightCorner<3,3>() = D_newvel_v; } if (Dtrans) { - *Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-8); - // - // *Dtrans = zeros(9,6); - // // directly affecting the pose - // insertSub(*Dtrans, DTc, 0, 0); // correct in tests - // - // // rotating the velocity - // Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z()); - // trans.rotation().print("Transform rotation"); - // gtsam::print(vRhat, "vRhat"); - // gtsam::print(DVr, "DVr"); - // // FIXME: find analytic derivative - //// insertSub(*Dtrans, vRhat, 6, 0); // works if PoseRTV.rotation() = I - //// insertSub(*Dtrans, trans.rotation().matrix() * vRhat, 6, 0); // FAIL: both tests fail + Dtrans->setZero(); + Dtrans->topLeftCorner<6,6>() = D_newpose_trans; + Dtrans->bottomLeftCorner<3,3>() = D_newvel_R; } return PoseRTV(newpose, newvel); } diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index ea7a2c9ff..78bd1fe6f 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -7,8 +7,8 @@ #pragma once #include -#include #include +#include namespace gtsam { @@ -19,29 +19,30 @@ typedef Point3 Velocity3; * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation */ -class GTSAM_UNSTABLE_EXPORT PoseRTV { +class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup { protected: - Pose3 Rt_; - Velocity3 v_; - + typedef ProductLieGroup Base; typedef OptionalJacobian<9, 9> ChartJacobian; public: - enum { dimension = 9 }; // constructors - with partial versions PoseRTV() {} - PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel) - : Rt_(rot, pt), v_(vel) {} - PoseRTV(const Rot3& rot, const Point3& pt, const Velocity3& vel) - : Rt_(rot, pt), v_(vel) {} - explicit PoseRTV(const Point3& pt) - : Rt_(Rot3::identity(), pt) {} + PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel) + : Base(Pose3(rot, t), vel) {} + PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel) + : Base(Pose3(rot, t), vel) {} + explicit PoseRTV(const Point3& t) + : Base(Pose3(Rot3(), t),Velocity3()) {} PoseRTV(const Pose3& pose, const Velocity3& vel) - : Rt_(pose), v_(vel) {} + : Base(pose, vel) {} explicit PoseRTV(const Pose3& pose) - : Rt_(pose) {} + : Base(pose,Velocity3()) {} + + // Construct from Base + PoseRTV(const Base& base) + : Base(base) {} /** build from components - useful for data files */ PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, @@ -51,72 +52,46 @@ public: explicit PoseRTV(const Vector& v); // access - const Point3& t() const { return Rt_.translation(); } - const Rot3& R() const { return Rt_.rotation(); } - const Velocity3& v() const { return v_; } - const Pose3& pose() const { return Rt_; } + const Pose3& pose() const { return first; } + const Velocity3& v() const { return second; } + const Point3& t() const { return pose().translation(); } + const Rot3& R() const { return pose().rotation(); } // longer function names - const Point3& translation() const { return Rt_.translation(); } - const Rot3& rotation() const { return Rt_.rotation(); } - const Velocity3& velocity() const { return v_; } + const Point3& translation() const { return pose().translation(); } + const Rot3& rotation() const { return pose().rotation(); } + const Velocity3& velocity() const { return second; } // Access to vector for ease of use with Matlab // and avoidance of Point3 Vector vector() const; - Vector translationVec() const { return Rt_.translation().vector(); } - Vector velocityVec() const { return v_.vector(); } + Vector translationVec() const { return pose().translation().vector(); } + Vector velocityVec() const { return velocity().vector(); } // testable bool equals(const PoseRTV& other, double tol=1e-6) const; void print(const std::string& s="") const; - // Manifold - static size_t Dim() { return 9; } - size_t dim() const { return Dim(); } + /// @name Manifold + /// @{ + using Base::dimension; + using Base::dim; + using Base::Dim; + using Base::retract; + using Base::localCoordinates; + /// @} - /** - * retract/unretract assume independence of components - * Tangent space parameterization: - * - v(0-2): Rot3 (roll, pitch, yaw) - * - v(3-5): Point3 - * - v(6-8): Translational velocity - */ - PoseRTV retract(const Vector& v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const; - Vector localCoordinates(const PoseRTV& p, ChartJacobian Horigin=boost::none,ChartJacobian Hp=boost::none) const; + /// @name measurement functions + /// @{ - // Lie TODO IS this a Lie group or just a Manifold???? - /** - * expmap/logmap are poor approximations that assume independence of components - * Currently implemented using the poor retract/unretract approximations - */ - static PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none); - static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none); - - static PoseRTV identity() { return PoseRTV(); } - - /** Derivatives calculated numerically */ - PoseRTV inverse(ChartJacobian H1=boost::none) const; - - /** Derivatives calculated numerically */ - PoseRTV compose(const PoseRTV& p, - ChartJacobian H1=boost::none, - ChartJacobian H2=boost::none) const; - - PoseRTV operator*(const PoseRTV& p) const { return compose(p); } - - /** Derivatives calculated numerically */ - PoseRTV between(const PoseRTV& p, - ChartJacobian H1=boost::none, - ChartJacobian H2=boost::none) const; - - // measurement functions - /** Derivatives calculated numerically */ + /** range between translations */ double range(const PoseRTV& other, OptionalJacobian<1,9> H1=boost::none, OptionalJacobian<1,9> H2=boost::none) const; + /// @} - // IMU-specific + /// @name IMU-specific + /// @{ /// Dynamics integrator for ground robots /// Always move from time 1 to time 2 @@ -164,7 +139,9 @@ public: ChartJacobian Dglobal = boost::none, OptionalJacobian<9, 6> Dtrans = boost::none) const; - // Utility functions + /// @} + /// @name Utility functions + /// @{ /// RRTMbn - Function computes the rotation rate transformation matrix from /// body axis rates to euler angle (global) rates @@ -177,19 +154,20 @@ public: static Matrix RRTMnb(const Vector& euler); static Matrix RRTMnb(const Rot3& att); + /// @} private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(Rt_); - ar & BOOST_SERIALIZATION_NVP(v_); + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(first); + ar & BOOST_SERIALIZATION_NVP(second); } }; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index a92ae0426..9ecc7619e 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -52,7 +52,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 0261257be..2ea582292 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -76,11 +76,12 @@ TEST( testPoseRTV, equals ) { /* ************************************************************************* */ TEST( testPoseRTV, Lie ) { // origin and zero deltas - EXPECT(assert_equal(PoseRTV(), PoseRTV().retract(zero(9)), tol)); - EXPECT(assert_equal(zero(9), PoseRTV().localCoordinates(PoseRTV()), tol)); + PoseRTV identity; + EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol)); + EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol)); PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(state1, state1.retract(zero(9)), tol)); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol)); EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished(); @@ -88,9 +89,9 @@ TEST( testPoseRTV, Lie ) { Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); PoseRTV state2(pt2, rot2, vel2); - EXPECT(assert_equal(state2, state1.retract(delta), 1e-1)); + EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1)); EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1)); - EXPECT(assert_equal(-delta, state2.localCoordinates(state1), 1e-1)); // loose tolerance due to retract approximation + EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1)); } /* ************************************************************************* */ @@ -119,6 +120,43 @@ TEST( testPoseRTV, dynamics_identities ) { } +/* ************************************************************************* */ +PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); } +TEST( testPoseRTV, compose ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); } +TEST( testPoseRTV, between ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); } +TEST( testPoseRTV, inverse ) { + PoseRTV state1(pt, rot, vel); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); + EXPECT(assert_equal(numericH1, actH1, tol)); +} + /* ************************************************************************* */ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } TEST( testPoseRTV, range ) { diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index c8a4e7123..e00dcee57 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -26,16 +26,14 @@ * -makes monocular observations of many landmarks */ -#include +#include +#include #include #include #include #include #include #include -#include - -#include #include #include @@ -46,7 +44,8 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartProjectionPoseFactor SmartFactor; + typedef PinholePose Camera; + typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; NonlinearFactorGraph graph; @@ -68,18 +67,17 @@ int main(int argc, char** argv){ cout << "Reading camera poses" << endl; ifstream pose_file(pose_loc.c_str()); - int pose_id; + int i; MatrixRowMajor m(4,4); //read camera pose parameters and use to make initial estimates of camera poses - while (pose_file >> pose_id) { - for (int i = 0; i < 16; i++) { + while (pose_file >> i) { + for (int i = 0; i < 16; i++) pose_file >> m.data()[i]; - } - initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + initial_estimate.insert(i, Pose3(m)); } - // camera and landmark keys - size_t x, l; + // landmark keys + size_t l; // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation @@ -89,24 +87,24 @@ int main(int argc, char** argv){ //read stereo measurements and construct smart factors - SmartFactor::shared_ptr factor(new SmartFactor()); + SmartFactor::shared_ptr factor(new SmartFactor(K)); size_t current_l = 3; // hardcoded landmark ID from first measurement - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); - factor = SmartFactor::shared_ptr(new SmartFactor()); + factor = SmartFactor::shared_ptr(new SmartFactor(K)); current_l = l; } - factor->add(Point2(uL,v), Symbol('x',x), model, K); + factor->add(Point2(uL,v), i, model); } - Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + Pose3 firstPose = initial_estimate.at(1); //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.push_back(NonlinearEquality(1,firstPose)); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 770c5f18c..ac2c31077 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartStereoProjectionPoseFactor SmartFactor; + typedef SmartStereoProjectionPoseFactor SmartFactor; bool output_poses = true; string poseOutput("../../../examples/data/optimized_poses.txt"); @@ -88,11 +88,13 @@ int main(int argc, char** argv){ } Values initial_pose_values = initial_estimate.filter(); - if(output_poses){ - init_pose3Out.open(init_poseOutput.c_str(),ios::out); - for(int i = 1; i<=initial_pose_values.size(); i++){ - init_pose3Out << i << " " << initial_pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, - " ", " ")) << endl; + if (output_poses) { + init_pose3Out.open(init_poseOutput.c_str(), ios::out); + for (size_t i = 1; i <= initial_pose_values.size(); i++) { + init_pose3Out + << i << " " + << initial_pose_values.at(Symbol('x', i)).matrix().format( + Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } } @@ -141,7 +143,7 @@ int main(int argc, char** argv){ if(output_poses){ pose3Out.open(poseOutput.c_str(),ios::out); - for(int i = 1; i<=pose_values.size(); i++){ + for(size_t i = 1; i<=pose_values.size(); i++){ pose3Out << i << " " << pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index 3c9247048..70a22b9a5 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -92,7 +92,7 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(azimuth_); ar & BOOST_SERIALIZATION_NVP(elevation_); } diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 9347b9ffb..4ce0eaedb 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -175,7 +175,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(k_); } diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index c48508fa0..9d01e20a5 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -133,7 +133,7 @@ private: // Serialization function friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) { + void serialize(Archive & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(T_); ar & BOOST_SERIALIZATION_NVP(z_); } diff --git a/gtsam_unstable/geometry/SimPolygon2D.h b/gtsam_unstable/geometry/SimPolygon2D.h index 02892c519..835bb4083 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.h +++ b/gtsam_unstable/geometry/SimPolygon2D.h @@ -128,7 +128,6 @@ public: }; typedef std::vector SimPolygon2DVector; -typedef std::vector Point2Vector; } //\namespace gtsam diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp new file mode 100644 index 000000000..8d75d767c --- /dev/null +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + + * 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 Similarity3.cpp + * @brief Implementation of Similarity3 transform + * @author Paul Drews + */ + +#include + +#include +#include +#include + +#include + +namespace gtsam { + +Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : + R_(R), t_(t), s_(s) { +} + +Similarity3::Similarity3() : + R_(), t_(), s_(1){ +} + +Similarity3::Similarity3(double s) : + s_ (s) { +} + +Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) : + R_ (R), t_ (t), s_ (s) { +} + +Similarity3::operator Pose3() const { + return Pose3(R_, s_*t_); +} + +Similarity3 Similarity3::identity() { + return Similarity3(); } + +//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { +// return Vector7(); +//} +// +//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { +// return Similarity3(); +//} + +bool Similarity3::operator==(const Similarity3& other) const { + return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); +} + +bool Similarity3::equals(const Similarity3& sim, double tol) const { + return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) + && s_ < (sim.s_+tol) && s_ > (sim.s_-tol); +} + +Similarity3::Translation Similarity3::transform_from(const Translation& p) const { + return R_ * (s_ * p) + t_; +} + +Matrix7 Similarity3::AdjointMap() const{ + const Matrix3 R = R_.matrix(); + const Vector3 t = t_.vector(); + Matrix3 A = s_ * skewSymmetric(t) * R; + Matrix7 adj; + adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1; + return adj; +} + +inline Similarity3::Translation Similarity3::operator*(const Translation& p) const { + return transform_from(p); +} + +Similarity3 Similarity3::inverse() const { + Rotation Rt = R_.inverse(); + Translation sRt = R_.inverse() * (-s_ * t_); + return Similarity3(Rt, sRt, 1.0/s_); +} + +Similarity3 Similarity3::operator*(const Similarity3& T) const { + return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_); +} + +void Similarity3::print(const std::string& s) const { + std::cout << std::endl; + std::cout << s; + rotation().print("R:\n"); + translation().print("t: "); + std::cout << "s: " << scale() << std::endl; +} + +std::ostream &operator<<(std::ostream &os, const Similarity3& p) { + os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " << + p.scale() << "]\';"; + return os; +} + +Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { + // Will retracting or localCoordinating R work if R is not a unit rotation? + // Also, how do we actually get s out? Seems like we need to store it somewhere. + Rotation r; //Create a zero rotation to do our retraction. + return Similarity3( // + r.retract(v.head<3>()), // retract rotation using v[0,1,2] + Translation(v.segment<3>(3)), // Retract the translation + 1.0 + v[6]); //finally, update scale using v[6] +} + +Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { + Rotation r; //Create a zero rotation to do the retraction + Vector7 v; + v.head<3>() = r.localCoordinates(other.R_); + v.segment<3>(3) = other.t_.vector(); + //v.segment<3>(3) = translation().localCoordinates(other.translation()); + v[6] = other.s_ - 1.0; + return v; +} +} + + diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h new file mode 100644 index 000000000..eec2124ee --- /dev/null +++ b/gtsam_unstable/geometry/Similarity3.h @@ -0,0 +1,152 @@ +/* ---------------------------------------------------------------------------- + + * 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 Similarity3.h + * @brief Implementation of Similarity3 transform + * @author Paul Drews + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +// Forward declarations +class Pose3; + +/** + * 3D similarity transform + */ +class Similarity3: public LieGroup { + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; + +private: + Rotation R_; + Translation t_; + double s_; + +public: + + /// @name Constructors + /// @{ + + Similarity3(); + + /// Construct pure scaling + Similarity3(double s); + + /// Construct from GTSAM types + Similarity3(const Rotation& R, const Translation& t, double s); + + /// Construct from Eigen types + Similarity3(const Matrix3& R, const Vector3& t, double s); + + /// @} + /// @name Testable + /// @{ + + /// Compare with tolerance + bool equals(const Similarity3& sim, double tol) const; + + /// Compare with standard tolerance + bool operator==(const Similarity3& other) const; + + /// Print with optional string + void print(const std::string& s) const; + + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); + + /// @} + /// @name Group + /// @{ + + /// Return an identity transform + static Similarity3 identity(); + + /// Return the inverse + Similarity3 inverse() const; + + Translation transform_from(const Translation& p) const; + + /** syntactic sugar for transform_from */ + inline Translation operator*(const Translation& p) const; + + Similarity3 operator*(const Similarity3& T) const; + + /// @} + /// @name Standard interface + /// @{ + + /// Return a GTSAM rotation + const Rotation& rotation() const { + return R_; + }; + + /// Return a GTSAM translation + const Translation& translation() const { + return t_; + }; + + /// Return the scale + double scale() const { + return s_; + }; + + /// Convert to a rigid body pose + operator Pose3() const; + + /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes + inline static size_t Dim() { + return 7; + }; + + /// Dimensionality of tangent space = 7 DOF + inline size_t dim() const { + return 7; + }; + + /// @} + /// @name Chart + /// @{ + + /// Update Similarity transform via 7-dim vector in tangent space + struct ChartAtOrigin { + static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none); + + /// 7-dimensional vector v in tangent space that makes other = this->retract(v) + static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none); + }; + + /// Project from one tangent space to another + Matrix7 AdjointMap() const; + + /// @} + /// @name Stubs + /// @{ + + /// Not currently implemented, required because this is a lie group + static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none); + static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none); + + using LieGroup::inverse; // version with derivative +}; + +template<> +struct traits : public internal::LieGroup {}; +} diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp new file mode 100644 index 000000000..b2b5d5702 --- /dev/null +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -0,0 +1,256 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSimilarity3.cpp + * @brief Unit tests for Similarity3 class + * @author Paul Drews + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace gtsam; +using namespace std; +using symbol_shorthand::X; + +GTSAM_CONCEPT_TESTABLE_INST(Similarity3) + +static Point3 P(0.2,0.7,-2); +static Rot3 R = Rot3::rodriguez(0.3,0,0); +static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); +static Similarity3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1); +static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1); + +//****************************************************************************** +TEST(Similarity3, Constructors) { + Similarity3 test; +} + +//****************************************************************************** +TEST(Similarity3, Getters) { + Similarity3 test; + EXPECT(assert_equal(Rot3(), test.rotation())); + EXPECT(assert_equal(Point3(), test.translation())); + EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9); +} + +//****************************************************************************** +TEST(Similarity3, Getters2) { + Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7); + EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation())); + EXPECT(assert_equal(Point3(4, 5, 6), test.translation())); + EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9); +} + +TEST(Similarity3, AdjointMap) { + Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Matrix7 result; + result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, + 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, + -2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000, + 0, 0, 0, -0.2248, -0.3502, -0.9093, 0, + 0, 0, 0, 0.9024, -0.4269, -0.0587, 0, + 0, 0, 0, -0.3676, -0.8337, 0.4120, 0, + 0, 0, 0, 0, 0, 0, 1.0000; + EXPECT(assert_equal(result, test.AdjointMap(), 1e-3)); +} + +TEST(Similarity3, inverse) { + Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Matrix3 Re; + Re << -0.2248, 0.9024, -0.3676, + -0.3502, -0.4269, -0.8337, + -0.9093, -0.0587, 0.4120; + Vector3 te(-9.8472, 59.7640, 10.2125); + Similarity3 expected(Re, te, 1.0/7.0); + EXPECT(assert_equal(expected, test.inverse(), 1e-3)); +} + +TEST(Similarity3, multiplication) { + Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11); + Matrix3 re; + re << 0.0688, 0.9863, -0.1496, + -0.5665, -0.0848, -0.8197, + -0.8211, 0.1412, 0.5530; + Vector3 te(-13.6797, 3.2441, -5.7794); + Similarity3 expected(re, te, 77); + EXPECT(assert_equal(expected, test1*test2, 1e-2)); +} + +//****************************************************************************** +TEST(Similarity3, Manifold) { + EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); + Vector z = Vector7::Zero(); + Similarity3 sim; + EXPECT(sim.retract(z) == sim); + + Vector7 v = Vector7::Zero(); + v(6) = 2; + Similarity3 sim2; + EXPECT(sim2.retract(z) == sim2); + + EXPECT(assert_equal(z, sim2.localCoordinates(sim))); + + Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1); + Vector v3(7); + v3 << 0, 0, 0, 1, 2, 3, 0; + EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); + +// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); + Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1); + + Vector vlocal = sim.localCoordinates(other); + + EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); + + Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1); + Rot3 R = Rot3::rodriguez(0.3,0,0); + + Vector vlocal2 = sim.localCoordinates(other2); + + EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2)); + + // TODO add unit tests for retract and localCoordinates +} + +/* ************************************************************************* */ +TEST( Similarity3, retract_first_order) +{ + Similarity3 id; + Vector v = zero(7); + v(0) = 0.3; + EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2)); + v(3)=0.2;v(4)=0.7;v(5)=-2; + EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2)); +} + +/* ************************************************************************* */ +TEST(Similarity3, localCoordinates_first_order) +{ + Vector d12 = repeat(7,0.1); + d12(6) = 1.0; + Similarity3 t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} + +/* ************************************************************************* */ +TEST(Similarity3, manifold_first_order) +{ + Similarity3 t1 = T; + Similarity3 t2 = T3; + Similarity3 origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); +} + +TEST(Similarity3, Optimization) { + Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); + Symbol key('x',1); + PriorFactor factor(key, prior, model); + + NonlinearFactorGraph graph; + graph.push_back(factor); + + Values initial; + initial.insert(key, Similarity3()); + + Values result; + LevenbergMarquardtParams params; + params.setVerbosityLM("TRYCONFIG"); + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + EXPECT(assert_equal(prior, result.at(key), 1e-4)); +} + +TEST(Similarity3, Optimization2) { + Similarity3 prior = Similarity3(); + Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); + Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); + Similarity3 loop = Similarity3(1.42); + + //prior.print("Goal Transform"); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01); + SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); + SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); + PriorFactor factor(X(1), prior, model); + BetweenFactor b1(X(1), X(2), m1, betweenNoise); + BetweenFactor b2(X(2), X(3), m2, betweenNoise); + BetweenFactor b3(X(3), X(4), m3, betweenNoise); + BetweenFactor b4(X(4), X(5), m4, betweenNoise); + BetweenFactor lc(X(5), X(1), loop, betweenNoise2); + + + + NonlinearFactorGraph graph; + graph.push_back(factor); + graph.push_back(b1); + graph.push_back(b2); + graph.push_back(b3); + graph.push_back(b4); + graph.push_back(lc); + + //graph.print("Full Graph\n"); + + Values initial; + initial.insert(X(1), Similarity3()); + initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); + initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); + initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); + initial.insert(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); + + //initial.print("Initial Estimate\n"); + + Values result; + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + //result.print("Optimized Estimate\n"); + Pose3 p1, p2, p3, p4, p5; + p1 = Pose3(result.at(X(1))); + p2 = Pose3(result.at(X(2))); + p3 = Pose3(result.at(X(3))); + p4 = Pose3(result.at(X(4))); + p5 = Pose3(result.at(X(5))); + + //p1.print("Pose1"); + //p2.print("Pose2"); + //p3.print("Pose3"); + //p4.print("Pose4"); + //p5.print("Pose5"); + + Similarity3 expected(0.7); + EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 95e635516..6b57bfcd0 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -8,6 +8,7 @@ class gtsam::Vector6; class gtsam::LieScalar; class gtsam::LieVector; class gtsam::Point2; +class gtsam::Point2Vector; class gtsam::Rot2; class gtsam::Pose2; class gtsam::Point3; @@ -159,32 +160,6 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; -// std::vector -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - //Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; - #include class SimWall2D { SimWall2D(); @@ -477,7 +452,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); }; -#include +#include #include virtual class VelocityConstraint3 : gtsam::NonlinearFactor { diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index f7a575f8c..68713f965 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -48,7 +48,7 @@ class QPSolver { GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process. VariableIndex costVariableIndex_, equalityVariableIndex_, inequalityVariableIndex_; - FastSet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph + KeySet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph public: /// Constructor diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 952b134b5..d74a0c302 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -28,25 +28,34 @@ namespace gtsam { /* ************************************************************************* */ -void BatchFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { +void BatchFixedLagSmoother::print(const std::string& s, + const KeyFormatter& keyFormatter) const { FixedLagSmoother::print(s, keyFormatter); // TODO: What else to print? } /* ************************************************************************* */ -bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - const BatchFixedLagSmoother* e = dynamic_cast (&rhs); - return e != NULL - && FixedLagSmoother::equals(*e, tol) - && factors_.equals(e->factors_, tol) - && theta_.equals(e->theta_, tol); +bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, + double tol) const { + const BatchFixedLagSmoother* e = + dynamic_cast(&rhs); + return e != NULL && FixedLagSmoother::equals(*e, tol) + && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol); } /* ************************************************************************* */ -FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { + throw std::runtime_error( + "BatchFixedLagSmoother::marginalCovariance not implemented"); +} + +/* ************************************************************************* */ +FixedLagSmoother::Result BatchFixedLagSmoother::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const KeyTimestampMap& timestamps) { const bool debug = ISDEBUG("BatchFixedLagSmoother update"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::update() START" << std::endl; } @@ -70,11 +79,13 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap // Get current timestamp double current_timestamp = getCurrentTimestamp(); - if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + if (debug) + std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); - if(debug) { + std::set marginalizableKeys = findKeysBefore( + current_timestamp - smootherLag_); + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizableKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -90,19 +101,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap // Optimize gttic(optimize); Result result; - if(factors_.size() > 0) { + if (factors_.size() > 0) { result = optimize(); } gttoc(optimize); // Marginalize out old variables. gttic(marginalize); - if(marginalizableKeys.size() > 0) { + if (marginalizableKeys.size() > 0) { marginalize(marginalizableKeys); } gttoc(marginalize); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl; } @@ -110,11 +121,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap } /* ************************************************************************* */ -void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors) { +void BatchFixedLagSmoother::insertFactors( + const NonlinearFactorGraph& newFactors) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { Key index; // Insert the factor into an existing hole in the factor graph, if possible - if(availableSlots_.size() > 0) { + if (availableSlots_.size() > 0) { index = availableSlots_.front(); availableSlots_.pop(); factors_.replace(index, factor); @@ -130,9 +142,10 @@ void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors } /* ************************************************************************* */ -void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) { +void BatchFixedLagSmoother::removeFactors( + const std::set& deleteFactors) { BOOST_FOREACH(size_t slot, deleteFactors) { - if(factors_.at(slot)) { + if (factors_.at(slot)) { // Remove references to this factor from the FactorIndex BOOST_FOREACH(Key key, *(factors_.at(slot))) { factorIndex_[key].erase(slot); @@ -143,7 +156,8 @@ void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) availableSlots_.push(slot); } else { // TODO: Throw an error?? - std::cout << "Attempting to remove a factor from slot " << slot << ", but it is already NULL." << std::endl; + std::cout << "Attempting to remove a factor from slot " << slot + << ", but it is already NULL." << std::endl; } } } @@ -159,7 +173,7 @@ void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { factorIndex_.erase(key); // Erase the key from the set of linearized keys - if(linearKeys_.exists(key)) { + if (linearKeys_.exists(key)) { linearKeys_.erase(key); } } @@ -178,11 +192,11 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { const bool debug = ISDEBUG("BatchFixedLagSmoother reorder"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::reorder() START" << std::endl; } - if(debug) { + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizeKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -191,13 +205,14 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { } // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); + ordering_ = Ordering::ColamdConstrainedFirst(factors_, + std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); - if(debug) { + if (debug) { ordering_.print("New Ordering: "); } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::reorder() FINISH" << std::endl; } } @@ -207,7 +222,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { const bool debug = ISDEBUG("BatchFixedLagSmoother optimize"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::optimize() START" << std::endl; } @@ -231,14 +246,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { result.error = factors_.error(evalpoint); // check if we're already close enough - if(result.error <= errorTol) { - if(debug) { std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " << result.error << " < " << errorTol << std::endl; } + if (result.error <= errorTol) { + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " + << result.error << " < " << errorTol << std::endl; + } return result; } - if(debug) { - std::cout << "BatchFixedLagSmoother::optimize linearValues: " << linearKeys_.size() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize Initial error: " << result.error << std::endl; + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize linearValues: " + << linearKeys_.size() << std::endl; + std::cout << "BatchFixedLagSmoother::optimize Initial error: " + << result.error << std::endl; } // Use a custom optimization loop so the linearization points can be controlled @@ -254,9 +274,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_); // Keep increasing lambda until we make make progress - while(true) { + while (true) { - if(debug) { std::cout << "BatchFixedLagSmoother::optimize trying lambda = " << lambda << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize trying lambda = " + << lambda << std::endl; + } // Add prior factors at the current solution gttic(damp); @@ -267,10 +290,11 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double sigma = 1.0 / std::sqrt(lambda); BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) { size_t dim = key_value.second.size(); - Matrix A = Matrix::Identity(dim,dim); + Matrix A = Matrix::Identity(dim, dim); Vector b = key_value.second; SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); - GaussianFactor::shared_ptr prior(new JacobianFactor(key_value.first, A, b, model)); + GaussianFactor::shared_ptr prior( + new JacobianFactor(key_value.first, A, b, model)); dampedFactorGraph.push_back(prior); } } @@ -279,7 +303,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { gttic(solve); // Solve Damped Gaussian Factor Graph - newDelta = dampedFactorGraph.optimize(ordering_, parameters_.getEliminationFunction()); + newDelta = dampedFactorGraph.optimize(ordering_, + parameters_.getEliminationFunction()); // update the evalpoint with the new delta evalpoint = theta_.retract(newDelta); gttoc(solve); @@ -289,12 +314,14 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double error = factors_.error(evalpoint); gttoc(compute_error); - if(debug) { - std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " << newDelta.norm() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize next error = " << error << std::endl; + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " + << newDelta.norm() << std::endl; + std::cout << "BatchFixedLagSmoother::optimize next error = " << error + << std::endl; } - if(error < result.error) { + if (error < result.error) { // Keep this change // Update the error value result.error = error; @@ -303,7 +330,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Reset the deltas to zeros delta_.setZero(); // Put the linearization points and deltas back for specific variables - if(enforceConsistency_ && (linearKeys_.size() > 0)) { + if (enforceConsistency_ && (linearKeys_.size() > 0)) { theta_.update(linearKeys_); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) { delta_.at(key_value.key) = newDelta.at(key_value.key); @@ -311,16 +338,18 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { } // Decrease lambda for next time lambda /= lambdaFactor; - if(lambda < lambdaLowerBound) { + if (lambda < lambdaLowerBound) { lambda = lambdaLowerBound; } // End this lambda search iteration break; } else { // Reject this change - if(lambda >= lambdaUpperBound) { + if (lambda >= lambdaUpperBound) { // The maximum lambda has been used. Print a warning and end the search. - std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl; + std::cout + << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" + << std::endl; break; } else { // Increase lambda and continue searching @@ -331,15 +360,22 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { } gttoc(optimizer_iteration); - if(debug) { std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda + << std::endl; + } result.iterations++; - } while(result.iterations < maxIterations && - !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); + } while (result.iterations < maxIterations + && !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, + previousError, result.error, NonlinearOptimizerParams::SILENT)); - if(debug) { std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error + << std::endl; + } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::optimize() FINISH" << std::endl; } @@ -356,9 +392,10 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { const bool debug = ISDEBUG("BatchFixedLagSmoother marginalize"); - if(debug) std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl; - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::marginalize Marginalize Keys: "; BOOST_FOREACH(Key key, marginalizeKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -370,11 +407,11 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { std::set removedFactorSlots; VariableIndex variableIndex(factors_); BOOST_FOREACH(Key key, marginalizeKeys) { - const FastList& slots = variableIndex[key]; + const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(slots.begin(), slots.end()); } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::marginalize Removed Factor Slots: "; BOOST_FOREACH(size_t slot, removedFactorSlots) { std::cout << slot << " "; @@ -385,20 +422,24 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { // Add the removed factors to a factor graph NonlinearFactorGraph removedFactors; BOOST_FOREACH(size_t slot, removedFactorSlots) { - if(factors_.at(slot)) { + if (factors_.at(slot)) { removedFactors.push_back(factors_.at(slot)); } } - if(debug) { - PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Removed Factors: "); + if (debug) { + PrintSymbolicGraph(removedFactors, + "BatchFixedLagSmoother::marginalize Removed Factors: "); } // Calculate marginal factors on the remaining keys - NonlinearFactorGraph marginalFactors = calculateMarginalFactors(removedFactors, theta_, marginalizeKeys, parameters_.getEliminationFunction()); + NonlinearFactorGraph marginalFactors = calculateMarginalFactors( + removedFactors, theta_, marginalizeKeys, + parameters_.getEliminationFunction()); - if(debug) { - PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Marginal Factors: "); + if (debug) { + PrintSymbolicGraph(removedFactors, + "BatchFixedLagSmoother::marginalize Marginal Factors: "); } // Remove marginalized factors from the factor graph @@ -412,7 +453,8 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -421,7 +463,8 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::st } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const gtsam::KeySet& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -430,9 +473,10 @@ void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, const s } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) { +void BatchFixedLagSmoother::PrintSymbolicFactor( + const NonlinearFactor::shared_ptr& factor) { std::cout << "f("; - if(factor) { + if (factor) { BOOST_FOREACH(Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); } @@ -443,7 +487,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_pt } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { +void BatchFixedLagSmoother::PrintSymbolicFactor( + const GaussianFactor::shared_ptr& factor) { std::cout << "f("; BOOST_FOREACH(Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -452,7 +497,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph( + const NonlinearFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); @@ -460,59 +506,79 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, + const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); } } - - /* ************************************************************************* */ -NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { +NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors( + const NonlinearFactorGraph& graph, const Values& theta, + const std::set& marginalizeKeys, + const GaussianFactorGraph::Eliminate& eliminateFunction) { const bool debug = ISDEBUG("BatchFixedLagSmoother calculateMarginalFactors"); - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" + << std::endl; - if(debug) PrintKeySet(marginalizeKeys, "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); + if (debug) + PrintKeySet(marginalizeKeys, + "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); // Get the set of all keys involved in the factor graph - FastSet allKeys(graph.keys()); - if(debug) PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); + KeySet allKeys(graph.keys()); + if (debug) + PrintKeySet(allKeys, + "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys - FastSet remainingKeys; - std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); - if(debug) PrintKeySet(remainingKeys, "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); + KeySet remainingKeys; + std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), + marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); + if (debug) + PrintKeySet(remainingKeys, + "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); - if(marginalizeKeys.size() == 0) { + if (marginalizeKeys.size() == 0) { // There are no keys to marginalize. Simply return the input factors - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" + << std::endl; return graph; } else { // Create the linear factor graph GaussianFactorGraph linearFactorGraph = *graph.linearize(theta); // .first is the eliminated Bayes tree, while .second is the remaining factor graph - GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; + GaussianFactorGraph marginalLinearFactors = + *linearFactorGraph.eliminatePartialMultifrontal( + std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; // Wrap in nonlinear container factors NonlinearFactorGraph marginalFactors; marginalFactors.reserve(marginalLinearFactors.size()); BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { - marginalFactors += boost::make_shared(gaussianFactor, theta); - if(debug) { - std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; + marginalFactors += boost::make_shared( + gaussianFactor, theta); + if (debug) { + std::cout + << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; PrintSymbolicFactor(marginalFactors.back()); } } - if(debug) PrintSymbolicGraph(marginalFactors, "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); + if (debug) + PrintSymbolicGraph(marginalFactors, + "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" + << std::endl; return marginalFactors; } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 5c66d411f..605f3a5e8 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -100,6 +100,12 @@ public: return delta_; } + /// Calculate marginal covariance on given variable + Matrix marginalCovariance(Key key) const; + + static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, + const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + protected: /** A typedef defining an Key-Factor mapping **/ @@ -134,8 +140,6 @@ protected: /** A cross-reference structure to allow efficient factor lookups by key **/ FactorIndex factorIndex_; - - /** Augment the list of factors with a set of new factors */ void insertFactors(const NonlinearFactorGraph& newFactors); @@ -154,13 +158,10 @@ protected: /** Marginalize out selected variables */ void marginalize(const std::set& marginalizableKeys); - static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); - private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label); - static void PrintKeySet(const gtsam::FastSet& keys, const std::string& label); + static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label); static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 40d1746ac..c3e307010 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -221,7 +221,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Updated Smoother Summarization:", DefaultKeyFormatter); } // Find the set of new separator keys - FastSet newSeparatorKeys; + KeySet newSeparatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { newSeparatorKeys.insert(key_value.key); } @@ -536,7 +536,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { std::vector removedFactorSlots; VariableIndex variableIndex(factors_); BOOST_FOREACH(Key key, keysToMove) { - const FastList& slots = variableIndex[key]; + const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } @@ -573,7 +573,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { } // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove - FastSet newSeparatorKeys = removedFactors.keys(); + KeySet newSeparatorKeys = removedFactors.keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { newSeparatorKeys.insert(key_value.key); } @@ -584,7 +584,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize ", "New Separator Keys:", DefaultKeyFormatter); } // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys - FastSet shortcutKeys = newSeparatorKeys; + KeySet shortcutKeys = newSeparatorKeys; BOOST_FOREACH(Key key, smootherSummarization_.keys()) { shortcutKeys.insert(key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index c410ad528..29ee14aee 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -230,7 +230,7 @@ void ConcurrentBatchSmoother::reorder() { // Recalculate the variable index variableIndex_ = VariableIndex(factors_); - FastList separatorKeys = separatorValues_.keys(); + KeyVector separatorKeys = separatorValues_.keys(); ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); } @@ -371,7 +371,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { } // Get the set of separator keys - gtsam::FastSet separatorKeys; + gtsam::KeySet separatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { separatorKeys.insert(key_value.key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 3b6b884f6..b893860cc 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -52,16 +52,16 @@ namespace internal { /* ************************************************************************* */ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const FastSet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { + const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { // Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys - FastSet rootKeys; - FastSet allKeys(graph.keys()); + KeySet rootKeys; + KeySet allKeys(graph.keys()); std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end())); // Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys - FastSet marginalizeKeys; + KeySet marginalizeKeys; std::set_difference(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end())); if(marginalizeKeys.size() == 0) { diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index fb4b78fc2..b8a9941ad 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -152,7 +152,7 @@ namespace internal { /** Calculate the marginal on the specified keys, returning a set of LinearContainerFactors. * Unlike other GTSAM functions with similar purposes, this version can operate on disconnected graphs. */ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const FastSet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 9742aefd7..4c4442141 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -184,7 +184,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth previousSmootherSummarization_ = smootherSummarization; // Find the set of new separator keys - const FastSet& newSeparatorKeys = isam2_.getFixedVariables(); + const KeySet& newSeparatorKeys = isam2_.getFixedVariables(); // Use the shortcut to calculate an updated marginal on the current separator // Combine just the shortcut and the previousSmootherSummarization @@ -291,7 +291,7 @@ std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2 std::vector removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); BOOST_FOREACH(Key key, keys) { - const FastList& slots = variableIndex[key]; + const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } @@ -312,7 +312,7 @@ std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2 void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& removedFactors) { // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys - FastSet shortcutKeys; + KeySet shortcutKeys; BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { @@ -343,7 +343,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() // variables that result from marginalizing out all of the other variables // Find the set of current separator keys - const FastSet& separatorKeys = isam2_.getFixedVariables(); + const KeySet& separatorKeys = isam2_.getFixedVariables(); // Find all cliques that contain any separator variables std::set separatorCliques; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 16a336695..b87409aae 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -245,7 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { } // Get the set of separator keys - gtsam::FastSet separatorKeys; + gtsam::KeySet separatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { separatorKeys.insert(key_value.key); } diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp index 66d367148..369db51c3 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -37,7 +37,7 @@ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { // Loop through each key and add/update it in the map BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { - // Check to see if this key already exists inthe database + // Check to see if this key already exists in the database KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); // If the key already exists diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index b5556994c..23cd42a0a 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -25,10 +25,13 @@ namespace gtsam { /* ************************************************************************* */ -void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { +void recursiveMarkAffectedKeys(const Key& key, + const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { // Check if the separator keys of the current clique contain the specified key - if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { + if (std::find(clique->conditional()->beginParents(), + clique->conditional()->endParents(), key) + != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique BOOST_FOREACH(Key i, clique->conditional()->frontals()) { @@ -44,32 +47,36 @@ void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& cl } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { +void IncrementalFixedLagSmoother::print(const std::string& s, + const KeyFormatter& keyFormatter) const { FixedLagSmoother::print(s, keyFormatter); // TODO: What else to print? } /* ************************************************************************* */ -bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - const IncrementalFixedLagSmoother* e = dynamic_cast (&rhs); - return e != NULL - && FixedLagSmoother::equals(*e, tol) +bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, + double tol) const { + const IncrementalFixedLagSmoother* e = + dynamic_cast(&rhs); + return e != NULL && FixedLagSmoother::equals(*e, tol) && isam_.equals(e->isam_, tol); } /* ************************************************************************* */ -FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +FixedLagSmoother::Result IncrementalFixedLagSmoother::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const KeyTimestampMap& timestamps) { const bool debug = ISDEBUG("IncrementalFixedLagSmoother update"); - if(debug) { + if (debug) { std::cout << "IncrementalFixedLagSmoother::update() Start" << std::endl; PrintSymbolicTree(isam_, "Bayes Tree Before Update:"); std::cout << "END" << std::endl; } FastVector removedFactors; - boost::optional > constrainedKeys = boost::none; + boost::optional > constrainedKeys = boost::none; // Update the Timestamps associated with the factor keys updateKeyTimestampMap(timestamps); @@ -77,12 +84,14 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Get current timestamp double current_timestamp = getCurrentTimestamp(); - if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + if (debug) + std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); + std::set marginalizableKeys = findKeysBefore( + current_timestamp - smootherLag_); - if(debug) { + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizableKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -93,11 +102,13 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Force iSAM2 to put the marginalizable variables at the beginning createOrderingConstraints(marginalizableKeys, constrainedKeys); - if(debug) { + if (debug) { std::cout << "Constrained Keys: "; - if(constrainedKeys) { - for(FastMap::const_iterator iter = constrainedKeys->begin(); iter != constrainedKeys->end(); ++iter) { - std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second << ") "; + if (constrainedKeys) { + for (FastMap::const_iterator iter = constrainedKeys->begin(); + iter != constrainedKeys->end(); ++iter) { + std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second + << ") "; } } std::cout << std::endl; @@ -114,23 +125,26 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 - ISAM2Result isamResult = isam_.update(newFactors, newTheta, FastVector(), constrainedKeys, boost::none, additionalMarkedKeys); + ISAM2Result isamResult = isam_.update(newFactors, newTheta, + FastVector(), constrainedKeys, boost::none, additionalMarkedKeys); - if(debug) { - PrintSymbolicTree(isam_, "Bayes Tree After Update, Before Marginalization:"); + if (debug) { + PrintSymbolicTree(isam_, + "Bayes Tree After Update, Before Marginalization:"); std::cout << "END" << std::endl; } // Marginalize out any needed variables - if(marginalizableKeys.size() > 0) { - FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); + if (marginalizableKeys.size() > 0) { + FastList leafKeys(marginalizableKeys.begin(), + marginalizableKeys.end()); isam_.marginalizeLeaves(leafKeys); } // Remove marginalized keys from the KeyTimestampMap eraseKeyTimestampMap(marginalizableKeys); - if(debug) { + if (debug) { PrintSymbolicTree(isam_, "Final Bayes Tree:"); std::cout << "END" << std::endl; } @@ -142,7 +156,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact result.nonlinearVariables = 0; result.error = 0; - if(debug) std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl; + if (debug) + std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl; return result; } @@ -151,30 +166,33 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) { TimestampKeyMap::iterator end = timestampKeyMap_.lower_bound(timestamp); TimestampKeyMap::iterator iter = timestampKeyMap_.begin(); - while(iter != end) { + while (iter != end) { keyTimestampMap_.erase(iter->second); timestampKeyMap_.erase(iter++); } } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::createOrderingConstraints(const std::set& marginalizableKeys, boost::optional >& constrainedKeys) const { - if(marginalizableKeys.size() > 0) { - constrainedKeys = FastMap(); +void IncrementalFixedLagSmoother::createOrderingConstraints( + const std::set& marginalizableKeys, + boost::optional >& constrainedKeys) const { + if (marginalizableKeys.size() > 0) { + constrainedKeys = FastMap(); // Generate ordering constraints so that the marginalizable variables will be eliminated first // Set all variables to Group1 BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { constrainedKeys->operator[](timestamp_key.second) = 1; } // Set marginalizable variables to Group0 - BOOST_FOREACH(Key key, marginalizableKeys){ + BOOST_FOREACH(Key key, marginalizableKeys) { constrainedKeys->operator[](key) = 0; } } } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -183,7 +201,8 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const s } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { +void IncrementalFixedLagSmoother::PrintSymbolicFactor( + const GaussianFactor::shared_ptr& factor) { std::cout << "f("; BOOST_FOREACH(gtsam::Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -192,7 +211,8 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shar } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicGraph( + const GaussianFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); @@ -200,27 +220,28 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, + const std::string& label) { std::cout << label << std::endl; - if(!isam.roots().empty()) - { - BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()){ - PrintSymbolicTreeHelper(root); + if (!isam.roots().empty()) { + BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) { + PrintSymbolicTreeHelper(root); } - } - else + } else std::cout << "{Empty Tree}" << std::endl; } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { +void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper( + const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { // Print the current clique std::cout << indent << "P( "; BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) { std::cout << gtsam::DefaultKeyFormatter(key) << " "; } - if(clique->conditional()->nrParents() > 0) std::cout << "| "; + if (clique->conditional()->nrParents() > 0) + std::cout << "| "; BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) { std::cout << gtsam::DefaultKeyFormatter(key) << " "; } @@ -228,7 +249,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Cliq // Recursively print all of the children BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { - PrintSymbolicTreeHelper(child, indent+" "); + PrintSymbolicTreeHelper(child, indent + " "); } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 9f015ef19..31ae20c50 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -23,7 +23,6 @@ #include #include - namespace gtsam { /** @@ -31,7 +30,7 @@ namespace gtsam { * such that the active states are placed in/near the root. This base class implements a function * to calculate the ordering, and an update function to incorporate new factors into the HMF. */ -class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoother { +class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother { public: @@ -39,20 +38,30 @@ public: typedef boost::shared_ptr shared_ptr; /** default constructor */ - IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters = ISAM2Params()) - : FixedLagSmoother(smootherLag), isam_(parameters) {} + IncrementalFixedLagSmoother(double smootherLag = 0.0, + const ISAM2Params& parameters = ISAM2Params()) : + FixedLagSmoother(smootherLag), isam_(parameters) { + } /** destructor */ - virtual ~IncrementalFixedLagSmoother() {} + virtual ~IncrementalFixedLagSmoother() { + } /** Print the factor for debugging and testing (implementing Testable) */ - virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two IncrementalFixedLagSmoother Objects are equal */ virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; - /** Add new factors, updating the solution and relinearizing as needed. */ - Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + /** + * Add new factors, updating the solution and re-linearizing as needed. + * @param newFactors new factors on old and/or new variables + * @param newTheta new values for new variables only + * @param timestamps an (optional) map from keys to real time stamps + */ + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + const Values& newTheta = Values(), // const KeyTimestampMap& timestamps = KeyTimestampMap()); /** Compute an estimate from the incomplete linear delta computed during the last update. @@ -94,6 +103,11 @@ public: return isam_.getDelta(); } + /// Calculate marginal covariance on given variable + Matrix marginalCovariance(Key key) const { + return isam_.marginalCovariance(key); + } + protected: /** An iSAM2 object used to perform inference. The smoother lag is controlled * by what factors are removed each iteration */ @@ -103,16 +117,23 @@ protected: void eraseKeysBefore(double timestamp); /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */ - void createOrderingConstraints(const std::set& marginalizableKeys, boost::optional >& constrainedKeys) const; + void createOrderingConstraints(const std::set& marginalizableKeys, + boost::optional >& constrainedKeys) const; private: /** Private methods for printing debug information */ - static void PrintKeySet(const std::set& keys, const std::string& label = "Keys:"); + static void PrintKeySet(const std::set& keys, const std::string& label = + "Keys:"); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); - static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label = "Factor Graph:"); - static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); - static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = ""); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, + const std::string& label = "Factor Graph:"); + static void PrintSymbolicTree(const gtsam::ISAM2& isam, + const std::string& label = "Bayes Tree:"); + static void PrintSymbolicTreeHelper( + const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = + ""); -}; // IncrementalFixedLagSmoother +}; +// IncrementalFixedLagSmoother -} /// namespace gtsam +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 72c7c66f5..c6710bd70 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -62,7 +62,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("LinearizedGaussianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(lin_points_); @@ -149,7 +149,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("LinearizedJacobianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(Ab_); @@ -264,7 +264,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("LinearizedHessianFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(info_); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 51c2a55a2..558654367 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -559,7 +559,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering... GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues); - FastSet eliminateKeys = linearFactors->keys(); + KeySet eliminateKeys = linearFactors->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { eliminateKeys.erase(key_value.key); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 5f91527e6..22dd0ccaa 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -579,7 +579,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); + KeySet allkeys = LinFactorGraph->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { allkeys.erase(key_value.key); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 9708eedb5..c372577ca 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -581,7 +581,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); + KeySet allkeys = LinFactorGraph->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) allkeys.erase(key_value.key); std::vector variables(allkeys.begin(), allkeys.end()); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp deleted file mode 100644 index 7c2f9d9b9..000000000 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ /dev/null @@ -1,247 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testExpressionMeta.cpp - * @date October 14, 2014 - * @author Frank Dellaert - * @brief Test meta-programming constructs for Expressions - */ - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -namespace mpl = boost::mpl; - -#include -#include -template struct Incomplete; - -// Check generation of FunctionalNode -typedef mpl::vector MyTypes; -typedef FunctionalNode::type Generated; -//Incomplete incomplete; - -// Try generating vectors of ExecutionTrace -typedef mpl::vector, ExecutionTrace > ExpectedTraces; - -typedef mpl::transform >::type MyTraces; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >)); - -template -struct MakeTrace { - typedef ExecutionTrace type; -}; -typedef mpl::transform >::type MyTraces1; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >)); - -// Try generating vectors of Expression types -typedef mpl::vector, Expression > ExpectedExpressions; -typedef mpl::transform >::type Expressions; -BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >)); - -// Try generating vectors of Jacobian types -typedef mpl::vector ExpectedJacobians; -typedef mpl::transform >::type Jacobians; -BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >)); - -// Try accessing a Jacobian -typedef mpl::at_c::type Jacobian23; // base zero ! -BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>)); - -/* ************************************************************************* */ -// Boost Fusion includes allow us to create/access values from MPL vectors -#include -#include - -// Create a value and access it -TEST(ExpressionFactor, JacobiansValue) { - using boost::fusion::at_c; - Matrix23 expected; - Jacobians jacobians; - - at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6; - - Matrix23 actual = at_c<1>(jacobians); - CHECK(actual.cols() == expected.cols()); - CHECK(actual.rows() == expected.rows()); -} - -/* ************************************************************************* */ -// Test out polymorphic transform -#include -#include -#include - -struct triple { - template struct result; // says we will provide result - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - // actual function - template - typename result::type operator()(const T& x) const { - return (double) x; - } -}; - -// Test out polymorphic transform -TEST(ExpressionFactor, Triple) { - typedef boost::fusion::vector IntDouble; - IntDouble H = boost::fusion::make_vector(1, 2.0); - - // Only works if I use Double2 - typedef boost::fusion::result_of::transform::type Result; - typedef boost::fusion::vector Double2; - Double2 D = boost::fusion::transform(H, triple()); - - using boost::fusion::at_c; - DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9); - DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9); -} - -/* ************************************************************************* */ -#include -#include -#include -#include - -// Test out invoke -TEST(ExpressionFactor, Invoke) { - EXPECT_LONGS_EQUAL(2, invoke(plus(),boost::fusion::make_vector(1,1))); - - // Creating a Pose3 (is there another way?) - boost::fusion::vector pair; - Pose3 pose = boost::fusion::invoke(boost::value_factory(), pair); -} - -/* ************************************************************************* */ -// debug const issue (how to make read/write arguments for invoke) -struct test { - typedef void result_type; - void operator()(int& a, int& b) const { - a = 6; - b = 7; - } -}; - -TEST(ExpressionFactor, ConstIssue) { - int a, b; - boost::fusion::invoke(test(), - boost::fusion::make_vector(boost::ref(a), boost::ref(b))); - LONGS_EQUAL(6, a); - LONGS_EQUAL(7, b); -} - -/* ************************************************************************* */ -// Test out invoke on a given GTSAM function -// then construct prototype for it's derivatives -TEST(ExpressionFactor, InvokeDerivatives) { - // This is the method in Pose3: - // Point3 transform_to(const Point3& p) const; - // Point3 transform_to(const Point3& p, - // boost::optional Dpose, boost::optional Dpoint) const; - - // Let's assign it it to a boost function object - // cast is needed because Pose3::transform_to is overloaded -// typedef boost::function F; -// F f = static_cast(&Pose3::transform_to); -// -// // Create arguments -// Pose3 pose; -// Point3 point; -// typedef boost::fusion::vector Arguments; -// Arguments args = boost::fusion::make_vector(pose, point); -// -// // Create fused function (takes fusion vector) and call it -// boost::fusion::fused g(f); -// Point3 actual = g(args); -// CHECK(assert_equal(point,actual)); -// -// // We can *immediately* do this using invoke -// Point3 actual2 = boost::fusion::invoke(f, args); -// CHECK(assert_equal(point,actual2)); - - // Now, let's create the optional Jacobian arguments - typedef Point3 T; - typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; - - // Unfortunately this is moot: we need a pointer to a function with the - // optional derivatives; I don't see a way of calling a function that we - // did not get access to by the caller passing us a pointer. - // Let's test below whether we can have a proxy object -} - -struct proxy { - typedef Point3 result_type; - Point3 operator()(const Pose3& pose, const Point3& point) const { - return pose.transform_to(point); - } - Point3 operator()(const Pose3& pose, const Point3& point, - OptionalJacobian<3,6> Dpose, - OptionalJacobian<3,3> Dpoint) const { - return pose.transform_to(point, Dpose, Dpoint); - } -}; - -TEST(ExpressionFactor, InvokeDerivatives2) { - // without derivatives - Pose3 pose; - Point3 point; - Point3 actual = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point)); - CHECK(assert_equal(point,actual)); - - // with derivatives, does not work, const issue again - Matrix36 Dpose; - Matrix3 Dpoint; - Point3 actual2 = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point, boost::ref(Dpose), - boost::ref(Dpoint))); - CHECK(assert_equal(point,actual2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 832d37d14..0b86a60e9 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -17,19 +17,20 @@ */ -#include #include -#include -#include -#include -#include -#include -#include #include #include +#include #include #include +#include +#include +#include +#include #include +#include + +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index dcf43c569..56b1269f0 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -417,7 +417,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index c3f3f1d10..6f39ce1b6 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -95,7 +95,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 614521e76..79a37c2ff 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -652,7 +652,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 8cf22946a..8216942cd 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -574,7 +574,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 857c07761..7f9564ee3 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -114,7 +114,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(dt_); ar & BOOST_SERIALIZATION_NVP(tau_); diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 6b111b759..d8fceeb68 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -386,7 +386,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index efa7f5f7d..ac481f979 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -114,7 +114,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 2998fdc9b..2fd964a35 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -132,7 +132,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 154afbdff..879e1e1dd 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -140,7 +140,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 2e2d1e310..feff0b62c 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -131,7 +131,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); @@ -253,7 +253,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 088bfd2ea..f1487f562 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -70,7 +70,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, - FastSet poseKeys, Key pointKey, const boost::shared_ptr& K, + KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, boost::optional body_P_sensor = boost::none) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), verboseCheirality_(false) { @@ -91,7 +91,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, - FastSet poseKeys, Key pointKey, const boost::shared_ptr& K, + KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality, boost::optional body_P_sensor = boost::none) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), @@ -216,7 +216,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 5f6d0ef92..e3080466f 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -136,7 +136,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 87b9f4f5c..d2a9110d9 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -120,7 +120,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 991aae1fd..2de060302 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -101,7 +101,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 0426c3ba4..5b1540c83 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -170,7 +170,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 5906a6664..069274065 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -161,7 +161,7 @@ namespace gtsam { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(throwCheirality_); diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index d331053b6..407652583 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -65,7 +65,7 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 5d2ba3323..f4e82d98d 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -33,50 +33,25 @@ namespace gtsam { -/** - * Structure for storing some state memory, used to speed up optimization - * @addtogroup SLAM - */ -class SmartStereoProjectionFactorState { - -protected: - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - SmartStereoProjectionFactorState() { - } - // Hessian representation (after Schur complement) - bool calculatedHessian; - Matrix H; - Vector gs_vector; - std::vector Gs; - std::vector gs; - double f; -}; - -enum LinearizationMode { - HESSIAN, JACOBIAN_SVD, JACOBIAN_Q -}; - /** * SmartStereoProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? */ -template -class SmartStereoProjectionFactor: public SmartFactorBase { +template +class SmartStereoProjectionFactor: public SmartFactorBase { protected: - // Some triangulation parameters - const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ + /// @name Caching triangulation + /// @{ + const TriangulationParameters parameters_; + // TODO: +// mutable TriangulationResult result_; ///< result from triangulateSafe + const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses + /// @} const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases - const bool enableEPI_; ///< if set to true, will refine triangulation using LM - const double linearizationThreshold_; ///< threshold to decide whether to re-linearize mutable std::vector cameraPosesLinearization_; ///< current linearization poses @@ -85,29 +60,22 @@ protected: mutable bool degenerate_; mutable bool cheiralityException_; - // verbosity handling for Cheirality Exceptions - const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) - - boost::shared_ptr state_; - - /// shorthand for smart projection factor state variable - typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; - - double landmarkDistanceThreshold_; // if the landmark is triangulated at a - // distance larger than that the factor is considered degenerate - - double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the - // average reprojection error is smaller than this threshold after triangulation, - // and the factor is disregarded if the error is large + typedef SmartFactorBase Base; /// shorthand for this class - typedef SmartStereoProjectionFactor This; + typedef SmartStereoProjectionFactor This; - enum {ZDim = 3}; ///< Dimension trait of measurement type + enum { + ZDim = 3 + }; ///< Dimension trait of measurement type + + /// @name Parameters governing how triangulation result is treated + /// @{ + const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} public: @@ -118,7 +86,7 @@ public: typedef StereoCamera Camera; /// Vector of cameras - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor @@ -129,18 +97,15 @@ public: * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations * @param body_P_sensor is the transform from body to sensor frame (default identity) */ - SmartStereoProjectionFactor(const double rankTol, const double linThreshold, - const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, - double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : - Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( - 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( + SmartStereoProjectionFactor(const double rankTolerance, + const double linThreshold, const bool manageDegeneracy, + const bool enableEPI, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold, + dynamicOutlierRejectionThreshold), // + retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state), - landmarkDistanceThreshold_(landmarkDistanceThreshold), - dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { + false), verboseCheirality_(false) { } /** Virtual destructor */ @@ -154,11 +119,12 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "SmartStereoProjectionFactor, z = \n"; - std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; + std::cout << s << "SmartStereoProjectionFactor\n"; + std::cout << "triangulationParameters:\n" << parameters_ << std::endl; std::cout << "degenerate_ = " << degenerate_ << std::endl; std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; - std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << std::endl; + std::cout << "linearizationThreshold_ = " << linearizationThreshold_ + << std::endl; Base::print("", keyFormatter); } @@ -198,40 +164,6 @@ public: return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation } - /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization - bool decideIfLinearize(const Cameras& cameras) const { - // "selective linearization" - // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose - // (we only care about the "rigidity" of the poses, not about their absolute pose) - - if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize - return true; - - // if we do not have a previous linearization point_ or the new linearization point_ includes more poses - if (cameraPosesLinearization_.empty() - || (cameras.size() != cameraPosesLinearization_.size())) - return true; - - Pose3 firstCameraPose, firstCameraPoseOld; - for (size_t i = 0; i < cameras.size(); i++) { - - if (i == 0) { // we store the initial pose, this is useful for selective re-linearization - firstCameraPose = cameras[i].pose(); - firstCameraPoseOld = cameraPosesLinearization_[i]; - continue; - } - - // we compare the poses in the frame of the first pose - Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); - Pose3 localCameraPoseOld = firstCameraPoseOld.between( - cameraPosesLinearization_[i]); - if (!localCameraPose.equals(localCameraPoseOld, - this->linearizationThreshold_)) - return true; // at least two "relative" poses are different, hence we re-linearize - } - return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize - } - /// triangulateSafe size_t triangulateSafe(const Values& values) const { return triangulateSafe(this->cameras(values)); @@ -268,8 +200,8 @@ public: const Cal3_S2& K = camera.calibration()->calibration(); mono_cameras.push_back(PinholeCamera(pose, K)); } - point_ = triangulatePoint3(mono_cameras, mono_measurements, - rankTolerance_, enableEPI_); + point_ = triangulatePoint3 >(mono_cameras, mono_measurements, + parameters_.rankTolerance, parameters_.enableEPI); // // // End temporary hack @@ -282,11 +214,11 @@ public: // Check landmark distance and reprojection errors to avoid outliers double totalReprojError = 0.0; - size_t i=0; + size_t i = 0; BOOST_FOREACH(const Camera& camera, cameras) { Point3 cameraTranslation = camera.pose().translation(); // we discard smart factors corresponding to points that are far away - if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) { degenerate_ = true; break; } @@ -301,8 +233,8 @@ public: } //std::cout << "totalReprojError error: " << totalReprojError << std::endl; // we discard smart factors that have large reprojection error - if(dynamicOutlierRejectionThreshold_ > 0 && - totalReprojError/m > dynamicOutlierRejectionThreshold_) + if (parameters_.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold) degenerate_ = true; } catch (TriangulationUnderconstrainedException&) { @@ -346,15 +278,15 @@ public: } /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + boost::shared_ptr > createHessianFactor( const Cameras& cameras, const double lambda = 0.0) const { bool isDebug = false; size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors - std::vector < Key > js; - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + std::vector js; + std::vector Gs(numKeys * (numKeys + 1) / 2); + std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) { std::cout @@ -363,139 +295,123 @@ public: exit(1); } - this->triangulateSafe(cameras); - if (isDebug) std::cout << "point_ = " << point_ << std::endl; + triangulateSafe(cameras); + if (isDebug) + std::cout << "point_ = " << point_ << std::endl; if (numKeys < 2 || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { - if (isDebug) std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) - m = zeros(D, D); + if (isDebug) + std::cout << "In linearize: exception" << std::endl; + BOOST_FOREACH(Matrix& m, Gs) + m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(D); - return boost::make_shared >(this->keys_, Gs, gs, + v = zero(Base::Dim); + return boost::make_shared >(this->keys_, Gs, gs, 0.0); } // instead, if we want to manage the exception.. if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors this->degenerate_ = true; - if (isDebug) std::cout << "degenerate_ = true" << std::endl; + if (isDebug) + std::cout << "degenerate_ = true" << std::endl; } - bool doLinearize = this->decideIfLinearize(cameras); - - if (isDebug) std::cout << "doLinearize = " << doLinearize << std::endl; - - if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize + if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize for (size_t i = 0; i < cameras.size(); i++) this->cameraPosesLinearization_[i] = cameras[i].pose(); - if (!doLinearize) { // return the previous Hessian factor - std::cout << "=============================" << std::endl; - std::cout << "doLinearize " << doLinearize << std::endl; - std::cout << "this->linearizationThreshold_ " - << this->linearizationThreshold_ << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; - std::cout - << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" - << std::endl; - exit(1); - return boost::make_shared >(this->keys_, - this->state_->Gs, this->state_->gs, this->state_->f); - } - // ================================================================== + std::vector Fblocks; Matrix F, E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + computeJacobians(Fblocks, E, b, cameras); + Base::FillDiagonalF(Fblocks, F); // expensive !!! // Schur complement trick // Frank says: should be possible to do this more efficiently? // And we care, as in grouped factors this is called repeatedly - Matrix H(D * numKeys, D * numKeys); + Matrix H(Base::Dim * numKeys, Base::Dim * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + Matrix3 P = Cameras::PointCov(E, lambda); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); - if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl; - if (isDebug) std::cout << "H:\n" << H << std::endl; + if (isDebug) + std::cout << "gs_vector size " << gs_vector.size() << std::endl; + if (isDebug) + std::cout << "H:\n" << H << std::endl; // Populate Gs and gs int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment < D > (i1D); - for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { + for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera + DenseIndex i1D = i1 * Base::Dim; + gs.at(i1) = gs_vector.segment(i1D); + for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) { if (i2 >= i1) { - Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); + Gs.at(GsCount2) = H.block(i1D, i2 * Base::Dim); GsCount2++; } } } // ================================================================== - if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables - this->state_->Gs = Gs; - this->state_->gs = gs; - this->state_->f = f; - } - return boost::make_shared >(this->keys_, Gs, gs, f); + double f = b.squaredNorm(); + return boost::make_shared >(this->keys_, Gs, gs, f); } // // create factor -// boost::shared_ptr > createImplicitSchurFactor( +// boost::shared_ptr > createImplicitSchurFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) // return Base::createImplicitSchurFactor(cameras, point_, lambda); // else -// return boost::shared_ptr >(); +// return boost::shared_ptr >(); // } // // /// create factor -// boost::shared_ptr > createJacobianQFactor( +// boost::shared_ptr > createJacobianQFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) // return Base::createJacobianQFactor(cameras, point_, lambda); // else -// return boost::make_shared< JacobianFactorQ >(this->keys_); +// return boost::make_shared< JacobianFactorQ >(this->keys_); // } // // /// Create a factor, takes values -// boost::shared_ptr > createJacobianQFactor( +// boost::shared_ptr > createJacobianQFactor( // const Values& values, double lambda) const { -// Cameras myCameras; +// Cameras cameras; // // TODO triangulate twice ?? -// bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); +// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); // if (nonDegenerate) -// return createJacobianQFactor(myCameras, lambda); +// return createJacobianQFactor(cameras, lambda); // else -// return boost::make_shared< JacobianFactorQ >(this->keys_); +// return boost::make_shared< JacobianFactorQ >(this->keys_); // } // /// different (faster) way to compute Jacobian factor - boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, - double lambda) const { + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared >(this->keys_); } /// Returns true if nonDegenerate bool computeCamerasAndTriangulate(const Values& values, - Cameras& myCameras) const { + Cameras& cameras) const { Values valuesFactor; // Select only the cameras BOOST_FOREACH(const Key key, this->keys_) valuesFactor.insert(key, values.at(key)); - myCameras = this->cameras(valuesFactor); - size_t nrCameras = this->triangulateSafe(myCameras); + cameras = this->cameras(valuesFactor); + size_t nrCameras = this->triangulateSafe(cameras); if (nrCameras < 2 || (!this->manageDegeneracy_ @@ -507,7 +423,8 @@ public: this->degenerate_ = true; if (this->degenerate_) { - std::cout << "SmartStereoProjectionFactor: this is not ready" << std::endl; + std::cout << "SmartStereoProjectionFactor: this is not ready" + << std::endl; std::cout << "this->cheiralityException_ " << this->cheiralityException_ << std::endl; std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; @@ -515,34 +432,32 @@ public: return true; } - /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Values& values) const { + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + cameras.project2(point_, boost::none, E); return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); - } - /// Version that takes values, and creates the point - bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + bool computeJacobians(std::vector& Fblocks, + Matrix& E, Vector& b, const Values& values) const { + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, cameras, 0.0); return nonDegenerate; } /// Compute F, E only (called below in both vanilla and SVD versions) /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate - double computeJacobians(std::vector& Fblocks, + void computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras) const { if (this->degenerate_) { throw("FIXME: computeJacobians degenerate case commented out!"); @@ -573,73 +488,36 @@ public: // // this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); // f += bi.squaredNorm(); -// Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); +// Fblocks.push_back(typename Base::MatrixZD(this->keys_[i], Fi)); // E.block < 2, 2 > (2 * i, 0) = Ei; // subInsert(b, bi, 2 * i); // } // return f; } else { // nondegenerate: just return Base version - return Base::computeJacobians(Fblocks, E, b, cameras, point_); + Base::computeJacobians(Fblocks, E, b, cameras, point_); } // end else } -// /// Version that computes PointCov, with optional lambda parameter -// double computeJacobians(std::vector& Fblocks, -// Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, -// const double lambda = 0.0) const { -// -// double f = computeJacobians(Fblocks, E, b, cameras); -// -// // Point covariance inv(E'*E) -// PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); -// -// return f; -// } -// -// /// takes values -// bool computeJacobiansSVD(std::vector& Fblocks, -// Matrix& Enull, Vector& b, const Values& values) const { -// typename Base::Cameras myCameras; -// double good = computeCamerasAndTriangulate(values, myCameras); -// if (good) -// computeJacobiansSVD(Fblocks, Enull, b, myCameras); -// return true; -// } -// -// /// SVD version -// double computeJacobiansSVD(std::vector& Fblocks, -// Matrix& Enull, Vector& b, const Cameras& cameras) const { -// return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); -// } -// -// /// Returns Matrix, TODO: maybe should not exist -> not sparse ! -// // TODO should there be a lambda? -// double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, -// const Cameras& cameras) const { -// return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); -// } - - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + /// takes values + bool triangulateAndComputeJacobiansSVD( + std::vector& Fblocks, Matrix& Enull, Vector& b, + const Values& values) const { + typename Base::Cameras cameras; + double good = computeCamerasAndTriangulate(values, cameras); + if (good) + return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); + return true; } /// Calculate vector of re-projection errors, before applying noise model - /// Assumes triangulation was done and degeneracy handled - Vector reprojectionError(const Cameras& cameras) const { - return Base::reprojectionError(cameras, point_); - } - - /// Calculate vector of re-projection errors, before applying noise model - Vector reprojectionError(const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + Vector reprojectionErrorAfterTriangulation(const Values& values) const { + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - return reprojectionError(myCameras); + return Base::unwhitenedError(cameras, point_); else - return zero(myCameras.size() * 3); + return zero(cameras.size() * 3); } /** @@ -677,7 +555,7 @@ public: } if (this->degenerate_) { - return 0.0; // TODO: this maybe should be zero? + return 0.0; // TODO: this maybe should be zero? // std::cout // << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!" // << std::endl; @@ -738,7 +616,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(throwCheirality_); ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); @@ -746,9 +624,9 @@ private: }; /// traits -template -struct traits > : - public Testable > { +template +struct traits > : public Testable< + SmartStereoProjectionFactor > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 2e3bc866b..bc4d3ccfb 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -15,6 +15,7 @@ * @author Luca Carlone * @author Chris Beall * @author Zsolt Kira + * @author Frank Dellaert */ #pragma once @@ -37,8 +38,15 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +template +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { + +public: + /// Linearization mode: what factor to linearize to + enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD + }; + protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -50,10 +58,10 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartStereoProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionPoseFactor This; + typedef SmartStereoProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -65,15 +73,16 @@ public: * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from body to sensor frame (default identity) */ SmartStereoProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN, + double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, - landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_( + linearizeTo) { + } /** Virtual destructor */ virtual ~SmartStereoProjectionPoseFactor() {} @@ -143,11 +152,6 @@ public: return e && Base::equals(p, tol); } - /// get the dimension of the factor - virtual size_t dim() const { - return 6 * this->keys_.size(); - } - /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding @@ -208,7 +212,7 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_all_); } @@ -216,9 +220,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : - public Testable > { +template +struct traits > : public Testable< + SmartStereoProjectionPoseFactor > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 2abc49fa1..156ac242e 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -217,7 +217,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); //ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index adfb9ae36..bf10ae531 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -415,7 +415,7 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); //ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 31995e769..a598fb689 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -5,6 +5,8 @@ * @author Alex Cunningham */ +#include +#include #include #include @@ -22,9 +24,6 @@ #include #include #include -#include -#include -//#include #include #include #include diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index ff7307840..a86306a8d 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -20,9 +20,8 @@ #include #include #include -#include - #include +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 20e96e6bf..99a4f90a4 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -73,9 +73,8 @@ TEST( InvDepthFactorVariant1, optimize) { // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - Vector6 actual = result.at(landmarkKey); - +// Vector6 actual = result.at(landmarkKey); // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); // pose1.print("Pose1 Truth:\n"); @@ -116,8 +115,11 @@ TEST( InvDepthFactorVariant1, optimize) { // However, since this is an over-parameterization, there can be // many 6D landmark values that equate to the same 3D world position // Instead, directly test the recovered 3D landmark position - //EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(landmark, world_landmarkAfter, 1e-9)); + + // Frank asks: why commented out? + //Vector6 actual = result.at(landmarkKey); + //EXPECT(assert_equal(expected, actual, 1e-9)); } diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index 2794e5707..ca91d4cb5 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -69,7 +69,7 @@ TEST( MultiProjectionFactor, create ){ n_measPixel << 10, 10, 10, 10, 10, 10; const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - FastSet views; + KeySet views; views.insert(x1); views.insert(x2); views.insert(x3); diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4691da384..258c8d0eb 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,8 +18,8 @@ * @date Sept 2013 */ +// TODO #include #include - #include #include #include @@ -32,42 +32,41 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = true; - // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static size_t w = 640, h = 480; static double b = 1; -static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b)); -static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); +static Cal3_S2Stereo::shared_ptr K2( + new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); +static boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); static double rankTol = 1.0; static double linThreshold = -1.0; // static bool manageDegeneracy = true; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(3)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; // tests data -static Symbol x1('X', 1); -static Symbol x2('X', 2); -static Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); static Key poseKey1(x1); static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); -typedef SmartStereoProjectionPoseFactor SmartFactor; -typedef SmartStereoProjectionPoseFactor SmartFactorBundler; +typedef SmartStereoProjectionPoseFactor SmartFactor; -vector stereo_projectToMultipleCameras( - const StereoCamera& cam1, const StereoCamera& cam2, - const StereoCamera& cam3, Point3 landmark){ +vector stereo_projectToMultipleCameras(const StereoCamera& cam1, + const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { vector measurements_cam; @@ -81,9 +80,10 @@ vector stereo_projectToMultipleCameras( return measurements_cam; } +LevenbergMarquardtParams params; + /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - fprintf(stderr,"Test 1 Complete"); SmartFactor::shared_ptr factor1(new SmartFactor()); } @@ -104,14 +104,6 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) { factor1.add(measurement1, poseKey1, model, K); } -/* ************************************************************************* */ -TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { - bool manageDegeneracy = true; - bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); - factor1.add(measurement1, poseKey1, model, K); -} - /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Equals ) { SmartFactor::shared_ptr factor1(new SmartFactor()); @@ -124,15 +116,14 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) { } /* *************************************************************************/ -TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; - +TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); // landmark ~5 meters infront of camera @@ -164,75 +155,72 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, noisy ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; - +TEST( SmartStereoProjectionPoseFactor, noisy ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate - StereoPoint2 pixelError(0.2,0.2,0); + StereoPoint2 pixelError(0.2, 0.2, 0); StereoPoint2 level_uv = level_camera.project(landmark) + pixelError; StereoPoint2 level_uv_right = level_camera_right.project(landmark); Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); SmartFactor::shared_ptr factor1(new SmartFactor()); factor1->add(level_uv, x1, model, K); factor1->add(level_uv_right, x2, model, K); - double actualError1= factor1->error(values); + double actualError1 = factor1->error(values); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); factor2->add(measurements, views, noises, Ks); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ - cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; - +TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K2); // three landmarks ~5 meters infront of camera @@ -241,11 +229,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -268,18 +259,22 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1); Values result; gttic_(SmartStereoProjectionPoseFactor); @@ -288,32 +283,33 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ gttoc_(SmartStereoProjectionPoseFactor); tictoc_finishedIteration_(); -// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); -// VectorValues delta = GFG->optimize(); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6); + + GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); + VectorValues delta = GFG->optimize(); + VectorValues expected = VectorValues::Zero(delta); + EXPECT(assert_equal(expected, delta, 1e-6)); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + EXPECT(assert_equal(pose3, result.at(x3))); } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ +TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -322,17 +318,23 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -344,38 +346,38 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); - LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ +TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -384,18 +386,26 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -407,40 +417,40 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -450,28 +460,35 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ Point3 landmark4(5, -0.5, 1); // 1. Project four landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark4); + measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier - measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -484,30 +501,30 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // create second camera 1 meter to the right of first camera // Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); @@ -528,13 +545,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor1->add(measurements_cam1, views, model, K); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor2->add(measurements_cam2, views, model, K); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor3->add(measurements_cam3, views, model, K); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -546,15 +563,14 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); // values.insert(x3, pose3*noise_pose); // -// LevenbergMarquardtParams params; -// Values result; +//// Values result; // LevenbergMarquardtOptimizer optimizer(graph, values, params); // result = optimizer.optimize(); // EXPECT(assert_equal(pose3,result.at(x3))); @@ -562,15 +578,14 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ -// // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -606,7 +621,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -614,36 +629,31 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // values.insert(L(1), landmark1); // values.insert(L(2), landmark2); // values.insert(L(3), landmark3); -// if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); // -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // LevenbergMarquardtOptimizer optimizer(graph, values, params); // Values result = optimizer.optimize(); // -// if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); // EXPECT(assert_equal(pose3,result.at(x3))); //} // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, CheckHessian){ +TEST( SmartStereoProjectionPoseFactor, CheckHessian) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + // create second camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + // create third camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -651,12 +661,15 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ Point3 landmark2(5, -0.5, 1.2); Point3 landmark3(3, 0, 3.0); - // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - + // Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + // Create smartfactors double rankTol = 10; SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); @@ -668,38 +681,44 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); smartFactor3->add(measurements_cam3, views, model, K); + // Create graph to optimize NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + values.insert(x3, pose3 * noise_pose); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + // TODO: next line throws Cheirality exception on Mac + boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = hessianFactor1->information() + + hessianFactor2->information() + hessianFactor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize(values); + boost::shared_ptr GaussianGraph = graph.linearize( + values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + + hessianFactor3->augmentedInformation(); // Check Information vector - // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); @@ -707,15 +726,14 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -745,7 +763,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); -// Point3 positionPrior = gtsam::Point3(0,0,1); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -754,17 +772,12 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2*noise_pose); // // initialize third pose with some noise, we expect it to move back to original pose3 // values.insert(x3, pose3*noise_pose*noise_pose); -// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); -// -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // // Values result; // gttic_(SmartStereoProjectionPoseFactor); @@ -774,23 +787,19 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // tictoc_finishedIteration_(); // // // result.print("results of 3 camera, 3 landmark optimization \n"); -// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; // // EXPECT(assert_equal(pose3,result.at(x3))); -// if(isDebugTest) tictoc_print_(); //} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // // create second camera 1 meter to the right of first camera @@ -826,7 +835,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); -// Point3 positionPrior = gtsam::Point3(0,0,1); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -836,18 +845,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); // // initialize third pose with some noise, we expect it to move back to original pose3 // values.insert(x3, pose3*noise_pose); -// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); -// -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // // Values result; // gttic_(SmartStereoProjectionPoseFactor); @@ -857,22 +861,18 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // tictoc_finishedIteration_(); // // // result.print("results of 3 camera, 3 landmark optimization \n"); -// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; // // EXPECT(assert_equal(pose3,result.at(x3))); -// if(isDebugTest) tictoc_print_(); //} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, Hessian ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -892,13 +892,12 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // SmartFactor::shared_ptr smartFactor1(new SmartFactor()); // smartFactor1->add(measurements_cam1,views, model, K2); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); // // boost::shared_ptr hessianFactor = smartFactor1->linearize(values); -// if(isDebugTest) hessianFactor->print("Hessian factor \n"); // // // compute triangulation from linearization point // // compute reprojection errors (sum squared) @@ -908,29 +907,28 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; - - std::vector views; +TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); smartFactorInstance->add(measurements_cam1, views, model, K); @@ -940,46 +938,52 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + boost::shared_ptr hessianFactor = + smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + boost::shared_ptr hessianFactorRot = + smartFactorInstance->linearize(rotValues); // hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-6)); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - - std::vector views; +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // Second and third cameras in same place, which is a degenerate configuration @@ -990,49 +994,56 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); SmartFactor::shared_ptr smartFactor(new SmartFactor()); smartFactor->add(measurements_cam1, views, model, K2); - Values values; values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor->linearize( + values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize( + rotValues); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-8)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactor->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-8)); } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/matlab.h b/matlab.h index 349cdeea4..4a9ac2309 100644 --- a/matlab.h +++ b/matlab.h @@ -71,16 +71,16 @@ FastVector createKeyVector(string s, const Vector& I) { } // Create a KeySet from indices -FastSet createKeySet(const Vector& I) { - FastSet set; +KeySet createKeySet(const Vector& I) { + KeySet set; for (int i = 0; i < I.size(); i++) set.insert(I[i]); return set; } // Create a KeySet from indices using symbol -FastSet createKeySet(string s, const Vector& I) { - FastSet set; +KeySet createKeySet(string s, const Vector& I) { + KeySet set; char c = s[0]; for (int i = 0; i < I.size(); i++) set.insert(symbol(c, I[i])); @@ -229,7 +229,7 @@ Values localToWorld(const Values& local, const Pose2& base, // if no keys given, get all keys from local values FastVector keys(user_keys); if (keys.size()==0) - keys = FastVector(local.keys()); + keys = local.keys(); // Loop over all keys BOOST_FOREACH(Key key, keys) { diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index c7b7d6441..9abd4e31d 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -25,7 +25,7 @@ set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_ma if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - if("${build_type_upper}" STREQUAL "RELEASE") + if(${build_type_upper} STREQUAL "RELEASE") set(build_type_tag "") # Don't create release mode tag on installed directory else() set(build_type_tag "${build_type}") diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m new file mode 100644 index 000000000..d46493328 --- /dev/null +++ b/matlab/gtsam_tests/testTriangulation.m @@ -0,0 +1,70 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Test triangulation +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Some common constants +sharedCal = Cal3_S2(1500, 1200, 0, 640, 480); + +%% Looking along X-axis, 1 meter above ground plane (x-y) +upright = Rot3.Ypr(-pi / 2, 0., -pi / 2); +pose1 = Pose3(upright, Point3(0, 0, 1)); +camera1 = SimpleCamera(pose1, sharedCal); + +%% create second camera 1 meter to the right of first camera +pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))); +camera2 = SimpleCamera(pose2, sharedCal); + +%% landmark ~5 meters infront of camera +landmark =Point3 (5, 0.5, 1.2); + +%% 1. Project two landmarks into two cameras and triangulate +z1 = camera1.project(landmark); +z2 = camera2.project(landmark); + +%% twoPoses +poses = Pose3Vector; +measurements = Point2Vector; + +poses.push_back(pose1); +poses.push_back(pose2); +measurements.push_back(z1); +measurements.push_back(z2); + +optimize = true; +rank_tol = 1e-9; + +triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); + +%% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) +measurements = Point2Vector; +measurements.push_back(z1.retract([0.1;0.5])); +measurements.push_back(z2.retract([-0.2;0.3])); + +triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2)); + +%% two Poses with Bundler Calibration +bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480); +camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal); +camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal); + +z1 = camera1.project(landmark); +z2 = camera2.project(landmark); + +measurements = Point2Vector; +measurements.push_back(z1); +measurements.push_back(z2); + +triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 4cbe42364..e43fbcf76 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -48,8 +48,10 @@ testVisualISAMExample display 'Starting: testUtilities' testUtilities -display 'Starting: testSerialization' -testSerialization +if(exist('testSerialization.m','file')) + display 'Starting: testSerialization' + testSerialization +end % end of tests display 'Tests complete!' diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 37d2455eb..a1080a6de 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -158,7 +158,7 @@ namespace simulated2D { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } @@ -204,7 +204,7 @@ namespace simulated2D { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } @@ -251,7 +251,7 @@ namespace simulated2D { /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); } diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp similarity index 89% rename from gtsam/nonlinear/tests/testExpressionFactor.cpp rename to tests/testExpressionFactor.cpp index 99b8120e3..312ad89eb 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -36,6 +37,10 @@ using namespace gtsam; Point2 measured(-17, 30); SharedNoiseModel model = noiseModel::Unit::Create(2); +// This deals with the overload problem and makes the expressions factor +// understand that we work on Point3 +Point2 (*Project)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; + namespace leaf { // Create some values struct MyValues: public Values { @@ -169,7 +174,7 @@ static Point2 myUncal(const Cal3_S2& K, const Point2& p, // Binary(Leaf,Leaf) TEST(ExpressionFactor, Binary) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; Cal3_S2_ K_(1); Point2_ p_(2); @@ -184,11 +189,15 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(8, sizeof(double)); EXPECT_LONGS_EQUAL(16, sizeof(Point2)); EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian::type)); - EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian::type)); - size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32; + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian::type)); + EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian::type)); + size_t expectedRecordSize = sizeof(Cal3_S2) + + sizeof(internal::ExecutionTrace) + + +sizeof(internal::Jacobian::type) + sizeof(Point2) + + sizeof(internal::ExecutionTrace) + + sizeof(internal::Jacobian::type); EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); // Check size @@ -197,8 +206,8 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = binary.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -212,9 +221,8 @@ TEST(ExpressionFactor, Binary) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT( - assert_equal(expected25, (Matrix) (*r)-> jacobian(), 1e-9)); - EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); + EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) @@ -250,22 +258,22 @@ TEST(ExpressionFactor, Shallow) { LONGS_EQUAL(3,dims[1]); // traceExecution of shallow tree - typedef UnaryExpression Unary; - typedef BinaryExpression Binary; + typedef internal::UnaryExpression Unary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); - EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record)); + EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record)); #ifdef GTSAM_USE_QUATERNIONS EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record)); - LONGS_EQUAL(112+352, expectedTraceSize); + LONGS_EQUAL(96+352, expectedTraceSize); #else - EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record)); - LONGS_EQUAL(112+400, expectedTraceSize); + EXPECT_LONGS_EQUAL(384, sizeof(Binary::Record)); + LONGS_EQUAL(96+384, expectedTraceSize); #endif size_t size = expression.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedTraceSize, size); - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -277,7 +285,7 @@ TEST(ExpressionFactor, Shallow) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9)); // Linearization ExpressionFactor f2(model, measured, expression); @@ -309,7 +317,7 @@ TEST(ExpressionFactor, tree) { // Create expression tree Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); // Create factor and check value, dimension, linearization @@ -327,8 +335,6 @@ TEST(ExpressionFactor, tree) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - TernaryExpression::Function fff = project6; - // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); @@ -485,7 +491,7 @@ TEST(ExpressionFactor, tree_finite_differences) { // Create expression tree Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); const double fd_step = 1e-5; @@ -493,6 +499,11 @@ TEST(ExpressionFactor, tree_finite_differences) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance); } +TEST(ExpressionFactor, push_back) { + NonlinearFactorGraph graph; + graph.addExpressionFactor(model, Point2(0, 0), leaf::p); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index b43f0d3ef..7922c14d1 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -23,10 +23,9 @@ #include #include #include -#include - #include #include +#include using namespace boost::assign; #include namespace br { using namespace boost::adaptors; using namespace boost::range; } @@ -187,12 +186,12 @@ done: // Permuted permuted(permutation, values); // // // After permutation, the indices above the threshold are 2 and 2 -// FastSet expected; +// KeySet expected; // expected.insert(2); // expected.insert(3); // // // Indices checked by CheckRelinearization -// FastSet actual = Impl::CheckRelinearization(permuted, 0.1); +// KeySet actual = Impl::CheckRelinearization(permuted, 0.1); // // EXPECT(assert_equal(expected, actual)); //} diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index cdb1509b6..98adfc1dc 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -15,29 +15,42 @@ * @author nikai */ -#include - -#if 0 - #include -#include -#include #include +#include +#include +#include #include #include -#include -#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include -#include // for operator += -#include // for operator += +#include + +#include +#include #include + +#include +#include +#include +#include + using namespace boost::assign; #include @@ -51,190 +64,218 @@ using symbol_shorthand::L; /* ************************************************************************* * Bayes tree for smoother with "nested dissection" ordering: - C1 x5 x6 x4 - C2 x3 x2 : x4 - C3 x1 : x2 - C4 x7 : x6 -*/ -TEST( GaussianJunctionTreeB, constructor2 ) -{ + C1 x5 x6 x4 + C2 x3 x2 : x4 + C3 x1 : x2 + C4 x7 : x6 + */ +TEST( GaussianJunctionTreeB, constructor2 ) { // create a graph - Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraph fg = createSmoother(7, ordering).first; + NonlinearFactorGraph nlfg; + Values values; + boost::tie(nlfg, values) = createNonlinearSmoother(7); + SymbolicFactorGraph::shared_ptr symbolic = nlfg.symbolic(); + + // linearize + GaussianFactorGraph::shared_ptr fg = nlfg.linearize(values); + + Ordering ordering; + ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4); // create an ordering - GaussianJunctionTree actual(fg); + GaussianEliminationTree etree(*fg, ordering); + SymbolicEliminationTree stree(*symbolic, ordering); + GaussianJunctionTree actual(etree); - vector frontal1; frontal1 += ordering[X(5)], ordering[X(6)], ordering[X(4)]; - vector frontal2; frontal2 += ordering[X(3)], ordering[X(2)]; - vector frontal3; frontal3 += ordering[X(1)]; - vector frontal4; frontal4 += ordering[X(7)]; - vector sep1; - vector sep2; sep2 += ordering[X(4)]; - vector sep3; sep3 += ordering[X(2)]; - vector sep4; sep4 += ordering[X(6)]; - EXPECT(assert_equal(frontal1, actual.root()->frontal)); - EXPECT(assert_equal(sep1, actual.root()->separator)); - LONGS_EQUAL(5, actual.root()->size()); - list::const_iterator child0it = actual.root()->children().begin(); - list::const_iterator child1it = child0it; ++child1it; - GaussianJunctionTree::sharedClique child0 = *child0it; - GaussianJunctionTree::sharedClique child1 = *child1it; - EXPECT(assert_equal(frontal2, child0->frontal)); - EXPECT(assert_equal(sep2, child0->separator)); - LONGS_EQUAL(4, child0->size()); - EXPECT(assert_equal(frontal3, child0->children().front()->frontal)); - EXPECT(assert_equal(sep3, child0->children().front()->separator)); - LONGS_EQUAL(2, child0->children().front()->size()); - EXPECT(assert_equal(frontal4, child1->frontal)); - EXPECT(assert_equal(sep4, child1->separator)); - LONGS_EQUAL(2, child1->size()); + Ordering o324; + o324 += X(3), X(2), X(4); + Ordering o56; + o56 += X(5), X(6); + Ordering o7; + o7 += X(7); + Ordering o1; + o1 += X(1); + + // Can no longer test these: +// Ordering sep1; +// Ordering sep2; sep2 += X(4); +// Ordering sep3; sep3 += X(6); +// Ordering sep4; sep4 += X(2); + + GaussianJunctionTree::sharedNode x324 = actual.roots().front(); + LONGS_EQUAL(2, x324->children.size()); + GaussianJunctionTree::sharedNode x1 = x324->children.front(); + GaussianJunctionTree::sharedNode x56 = x324->children.back(); + if (x1->children.size() > 0) + x1.swap(x56); // makes it work with different tie-breakers + + LONGS_EQUAL(0, x1->children.size()); + LONGS_EQUAL(1, x56->children.size()); + GaussianJunctionTree::sharedNode x7 = x56->children[0]; + LONGS_EQUAL(0, x7->children.size()); + + EXPECT(assert_equal(o324, x324->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(5, x324->factors.size()); + EXPECT_LONGS_EQUAL(9, x324->problemSize_); + + EXPECT(assert_equal(o56, x56->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(4, x56->factors.size()); + EXPECT_LONGS_EQUAL(9, x56->problemSize_); + + EXPECT(assert_equal(o7, x7->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(2, x7->factors.size()); + EXPECT_LONGS_EQUAL(4, x7->problemSize_); + + EXPECT(assert_equal(o1, x1->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(2, x1->factors.size()); + EXPECT_LONGS_EQUAL(4, x1->problemSize_); } +///* ************************************************************************* */ +//TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) +//{ +// // create a graph +// GaussianFactorGraph fg; +// Ordering ordering; +// boost::tie(fg,ordering) = createSmoother(7); +// +// // optimize the graph +// GaussianJunctionTree tree(fg); +// VectorValues actual = tree.optimize(&EliminateQR); +// +// // verify +// VectorValues expected(vector(7,2)); // expected solution +// Vector v = Vector2(0., 0.); +// for (int i=1; i<=7; i++) +// expected[ordering[X(i)]] = v; +// EXPECT(assert_equal(expected,actual)); +//} +// +///* ************************************************************************* */ +//TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) +//{ +// // create a graph +// example::Graph nlfg = createNonlinearFactorGraph(); +// Values noisy = createNoisyValues(); +// Ordering ordering; ordering += X(1),X(2),L(1); +// GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); +// +// // optimize the graph +// GaussianJunctionTree tree(fg); +// VectorValues actual = tree.optimize(&EliminateQR); +// +// // verify +// VectorValues expected = createCorrectDelta(ordering); // expected solution +// EXPECT(assert_equal(expected,actual)); +//} +// +///* ************************************************************************* */ +//TEST(GaussianJunctionTreeB, slamlike) { +// Values init; +// NonlinearFactorGraph newfactors; +// NonlinearFactorGraph fullgraph; +// SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); +// SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); +// +// size_t i = 0; +// +// newfactors = NonlinearFactorGraph(); +// newfactors.add(PriorFactor(X(0), Pose2(0.0, 0.0, 0.0), odoNoise)); +// init.insert(X(0), Pose2(0.01, 0.01, 0.01)); +// fullgraph.push_back(newfactors); +// +// for( ; i<5; ++i) { +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); +// fullgraph.push_back(newfactors); +// } +// +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); +// init.insert(X(i+1), Pose2(1.01, 0.01, 0.01)); +// init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); +// init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); +// fullgraph.push_back(newfactors); +// ++ i; +// +// for( ; i<5; ++i) { +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); +// fullgraph.push_back(newfactors); +// } +// +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); +// init.insert(X(i+1), Pose2(6.9, 0.1, 0.01)); +// fullgraph.push_back(newfactors); +// ++ i; +// +// // Compare solutions +// Ordering ordering = *fullgraph.orderingCOLAMD(init); +// GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering); +// +// GaussianJunctionTree gjt(linearized); +// VectorValues deltaactual = gjt.optimize(&EliminateQR); +// Values actual = init.retract(deltaactual, ordering); +// +// GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); +// VectorValues delta = optimize(gbn); +// Values expected = init.retract(delta, ordering); +// +// EXPECT(assert_equal(expected, actual)); +//} +// +///* ************************************************************************* */ +//TEST(GaussianJunctionTreeB, simpleMarginal) { +// +// typedef BayesTree GaussianBayesTree; +// +// // Create a simple graph +// NonlinearFactorGraph fg; +// fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); +// fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0)))); +// +// Values init; +// init.insert(X(0), Pose2()); +// init.insert(X(1), Pose2(1.0, 0.0, 0.0)); +// +// Ordering ordering; +// ordering += X(1), X(0); +// +// GaussianFactorGraph gfg = *fg.linearize(init, ordering); +// +// // Compute marginals with both sequential and multifrontal +// Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1); +// +// Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1); +// +// // Compute marginal directly from marginal factor +// GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); +// JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); +// Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); +// +// // Compute marginal directly from BayesTree +// GaussianBayesTree gbt; +// gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky)); +// marginalFactor = gbt.marginalFactor(1, EliminateCholesky); +// marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); +// Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); +// +// EXPECT(assert_equal(expected, actual1)); +// EXPECT(assert_equal(expected, actual2)); +// EXPECT(assert_equal(expected, actual3)); +//} + /* ************************************************************************* */ -TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) -{ - // create a graph - GaussianFactorGraph fg; - Ordering ordering; - boost::tie(fg,ordering) = createSmoother(7); - - // optimize the graph - GaussianJunctionTree tree(fg); - VectorValues actual = tree.optimize(&EliminateQR); - - // verify - VectorValues expected(vector(7,2)); // expected solution - Vector v = Vector2(0., 0.); - for (int i=1; i<=7; i++) - expected[ordering[X(i)]] = v; - EXPECT(assert_equal(expected,actual)); +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); } - -/* ************************************************************************* */ -TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) -{ - // create a graph - example::Graph nlfg = createNonlinearFactorGraph(); - Values noisy = createNoisyValues(); - Ordering ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); - - // optimize the graph - GaussianJunctionTree tree(fg); - VectorValues actual = tree.optimize(&EliminateQR); - - // verify - VectorValues expected = createCorrectDelta(ordering); // expected solution - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(GaussianJunctionTreeB, slamlike) { - Values init; - NonlinearFactorGraph newfactors; - NonlinearFactorGraph fullgraph; - SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); - - size_t i = 0; - - newfactors = NonlinearFactorGraph(); - newfactors.add(PriorFactor(X(0), Pose2(0.0, 0.0, 0.0), odoNoise)); - init.insert(X(0), Pose2(0.01, 0.01, 0.01)); - fullgraph.push_back(newfactors); - - for( ; i<5; ++i) { - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullgraph.push_back(newfactors); - } - - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); - newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); - init.insert(X(i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullgraph.push_back(newfactors); - ++ i; - - for( ; i<5; ++i) { - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullgraph.push_back(newfactors); - } - - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - init.insert(X(i+1), Pose2(6.9, 0.1, 0.01)); - fullgraph.push_back(newfactors); - ++ i; - - // Compare solutions - Ordering ordering = *fullgraph.orderingCOLAMD(init); - GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering); - - GaussianJunctionTree gjt(linearized); - VectorValues deltaactual = gjt.optimize(&EliminateQR); - Values actual = init.retract(deltaactual, ordering); - - GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); - VectorValues delta = optimize(gbn); - Values expected = init.retract(delta, ordering); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(GaussianJunctionTreeB, simpleMarginal) { - - typedef BayesTree GaussianBayesTree; - - // Create a simple graph - NonlinearFactorGraph fg; - fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0)))); - - Values init; - init.insert(X(0), Pose2()); - init.insert(X(1), Pose2(1.0, 0.0, 0.0)); - - Ordering ordering; - ordering += X(1), X(0); - - GaussianFactorGraph gfg = *fg.linearize(init, ordering); - - // Compute marginals with both sequential and multifrontal - Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1); - - Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1); - - // Compute marginal directly from marginal factor - GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); - JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); - Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); - - // Compute marginal directly from BayesTree - GaussianBayesTree gbt; - gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky)); - marginalFactor = gbt.marginalFactor(1, EliminateCholesky); - marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); - Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); - - EXPECT(assert_equal(expected, actual1)); - EXPECT(assert_equal(expected, actual2)); - EXPECT(assert_equal(expected, actual3)); -} - -#endif - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp new file mode 100644 index 000000000..d43e7e31a --- /dev/null +++ b/tests/testGeneralSFMFactorB.cpp @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testGeneralSFMFactorB.cpp + * @author Frank Dellaert + * @brief test general SFM class, with nonlinear optimization and BAL files + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef GeneralSFMFactor, Point3> sfmFactor; +using symbol_shorthand::P; + +/* ************************************************************************* */ +TEST(PinholeCamera, BAL) { + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data db; + bool success = readBAL(filename, db); + if (!success) throw runtime_error("Could not access file!"); + + SharedNoiseModel unit2 = noiseModel::Unit::Create(2); + NonlinearFactorGraph graph; + + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) + graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + } + + Values initial = initialCamerasAndPointsEstimate(db); + + LevenbergMarquardtOptimizer lm(graph, initial); + + Values actual = lm.optimize(); + double actualError = graph.error(actual); + EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-7); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 81bcac344..e45f234aa 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -1,7 +1,14 @@ +/** + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Test simple CG optimizer + * @author Yong-Dian Jian + * @date June 11, 2012 + */ + /** * @file testGradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @brief Small test of NonlinearConjugateGradientOptimizer + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -18,89 +25,74 @@ #include #include - using namespace std; using namespace gtsam; - +// Generate a small PoseSLAM problem boost::tuple generateProblem() { // 1. Create graph container and add factors to it - NonlinearFactorGraph graph ; + NonlinearFactorGraph graph; // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.3, 0.3, 0.1)); graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), + constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estimate to the solution Values initialEstimate; - Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); - Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3); - Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insert(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5); + Pose2 x1(0.5, 0.0, 0.2); + initialEstimate.insert(1, x1); + Pose2 x2(2.3, 0.1, -0.2); + initialEstimate.insert(2, x2); + Pose2 x3(4.1, 0.1, M_PI_2); + initialEstimate.insert(3, x3); + Pose2 x4(4.0, 2.0, M_PI); + initialEstimate.insert(4, x4); + Pose2 x5(2.1, 2.1, -M_PI_2); + initialEstimate.insert(5, x5); return boost::tie(graph, initialEstimate); } /* ************************************************************************* */ -TEST(optimize, GradientDescentOptimizer) { +TEST(NonlinearConjugateGradientOptimizer, Optimize) { NonlinearFactorGraph graph; Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); - // cout << "initial error = " << graph.error(initialEstimate) << endl ; +// cout << "initial error = " << graph.error(initialEstimate) << endl; - // Single Step Optimization using Levenberg-Marquardt NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ - param.verbosity = NonlinearOptimizerParams::SILENT; - - NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); - Values result = optimizer.optimize(); -// cout << "gd1 solver final error = " << graph.error(result) << endl; - - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); - - CHECK(1); -} - -/* ************************************************************************* */ -TEST(optimize, ConjugateGradientOptimizer) { - - NonlinearFactorGraph graph; - Values initialEstimate; - - boost::tie(graph, initialEstimate) = generateProblem(); -// cout << "initial error = " << graph.error(initialEstimate) << endl ; - - // Single Step Optimization using Levenberg-Marquardt - NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ + param.maxIterations = 500; /* requires a larger number of iterations to converge */ param.verbosity = NonlinearOptimizerParams::SILENT; NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); // cout << "cg final error = " << graph.error(result) << endl; - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/tests/testLie.cpp b/tests/testLie.cpp new file mode 100644 index 000000000..7be629053 --- /dev/null +++ b/tests/testLie.cpp @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------1------------------------------------------- */ + +/** + * @file testLie.cpp + * @date May, 2015 + * @author Frank Dellaert + * @brief unit tests for Lie group type machinery + */ + +#include + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +static const double tol = 1e-9; + +//****************************************************************************** +typedef ProductLieGroup Product; + +// Define any direct product group to be a model of the multiplicative Group concept +namespace gtsam { +template<> struct traits : internal::LieGroupTraits { + static void Print(const Product& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second.translation() << "/" + << m.second.theta() << ")" << endl; + } + static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { + return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol); + } +}; +} + +//****************************************************************************** +TEST(Lie, ProductLieGroup) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + Product pair1; + Vector5 d; + d << 1, 2, 0.1, 0.2, 0.3; + Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3))); + Product pair2 = pair1.retract(d); + EXPECT(assert_equal(expected, pair2, 1e-9)); + EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9)); +} + +/* ************************************************************************* */ +Product compose_proxy(const Product& A, const Product& B) { + return A.compose(B); +} +TEST( testProduct, compose ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +Product between_proxy(const Product& A, const Product& B) { + return A.between(B); +} +TEST( testProduct, between ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +Product inverse_proxy(const Product& A) { + return A.inverse(); +} +TEST( testProduct, inverse ) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); + EXPECT(assert_equal(numericH1, actH1, tol)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index f6180fb73..89b296824 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -10,16 +10,16 @@ * -------------------------------1------------------------------------------- */ /** - * @file testExpression.cpp + * @file testManifold.cpp * @date September 18, 2014 * @author Frank Dellaert * @author Paul Furgale - * @brief unit tests for Block Automatic Differentiation + * @brief unit tests for Manifold type machinery */ -#include +#include #include -#include +#include #include #include #include @@ -149,6 +149,32 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(zero(3), traits::Local(R, R))); } +//****************************************************************************** +typedef ProductManifold MyPoint2Pair; + +// Define any direct product group to be a model of the multiplicative Group concept +namespace gtsam { +template<> struct traits : internal::ManifoldTraits { + static void Print(const MyPoint2Pair& m, const string& s = "") { + cout << s << "(" << m.first << "," << m.second << ")" << endl; + } + static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, double tol = 1e-8) { + return m1 == m2; + } +}; +} + +TEST(Manifold, ProductManifold) { + BOOST_CONCEPT_ASSERT((IsManifold)); + MyPoint2Pair pair1; + Vector4 d; + d << 1,2,3,4; + MyPoint2Pair expected(Point2(1,2),Point2(3,4)); + MyPoint2Pair pair2 = pair1.retract(d); + EXPECT(assert_equal(expected,pair2,1e-9)); + EXPECT(assert_equal(d, pair1.localCoordinates(pair2),1e-9)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index dd22ae738..bd202e991 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -226,7 +226,8 @@ TEST(Marginals, order) { vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2)); Marginals marginals(fg, vals); - FastVector keys(fg.keys()); + KeySet set = fg.keys(); + FastVector keys(set.begin(), set.end()); JointMarginal joint = marginals.jointMarginalCovariance(keys); LONGS_EQUAL(3, (long)joint(0,0).rows()); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 8e6b033e9..ee60f9714 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -66,9 +66,9 @@ TEST( NonlinearFactorGraph, error ) TEST( NonlinearFactorGraph, keys ) { NonlinearFactorGraph fg = createNonlinearFactorGraph(); - FastSet actual = fg.keys(); + KeySet actual = fg.keys(); LONGS_EQUAL(3, (long)actual.size()); - FastSet::const_iterator it = actual.begin(); + KeySet::const_iterator it = actual.begin(); LONGS_EQUAL((long)L(1), (long)*(it++)); LONGS_EQUAL((long)X(1), (long)*(it++)); LONGS_EQUAL((long)X(2), (long)*(it++)); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 6d8a056fc..f927f45ae 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -187,7 +187,9 @@ TEST( NonlinearOptimizer, Factorization ) ordering.push_back(X(1)); ordering.push_back(X(2)); - LevenbergMarquardtOptimizer optimizer(graph, config, ordering); + LevenbergMarquardtParams params; + LevenbergMarquardtParams::SetLegacyDefaults(¶ms); + LevenbergMarquardtOptimizer optimizer(graph, config, ordering, params); optimizer.iterate(); Values expected; @@ -260,13 +262,13 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { expectedGradient.insert(2,zero(3)); // Try LM and Dogleg - LevenbergMarquardtParams params; -// params.setVerbosityLM("TRYDELTA"); -// params.setVerbosity("TERMINATION"); - params.setlambdaUpperBound(1e9); -// params.setRelativeErrorTol(0); -// params.setAbsoluteErrorTol(0); - //params.setlambdaInitial(10); + LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults(); + // params.setVerbosityLM("TRYDELTA"); + // params.setVerbosity("TERMINATION"); + params.lambdaUpperBound = 1e9; +// params.relativeErrorTol = 0; +// params.absoluteErrorTol = 0; + //params.lambdaInitial = 10; { LevenbergMarquardtOptimizer optimizer(fg, init, params); @@ -290,7 +292,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { initBetter.insert(2, Pose2(11,7,M_PI/2)); { - params.setDiagonalDamping(true); + params.diagonalDamping = true; LevenbergMarquardtOptimizer optimizer(fg, initBetter, params); // test the diagonal @@ -399,7 +401,7 @@ public: /// Constructor IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, const ConjugateGradientParameters &p, - const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) : + const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) : LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p) { } @@ -446,8 +448,7 @@ TEST( NonlinearOptimizer, logfile ) // Levenberg-Marquardt LevenbergMarquardtParams lmParams; static const string filename("testNonlinearOptimizer.log"); - lmParams.setLogFile(filename); - CHECK(lmParams.getLogFile()==filename); + lmParams.logFile = filename; LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); // stringstream expected,actual; diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index c90b09db1..66b022c82 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -17,21 +17,11 @@ */ #include -#include -#include -#include -#include -#include -#include -#include #include #include #include -#include #include -#include #include -#include #include #include diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 89c3d5cf8..f00916475 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -18,11 +18,12 @@ #include -#include -#include #include -#include +#include +#include +#include #include +#include using namespace std; using namespace gtsam; diff --git a/timing/DummyFactor.h b/timing/DummyFactor.h new file mode 100644 index 000000000..08e9d8f4b --- /dev/null +++ b/timing/DummyFactor.h @@ -0,0 +1,56 @@ +/** + * @file DummyFactor.h + * @brief Just to help in timing overhead + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * DummyFactor + */ +template // +class DummyFactor: public RegularImplicitSchurFactor { + +public: + + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; + + DummyFactor() { + } + + DummyFactor(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b) : + RegularImplicitSchurFactor(Fblocks, E, P, b) { + } + + virtual ~DummyFactor() { + } + +public: + + /** + * @brief Dummy version to measure overhead of key access + */ + void multiplyHessian(double alpha, const VectorValues& x, + VectorValues& y) const { + + BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) { + static const Vector empty; + Key key = Fi.first; + std::pair it = y.tryInsert(key, empty); + Vector& yi = it.first->second; + yi = x.at(key); + } + } + +}; +// DummyFactor + +} + diff --git a/timing/timeCalibratedCamera.cpp b/timing/timeCalibratedCamera.cpp index 0a003a4c7..2d0576aea 100644 --- a/timing/timeCalibratedCamera.cpp +++ b/timing/timeCalibratedCamera.cpp @@ -15,10 +15,10 @@ * @author Frank Dellaert */ -#include -#include - +#include #include +#include +#include using namespace std; using namespace gtsam; diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index 931e2498f..3a4da89b5 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 4ad9d7d47..26eed0207 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -15,10 +15,10 @@ * @author Richard Roberts */ -#include +#include #include -#include +#include using namespace std; using namespace gtsam; diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp new file mode 100644 index 000000000..68b3c4932 --- /dev/null +++ b/timing/timeSFMBAL.cpp @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timeSFMBAL.cpp + * @brief time structure from motion with BAL file + * @author Frank Dellaert + * @date June 6, 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +#define USE_GTSAM_FACTOR +#ifdef USE_GTSAM_FACTOR +#include +#include +#include +typedef PinholeCamera Camera; +typedef GeneralSFMFactor SfmFactor; +#else +#include +#include +#include +// Special version of Cal3Bundler so that default constructor = 0,0,0 +struct CeresCalibration: public Cal3Bundler { + CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, + double v0 = 0) : + Cal3Bundler(f, k1, k2, u0, v0) { + } + CeresCalibration(const Cal3Bundler& cal) : + Cal3Bundler(cal) { + } + CeresCalibration retract(const Vector& d) const { + return CeresCalibration(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + } + Vector3 localCoordinates(const CeresCalibration& T2) const { + return T2.vector() - vector(); + } +}; + +namespace gtsam { +template<> +struct traits : public internal::Manifold { +}; +} + +// With that, camera below behaves like Snavely's 9-dim vector +typedef PinholeCamera Camera; +#endif + +int main(int argc, char* argv[]) { + using symbol_shorthand::P; + + // Load BAL file (default is tiny) + string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data db; + bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db); + if (!success) + throw runtime_error("Could not access file!"); + +#ifndef USE_GTSAM_FACTOR + AdaptAutoDiff snavely; +#endif + + // Build graph + SharedNoiseModel unit2 = noiseModel::Unit::Create(2); + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + size_t i = m.first; + Point2 measurement = m.second; +#ifdef USE_GTSAM_FACTOR + graph.push_back(SfmFactor(measurement, unit2, i, P(j))); +#else + Expression camera_(i); + Expression point_(P(j)); + graph.addExpressionFactor(unit2, measurement, + Expression(snavely, camera_, point_)); +#endif + } + } + + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH(const SfM_Camera& camera, db.cameras) { +#ifdef USE_GTSAM_FACTOR + initial.insert((i++), camera); +#else + Camera ceresCamera(camera.pose(), camera.calibration()); + initial.insert((i++), ceresCamera); +#endif + } + BOOST_FOREACH(const SfM_Track& track, db.tracks) + initial.insert(P(j++), track.p); + + // Check projection + Point2 expected = db.tracks.front().measurements.front().second; + Camera camera = initial.at(0); + Point3 point = initial.at(P(0)); +#ifdef USE_GTSAM_FACTOR + Point2 actual = camera.project(point); +#else + Point2 actual = snavely(camera, point); +#endif + assert_equal(expected,actual,10); + + // Create Schur-complement ordering +#ifdef CCOLAMD + vector pointKeys; + for (size_t j = 0; j < db.number_tracks(); j++) pointKeys.push_back(P(j)); + Ordering ordering = Ordering::colamdConstrainedFirst(graph, pointKeys, true); +#else + Ordering ordering; + for (size_t j = 0; j < db.number_tracks(); j++) + ordering.push_back(P(j)); + for (size_t i = 0; i < db.number_cameras(); i++) + ordering.push_back(i); +#endif + + // Optimize + // Set parameters to be similar to ceres + LevenbergMarquardtParams params; + LevenbergMarquardtParams::SetCeresDefaults(¶ms); + params.setOrdering(ordering); + params.setVerbosity("ERROR"); + params.setVerbosityLM("TRYLAMBDA"); + LevenbergMarquardtOptimizer lm(graph, initial, params); + Values result = lm.optimize(); + + tictoc_finishedIteration_(); + tictoc_print_(); + + return 0; +} diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp new file mode 100644 index 000000000..e08924400 --- /dev/null +++ b/timing/timeSchurFactors.cpp @@ -0,0 +1,157 @@ +/** + * @file timeSchurFactors.cpp + * @brief Time various Schur-complement Jacobian factors + * @author Frank Dellaert + * @date Oct 27, 2013 + */ + +#include "DummyFactor.h" +#include + +#include +#include "gtsam/slam/JacobianFactorQR.h" +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +#define SLOW +#define RAW +#define HESSIAN +#define NUM_ITERATIONS 1000 + +// Create CSV file for results +ofstream os("timeSchurFactors.csv"); + +/*************************************************************************************/ +template +void timeAll(size_t m, size_t N) { + + cout << m << endl; + + // create F + static const int D = CAMERA::dimension; + typedef Eigen::Matrix Matrix2D; + FastVector keys; + vector Fblocks; + for (size_t i = 0; i < m; i++) { + keys.push_back(i); + Fblocks.push_back((i + 1) * Matrix::Ones(2, D)); + } + + // create E + Matrix E(2 * m, 3); + for (size_t i = 0; i < m; i++) + E.block < 2, 3 > (2 * i, 0) = Matrix::Ones(2, 3); + + // Calculate point covariance + Matrix P = (E.transpose() * E).inverse(); + + // RHS and sigmas + const Vector b = gtsam::repeat(2 * m, 1); + const SharedDiagonal model; + + // parameters for multiplyHessianAdd + double alpha = 0.5; + VectorValues xvalues, yvalues; + for (size_t i = 0; i < m; i++) + xvalues.insert(i, gtsam::repeat(D, 2)); + + // Implicit + RegularImplicitSchurFactor implicitFactor(keys, Fblocks, E, P, b); + // JacobianFactor with same error + JacobianFactorQ jf(keys, Fblocks, E, P, b, model); + // JacobianFactorQR with same error + JacobianFactorQR jqr(keys, Fblocks, E, P, b, model); + // Hessian + HessianFactor hessianFactor(jqr); + +#define TIME(label,factor,xx,yy) {\ + for (size_t t = 0; t < N; t++) \ + factor.multiplyHessianAdd(alpha, xx, yy);\ + gttic_(label);\ + for (size_t t = 0; t < N; t++) {\ + factor.multiplyHessianAdd(alpha, xx, yy);\ + }\ + gttoc_(label);\ + tictoc_getNode(timer, label)\ + os << timer->secs()/NUM_ITERATIONS << ", ";\ + } + +#ifdef SLOW + TIME(Implicit, implicitFactor, xvalues, yvalues) + TIME(Jacobian, jf, xvalues, yvalues) + TIME(JacobianQR, jqr, xvalues, yvalues) +#endif + +#ifdef HESSIAN + TIME(Hessian, hessianFactor, xvalues, yvalues) +#endif + +#ifdef OVERHEAD + DummyFactor dummy(Fblocks, E, P, b); + TIME(Overhead,dummy,xvalues,yvalues) +#endif + +#ifdef RAW + { // Raw memory Version + FastVector < Key > keys; + for (size_t i = 0; i < m; i++) + keys += i; + Vector x = xvalues.vector(keys); + double* xdata = x.data(); + + // create a y + Vector y = zero(m * D); + TIME(RawImplicit, implicitFactor, xdata, y.data()) + TIME(RawJacobianQ, jf, xdata, y.data()) + TIME(RawJacobianQR, jqr, xdata, y.data()) + } +#endif + + os << m << endl; + +} // timeAll + +/*************************************************************************************/ +int main(void) { +#ifdef SLOW + os << "Implicit,"; + os << "JacobianQ,"; + os << "JacobianQR,"; +#endif +#ifdef HESSIAN + os << "Hessian,"; +#endif +#ifdef OVERHEAD + os << "Overhead,"; +#endif +#ifdef RAW + os << "RawImplicit,"; + os << "RawJacobianQ,"; + os << "RawJacobianQR,"; +#endif + os << "m" << endl; + // define images + vector < size_t > ms; + // ms += 2; + // ms += 3, 4, 5, 6, 7, 8, 9, 10; + // ms += 11,12,13,14,15,16,17,18,19; + // ms += 20, 30, 40, 50; + // ms += 20,30,40,50,60,70,80,90,100; + for (size_t m = 2; m <= 50; m += 2) + ms += m; + //for (size_t m=10;m<=100;m+=10) ms += m; + // loop over number of images + BOOST_FOREACH(size_t m,ms) + timeAll >(m, NUM_ITERATIONS); +} + +//************************************************************************************* diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 1a9c82143..c03c40444 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -41,10 +41,6 @@ static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not define typedef vector strvec; -// NOTE: as this path is only used to generate makefiles, it is hardcoded here for testing -// In practice, this path will be an absolute system path, which makes testing it more annoying -static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap"; - /* ************************************************************************* */ TEST( wrap, ArgumentList ) { ArgumentList args; diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 51dc6f4c3..57a5bce29 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -91,7 +91,8 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; command << "diff --ignore-all-space " << expected << " " << actual << endl; - system(command.str().c_str()); + if (system(command.str().c_str())<0) + throw "command '" + command.str() + "' failed"; cerr << ">>>\n"; } return equal;