diff --git a/.cproject b/.cproject index 90e48745b..46b623bb9 100644 --- a/.cproject +++ b/.cproject @@ -582,7 +582,6 @@ make - tests/testBayesTree.run true false @@ -590,7 +589,6 @@ make - testBinaryBayesNet.run true false @@ -638,7 +636,6 @@ make - testSymbolicBayesNet.run true false @@ -646,7 +643,6 @@ make - tests/testSymbolicFactor.run true false @@ -654,7 +650,6 @@ make - testSymbolicFactorGraph.run true false @@ -670,7 +665,6 @@ make - tests/testBayesTree true false @@ -1006,7 +1000,6 @@ make - testErrors.run true false @@ -1236,46 +1229,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1358,6 +1311,7 @@ make + testSimulated2DOriented.run true false @@ -1397,6 +1351,7 @@ make + testSimulated2D.run true false @@ -1404,6 +1359,7 @@ make + testSimulated3D.run true false @@ -1417,6 +1373,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1674,7 +1670,6 @@ cpack - -G DEB true false @@ -1682,7 +1677,6 @@ cpack - -G RPM true false @@ -1690,7 +1684,6 @@ cpack - -G TGZ true false @@ -1698,7 +1691,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2425,7 +2417,6 @@ make - testGraph.run true false @@ -2433,7 +2424,6 @@ make - testJunctionTree.run true false @@ -2441,7 +2431,6 @@ make - testSymbolicBayesNetB.run true false @@ -2647,14 +2636,6 @@ true true - - make - -j5 - testInitializePose3.run - true - true - true - make -j2 @@ -2929,6 +2910,7 @@ make + tests/testGaussianISAM2 true false diff --git a/CMakeLists.txt b/CMakeLists.txt index 004ded5e4..6fcce54b6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -123,6 +123,11 @@ else() endif() +if(${Boost_VERSION} EQUAL 105600) + message("Ignoring Boost restriction on optional lvalue assignment from rvalues") + add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES) +endif() + ############################################################################### # Find TBB find_package(TBB) @@ -169,9 +174,9 @@ endif() ############################################################################### # Find OpenMP (if we're also using MKL) -if(GTSAM_WITH_EIGEN_MKL AND GTSAM_USE_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) - find_package(OpenMP) +find_package(OpenMP) # do this here to generate correct message if disabled +if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake index c387ce9d3..44681f7b8 100644 --- a/cmake/FindMKL.cmake +++ b/cmake/FindMKL.cmake @@ -58,6 +58,7 @@ FIND_PATH(MKL_ROOT_DIR /opt/intel/mkl/*/ /opt/intel/cmkl/ /opt/intel/cmkl/*/ + /opt/intel/*/mkl/ /Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal "C:/Program Files (x86)/Intel/ComposerXE-2011/mkl" "C:/Program Files (x86)/Intel/Composer XE 2013/mkl" diff --git a/examples/Data/pose3example-offdiagonal.txt b/examples/Data/pose3example-offdiagonal.txt index c3eb883a4..08ca2e1d1 100644 --- a/examples/Data/pose3example-offdiagonal.txt +++ b/examples/Data/pose3example-offdiagonal.txt @@ -1,3 +1,3 @@ VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 -EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000 10000.000000 2.000000 2.000000 2.000000 2.000000 10000.000000 3.000000 3.000000 3.000000 10000.000000 4.000000 4.000000 10000.000000 5.000000 10000.000000 \ No newline at end of file +EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000 10000.000000 2.000000 2.000000 2.000000 2.000000 10000.000000 3.000000 3.000000 3.000000 10000.000000 4.000000 4.000000 10000.000000 5.000000 10000.0000 \ No newline at end of file diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index a34a96c9b..5454f4c11 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -120,15 +120,15 @@ int main(int argc, char** argv) { // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); - graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y - graph.push_back(boost::make_shared(1, 0.0, 0.0, unaryNoise)); - graph.push_back(boost::make_shared(2, 2.0, 0.0, unaryNoise)); - graph.push_back(boost::make_shared(3, 4.0, 0.0, unaryNoise)); + graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); + graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); + graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index f2d4c6497..70c6e6fb0 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -65,15 +65,15 @@ int main(int argc, char** argv) { // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); - graph.push_back(PriorFactor(1, priorMean, priorNoise)); + graph.add(PriorFactor(1, priorMean, priorNoise)); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.push_back(BetweenFactor(1, 2, odometry, odometryNoise)); - graph.push_back(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); + graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); graph.print("\nFactor Graph:\n"); // print // Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index a1c75eab7..91ca423a2 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -81,13 +81,13 @@ int main(int argc, char** argv) { // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.push_back(PriorFactor(x1, prior, priorNoise)); // add directly to graph + graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.push_back(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.push_back(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); + graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements @@ -101,9 +101,9 @@ int main(int argc, char** argv) { range32 = 2.0; // Add Bearing-Range factors - graph.push_back(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.push_back(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.push_back(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); + graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); + graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); // Print graph.print("Factor Graph:\n"); diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index e1a51a493..ae34278ec 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -72,23 +72,23 @@ int main(int argc, char** argv) { // 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) noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); - graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses - graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.push_back(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); + graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); + graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); + graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); // 2c. Add the loop closure constraint // This factor encodes the fact that we have returned to the same pose. In real systems, // these constraints may be identified in many ways, such as appearance-based techniques // with camera images. We will use another Between Factor to enforce this constraint: - graph.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index ae4ee5a8c..f3f770750 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -17,7 +17,6 @@ * @author Luca Carlone */ -#include #include #include #include diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index c2acb8091..f992c78b1 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -17,7 +17,6 @@ * @author Luca Carlone */ -#include #include #include #include diff --git a/examples/Pose3SLAMExample_rotOnlyGN.cpp b/examples/Pose3SLAMExample_rotOnlyGN.cpp deleted file mode 100644 index 293fa1a09..000000000 --- a/examples/Pose3SLAMExample_rotOnlyGN.cpp +++ /dev/null @@ -1,95 +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 Pose3SLAMExample_initializePose3.cpp - * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o - * @date Aug 25, 2014 - * @author Luca Carlone - */ - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -int main(const int argc, const char *argv[]) { - - // Read graph from file - string g2oFile; - if (argc < 2) - g2oFile = findExampleDataFile("pose3example.txt"); - else - g2oFile = argv[1]; - - NonlinearFactorGraph::shared_ptr graph; - Values::shared_ptr initial; - bool is3D = true; - boost::tie(graph, initial) = readG2o(g2oFile, is3D); - - Values initialRot; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { - Key key = key_value.key; - Pose3 pose = initial->at(key); - Rot3 R = pose.rotation(); - initialRot.insert(key,R); - } - - noiseModel::Unit::shared_ptr identityModel = noiseModel::Unit::Create(3); - NonlinearFactorGraph graphRot; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *graph) { - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); - if(pose3Between){ - Key key1 = pose3Between->key1(); - Key key2 = pose3Between->key2(); - Pose3 Pij = pose3Between->measured(); - Rot3 Rij = Pij.rotation(); - NonlinearFactor::shared_ptr factorRot(new BetweenFactor(key1, key2, Rij, identityModel)); - graphRot.add(factorRot); - }else{ - std::cout << "Found a factor that is not a Between: not admitted" << std::endl; - return 1; - } - } - // Add prior on the first key - graphRot.add(PriorFactor(0, Rot3(), identityModel)); - - std::cout << "Optimizing Rot3 via GN" << std::endl; - // GaussNewtonParams params; - GaussNewtonOptimizer optimizer(graphRot, initialRot); - Values GNrot = optimizer.optimize(); - std::cout << "done!" << std::endl; - - // Wrap estimate as poses to write in g2o format - Values GNposes; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNrot) { - Key key = key_value.key; - Rot3 R = GNrot.at(key); - GNposes.insert(key,Pose3(R,Point3())); - } - - if (argc < 3) { - GNrot.print("initialization"); - } else { - const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, GNposes, outputFile); - std::cout << "done! " << std::endl; - } - return 0; -} diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 2f0c71a40..26abfff85 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -13,6 +13,22 @@ * @brief Incremental and batch solving, timing, and accuracy comparisons * @author Richard Roberts * @date August, 2013 +* +* Here is an example. Below, to run in batch mode, we first generate an initialization in incremental mode. +* +* Solve in incremental and write to file w_inc: +* ./SolverComparer --incremental -d w10000 -o w_inc +* +* You can then perturb that initialization to get batch something to optimize. +* Read in w_inc, perturb it with noise of stddev 0.6, and write to w_pert: +* ./SolverComparer --perturb 0.6 -i w_inc -o w_pert +* +* Then optimize with batch, read in w_pert, solve in batch, and write to w_batch: +* ./SolverComparer --batch -d w10000 -i w_pert -o w_batch +* +* And finally compare solutions in w_inc and w_batch to check that batch converged to the global minimum +* ./SolverComparer --compare w_inc w_batch +* */ #include diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index b5645e214..8a32f5dcc 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -14,6 +14,7 @@ * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset * This version uses iSAM to solve the problem incrementally * @author Duy-Nguyen Ta + * @author Frank Dellaert */ /** @@ -61,7 +62,8 @@ int main(int argc, char* argv[]) { 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 + noiseModel::Isotropic::shared_ptr noise = // + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks vector points = createPoints(); @@ -69,7 +71,8 @@ int main(int argc, char* argv[]) { // Create the set of ground-truth poses vector poses = createPoses(); - // Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates + // Create a NonlinearISAM object which will relinearize and reorder the variables + // every "relinearizeInterval" updates int relinearizeInterval = 3; NonlinearISAM isam(relinearizeInterval); @@ -82,32 +85,44 @@ int main(int argc, char* argv[]) { // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { + // Create ground truth measurement SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + // Add measurement + graph.add( + GenericProjectionFactor(measurement, noise, + Symbol('x', i), Symbol('l', j), K)); } - // Add an initial guess for the current pose // Intentionally initialize the variables off from the ground truth - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + Pose3 initial_xi = poses[i].compose(noise); + + // Add an initial guess for the current pose + initialEstimate.insert(Symbol('x', i), initial_xi); // If this is the first iteration, add a prior on the first pose to set the coordinate frame // and a prior on the first landmark to set the scale // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before // adding it to iSAM. - if( i == 0) { - // Add a prior on pose x0 - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + if (i == 0) { + // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); + graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0 - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + noiseModel::Isotropic::shared_ptr pointNoise = + noiseModel::Isotropic::Sigma(3, 0.1); + graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // Add initial guesses to all observed landmarks - // Intentionally initialize the variables off from the ground truth - for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + Point3 noise(-0.25, 0.20, 0.15); + for (size_t j = 0; j < points.size(); ++j) { + // Intentionally initialize the variables off from the ground truth + Point3 initial_lj = points[j].compose(noise); + initialEstimate.insert(Symbol('l', j), initial_lj); + } } else { // Update iSAM with the new factors diff --git a/gtsam/3rdparty/Eigen/CTestConfig.cmake b/gtsam/3rdparty/Eigen/CTestConfig.cmake index 4c0027824..0557c491a 100644 --- a/gtsam/3rdparty/Eigen/CTestConfig.cmake +++ b/gtsam/3rdparty/Eigen/CTestConfig.cmake @@ -4,14 +4,10 @@ ## # The following are required to uses Dart and the Cdash dashboard ## ENABLE_TESTING() ## INCLUDE(CTest) -set(CTEST_PROJECT_NAME "Eigen") +set(CTEST_PROJECT_NAME "Eigen3.2") set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC") set(CTEST_DROP_METHOD "http") set(CTEST_DROP_SITE "manao.inria.fr") -set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen") +set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.2") set(CTEST_DROP_SITE_CDASH TRUE) -set(CTEST_PROJECT_SUBPROJECTS -Official -Unsupported -) diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core index 9131cc3fc..c87f99df3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Core +++ b/gtsam/3rdparty/Eigen/Eigen/Core @@ -95,7 +95,7 @@ extern "C" { // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly. // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus: - #ifdef __INTEL_COMPILER + #if defined(__INTEL_COMPILER) && __INTEL_COMPILER >= 1110 #include #else #include @@ -165,7 +165,7 @@ #endif // required for __cpuid, needs to be included after cmath -#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64)) +#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64)) && (!defined(_WIN32_WCE)) #include #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index d026418f8..c52b7d1a6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -274,30 +274,13 @@ template<> struct ldlt_inplace return true; } - RealScalar cutoff(0), biggest_in_corner; - for (Index k = 0; k < size; ++k) { // Find largest diagonal element Index index_of_biggest_in_corner; - biggest_in_corner = mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner); + mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner); index_of_biggest_in_corner += k; - if(k == 0) - { - // The biggest overall is the point of reference to which further diagonals - // are compared; if any diagonal is negligible compared - // to the largest overall, the algorithm bails. - cutoff = abs(NumTraits::epsilon() * biggest_in_corner); - } - - // Finish early if the matrix is not full rank. - if(biggest_in_corner < cutoff) - { - for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i; - break; - } - transpositions.coeffRef(k) = index_of_biggest_in_corner; if(k != index_of_biggest_in_corner) { @@ -328,15 +311,20 @@ template<> struct ldlt_inplace if(k>0) { - temp.head(k) = mat.diagonal().head(k).asDiagonal() * A10.adjoint(); + temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint(); mat.coeffRef(k,k) -= (A10 * temp.head(k)).value(); if(rs>0) A21.noalias() -= A20 * temp.head(k); } - if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff)) - A21 /= mat.coeffRef(k,k); - + + // In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot + // was smaller than the cutoff value. However, soince LDLT is not rank-revealing + // we should only make sure we do not introduce INF or NaN values. + // LAPACK also uses 0 as the cutoff value. RealScalar realAkk = numext::real(mat.coeffRef(k,k)); + if((rs>0) && (abs(realAkk) > RealScalar(0))) + A21 /= realAkk; + if (sign == PositiveSemiDef) { if (realAkk < 0) sign = Indefinite; } else if (sign == NegativeSemiDef) { @@ -516,14 +504,20 @@ struct solve_retval, Rhs> typedef typename LDLTType::MatrixType MatrixType; typedef typename LDLTType::Scalar Scalar; typedef typename LDLTType::RealScalar RealScalar; - const Diagonal vectorD = dec().vectorD(); - RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits::epsilon(), - RealScalar(1) / NumTraits::highest()); // motivated by LAPACK's xGELSS + 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 + // as motivated by LAPACK's xGELSS: + // RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() *NumTraits::epsilon(),RealScalar(1) / NumTraits::highest()); + // However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest + // diagonal element is not well justified and to numerical issues in some cases. + // Moreover, Lapack's xSYTRS routines use 0 for the tolerance. + RealScalar tolerance = RealScalar(1) / NumTraits::highest(); + for (Index i = 0; i < vectorD.size(); ++i) { if(abs(vectorD(i)) > tolerance) - dst.row(i) /= vectorD(i); + dst.row(i) /= vectorD(i); else - dst.row(i).setZero(); + dst.row(i).setZero(); } // dst = L^-T (D^-1 L^-1 P b) @@ -576,7 +570,7 @@ MatrixType LDLT::reconstructedMatrix() const // L^* P res = matrixU() * res; // D(L^*P) - res = vectorD().asDiagonal() * res; + res = vectorD().real().asDiagonal() * res; // L(DL^*P) res = matrixL() * res; // P^T (LDL^*P) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h index 358b3188b..87bedfa46 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h @@ -81,7 +81,7 @@ struct traits > : traits::Flags&LinearAccessBit))) ? LinearAccessBit : 0, FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0, Flags0 = traits::Flags & ( (HereditaryBits & ~RowMajorBit) | diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h index 46e3a960a..cb66caf5e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h @@ -47,6 +47,17 @@ struct CommaInitializer : m_xpr.block(0, 0, other.rows(), other.cols()) = other; } + /* Copy/Move constructor which transfers ownership. This is crucial in + * absence of return value optimization to avoid assertions during destruction. */ + // FIXME in C++11 mode this could be replaced by a proper RValue constructor + inline CommaInitializer(const CommaInitializer& o) + : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) { + // Mark original object as finished. In absence of R-value references we need to const_cast: + const_cast(o).m_row = m_xpr.rows(); + const_cast(o).m_col = m_xpr.cols(); + const_cast(o).m_currentBlockRows = 0; + } + /* inserts a scalar value in the target matrix */ CommaInitializer& operator,(const Scalar& s) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig new file mode 100644 index 000000000..a036d8c3b --- /dev/null +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig @@ -0,0 +1,154 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2006-2008 Benoit Jacob +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_COMMAINITIALIZER_H +#define EIGEN_COMMAINITIALIZER_H + +namespace Eigen { + +/** \class CommaInitializer + * \ingroup Core_Module + * + * \brief Helper class used by the comma initializer operator + * + * This class is internally used to implement the comma initializer feature. It is + * the return type of MatrixBase::operator<<, and most of the time this is the only + * way it is used. + * + * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished() + */ +template +struct CommaInitializer +{ + typedef typename XprType::Scalar Scalar; + typedef typename XprType::Index Index; + + inline CommaInitializer(XprType& xpr, const Scalar& s) + : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) + { + m_xpr.coeffRef(0,0) = s; + } + + template + inline CommaInitializer(XprType& xpr, const DenseBase& other) + : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows()) + { + m_xpr.block(0, 0, other.rows(), other.cols()) = other; + } + + /* Copy/Move constructor which transfers ownership. This is crucial in + * absence of return value optimization to avoid assertions during destruction. */ + // FIXME in C++11 mode this could be replaced by a proper RValue constructor + inline CommaInitializer(const CommaInitializer& o) + : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) { + // Mark original object as finished. In absence of R-value references we need to const_cast: + const_cast(o).m_row = m_xpr.rows(); + const_cast(o).m_col = m_xpr.cols(); + const_cast(o).m_currentBlockRows = 0; + } + + /* inserts a scalar value in the target matrix */ + CommaInitializer& operator,(const Scalar& s) + { + if (m_col==m_xpr.cols()) + { + m_row+=m_currentBlockRows; + m_col = 0; + m_currentBlockRows = 1; + eigen_assert(m_row + CommaInitializer& operator,(const DenseBase& other) + { + if(other.cols()==0 || other.rows()==0) + return *this; + if (m_col==m_xpr.cols()) + { + m_row+=m_currentBlockRows; + m_col = 0; + m_currentBlockRows = other.rows(); + eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows() + && "Too many rows passed to comma initializer (operator<<)"); + } + eigen_assert(m_col + (m_row, m_col) = other; + else + m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other; + m_col += other.cols(); + return *this; + } + + inline ~CommaInitializer() + { + eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows() + && m_col == m_xpr.cols() + && "Too few coefficients passed to comma initializer (operator<<)"); + } + + /** \returns the built matrix once all its coefficients have been set. + * Calling finished is 100% optional. Its purpose is to write expressions + * like this: + * \code + * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished()); + * \endcode + */ + inline XprType& finished() { return m_xpr; } + + XprType& m_xpr; // target expression + Index m_row; // current row id + Index m_col; // current col id + Index m_currentBlockRows; // current block height +}; + +/** \anchor MatrixBaseCommaInitRef + * Convenient operator to set the coefficients of a matrix. + * + * The coefficients must be provided in a row major order and exactly match + * the size of the matrix. Otherwise an assertion is raised. + * + * Example: \include MatrixBase_set.cpp + * Output: \verbinclude MatrixBase_set.out + * + * \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary order. + * + * \sa CommaInitializer::finished(), class CommaInitializer + */ +template +inline CommaInitializer DenseBase::operator<< (const Scalar& s) +{ + return CommaInitializer(*static_cast(this), s); +} + +/** \sa operator<<(const Scalar&) */ +template +template +inline CommaInitializer +DenseBase::operator<<(const DenseBase& other) +{ + return CommaInitializer(*static_cast(this), other); +} + +} // end namespace Eigen + +#endif // EIGEN_COMMAINITIALIZER_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h index 3e7f9c1b7..a72738e55 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h @@ -24,6 +24,14 @@ namespace internal { struct constructor_without_unaligned_array_assert {}; +template void check_static_allocation_size() +{ + // if EIGEN_STACK_ALLOCATION_LIMIT is defined to 0, then no limit + #if EIGEN_STACK_ALLOCATION_LIMIT + EIGEN_STATIC_ASSERT(Size * sizeof(T) <= EIGEN_STACK_ALLOCATION_LIMIT, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG); + #endif +} + /** \internal * Static array. If the MatrixOrArrayOptions require auto-alignment, the array will be automatically aligned: * to 16 bytes boundary if the total size is a multiple of 16 bytes. @@ -38,12 +46,12 @@ struct plain_array plain_array() { - EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG); + check_static_allocation_size(); } plain_array(constructor_without_unaligned_array_assert) { - EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG); + check_static_allocation_size(); } }; @@ -76,12 +84,12 @@ struct plain_array plain_array() { EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(0xf); - EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG); + check_static_allocation_size(); } plain_array(constructor_without_unaligned_array_assert) { - EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG); + check_static_allocation_size(); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h index 04fb21732..b08b967ff 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h @@ -589,7 +589,7 @@ struct linspaced_op_impl template EIGEN_STRONG_INLINE const Packet packetOp(Index i) const - { return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1(i),m_interPacket))); } + { return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1(Scalar(i)),m_interPacket))); } const Scalar m_low; const Scalar m_step; @@ -609,7 +609,7 @@ template struct functor_traits< linspaced_o template struct linspaced_op { typedef typename packet_traits::type Packet; - linspaced_op(const Scalar& low, const Scalar& high, DenseIndex num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/(num_steps-1))) {} + linspaced_op(const Scalar& low, const Scalar& high, DenseIndex num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/Scalar(num_steps-1))) {} template EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return impl(i); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index 6876de588..ab50c9b81 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -237,6 +237,8 @@ template class MapBase using Base::Base::operator=; }; +#undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS + } // end namespace Eigen #endif // EIGEN_MAPBASE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index 00d9e6d2b..cd6d949c4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -101,7 +101,7 @@ struct traits > template struct match { enum { HasDirectAccess = internal::has_direct_access::ret, - StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)), + StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)), InnerStrideMatch = int(StrideType::InnerStrideAtCompileTime)==int(Dynamic) || int(StrideType::InnerStrideAtCompileTime)==int(Derived::InnerStrideAtCompileTime) || (int(StrideType::InnerStrideAtCompileTime)==0 && int(Derived::InnerStrideAtCompileTime)==1), @@ -172,8 +172,12 @@ protected: } else ::new (static_cast(this)) Base(expr.data(), expr.rows(), expr.cols()); - ::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(), - StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride()); + + if(Expression::IsVectorAtCompileTime && (!PlainObjectType::IsVectorAtCompileTime) && ((Expression::Flags&RowMajorBit)!=(PlainObjectType::Flags&RowMajorBit))) + ::new (&m_stride) StrideBase(expr.innerStride(), StrideType::InnerStrideAtCompileTime==0?0:1); + else + ::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(), + StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride()); } StrideBase m_stride; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h index fba07365f..845ae1aec 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h @@ -278,21 +278,21 @@ template class TriangularView /** Efficient triangular matrix times vector/matrix product */ template - TriangularProduct + TriangularProduct operator*(const MatrixBase& rhs) const { return TriangularProduct - + (m_matrix, rhs.derived()); } /** Efficient vector/matrix times triangular matrix product */ template friend - TriangularProduct + TriangularProduct operator*(const MatrixBase& lhs, const TriangularView& rhs) { return TriangularProduct - + (lhs.derived(),rhs.m_matrix); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h index 1e6e355d6..8acca9c8c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h @@ -54,8 +54,25 @@ #endif #if defined EIGEN_USE_MKL +# include +/*Check IMKL version for compatibility: < 10.3 is not usable with Eigen*/ +# ifndef INTEL_MKL_VERSION +# undef EIGEN_USE_MKL /* INTEL_MKL_VERSION is not even defined on older versions */ +# elif INTEL_MKL_VERSION < 100305 /* the intel-mkl-103-release-notes say this was when the lapacke.h interface was added*/ +# undef EIGEN_USE_MKL +# endif +# ifndef EIGEN_USE_MKL + /*If the MKL version is too old, undef everything*/ +# undef EIGEN_USE_MKL_ALL +# undef EIGEN_USE_BLAS +# undef EIGEN_USE_LAPACKE +# undef EIGEN_USE_MKL_VML +# undef EIGEN_USE_LAPACKE_STRICT +# undef EIGEN_USE_LAPACKE +# endif +#endif -#include +#if defined EIGEN_USE_MKL #include #define EIGEN_MKL_VML_THRESHOLD 128 diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index 0088621ac..3a010ec6a 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 1 +#define EIGEN_MINOR_VERSION 2 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -289,7 +289,8 @@ namespace Eigen { #endif #ifndef EIGEN_STACK_ALLOCATION_LIMIT -#define EIGEN_STACK_ALLOCATION_LIMIT 20000 +// 131072 == 128 KB +#define EIGEN_STACK_ALLOCATION_LIMIT 131072 #endif #ifndef EIGEN_DEFAULT_IO_FORMAT diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index cacbd02fd..6c2461725 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -272,12 +272,12 @@ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size) // The defined(_mm_free) is just here to verify that this MSVC version // implements _mm_malloc/_mm_free based on the corresponding _aligned_ // functions. This may not always be the case and we just try to be safe. - #if defined(_MSC_VER) && defined(_mm_free) + #if defined(_MSC_VER) && (!defined(_WIN32_WCE)) && defined(_mm_free) result = _aligned_realloc(ptr,new_size,16); #else result = generic_aligned_realloc(ptr,new_size,old_size); #endif -#elif defined(_MSC_VER) +#elif defined(_MSC_VER) && (!defined(_WIN32_WCE)) result = _aligned_realloc(ptr,new_size,16); #else result = handmade_aligned_realloc(ptr,new_size,old_size); @@ -630,6 +630,8 @@ template class aligned_stack_memory_handler } \ void operator delete(void * ptr) throw() { Eigen::internal::conditional_aligned_free(ptr); } \ void operator delete[](void * ptr) throw() { Eigen::internal::conditional_aligned_free(ptr); } \ + void operator delete(void * ptr, std::size_t /* sz */) throw() { Eigen::internal::conditional_aligned_free(ptr); } \ + void operator delete[](void * ptr, std::size_t /* sz */) throw() { Eigen::internal::conditional_aligned_free(ptr); } \ /* in-place new and delete. since (at least afaik) there is no actual */ \ /* memory allocated we can safely let the default implementation handle */ \ /* this particular case. */ \ @@ -777,9 +779,9 @@ namespace internal { #ifdef EIGEN_CPUID -inline bool cpuid_is_vendor(int abcd[4], const char* vendor) +inline bool cpuid_is_vendor(int abcd[4], const int vendor[3]) { - return abcd[1]==(reinterpret_cast(vendor))[0] && abcd[3]==(reinterpret_cast(vendor))[1] && abcd[2]==(reinterpret_cast(vendor))[2]; + return abcd[1]==vendor[0] && abcd[3]==vendor[1] && abcd[2]==vendor[2]; } inline void queryCacheSizes_intel_direct(int& l1, int& l2, int& l3) @@ -921,13 +923,16 @@ inline void queryCacheSizes(int& l1, int& l2, int& l3) { #ifdef EIGEN_CPUID int abcd[4]; + const int GenuineIntel[] = {0x756e6547, 0x49656e69, 0x6c65746e}; + const int AuthenticAMD[] = {0x68747541, 0x69746e65, 0x444d4163}; + const int AMDisbetter_[] = {0x69444d41, 0x74656273, 0x21726574}; // "AMDisbetter!" // identify the CPU vendor EIGEN_CPUID(abcd,0x0,0); int max_std_funcs = abcd[1]; - if(cpuid_is_vendor(abcd,"GenuineIntel")) + if(cpuid_is_vendor(abcd,GenuineIntel)) queryCacheSizes_intel(l1,l2,l3,max_std_funcs); - else if(cpuid_is_vendor(abcd,"AuthenticAMD") || cpuid_is_vendor(abcd,"AMDisbetter!")) + else if(cpuid_is_vendor(abcd,AuthenticAMD) || cpuid_is_vendor(abcd,AMDisbetter_)) queryCacheSizes_amd(l1,l2,l3); else // by default let's use Intel's API diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index 9fee0c919..f06653f1c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -203,6 +203,8 @@ public: * \li \c Quaternionf for \c float * \li \c Quaterniond for \c double * + * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized. + * * \sa class AngleAxis, class Transform */ @@ -344,7 +346,7 @@ class Map, _Options > /** Constructs a Mapped Quaternion object from the pointer \a coeffs * - * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order: + * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: * \code *coeffs == {x, y, z, w} \endcode * * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ @@ -464,7 +466,7 @@ QuaternionBase::_transformVector(Vector3 v) const // Note that this algorithm comes from the optimization by hand // of the conversion to a Matrix followed by a Matrix/Vector product. // It appears to be much faster than the common algorithm found - // in the litterature (30 versus 39 flops). It also requires two + // in the literature (30 versus 39 flops). It also requires two // Vector3 as temporaries. Vector3 uv = this->vec().cross(v); uv += uv; @@ -584,7 +586,7 @@ inline Derived& QuaternionBase::setFromTwoVectors(const MatrixBase::dummy_precision()) { - c = max(c,-1); + c = (max)(c,Scalar(-1)); Matrix m; m << v0.transpose(), v1.transpose(); JacobiSVD > svd(m, ComputeFullV); Vector3 axis = svd.matrixV().col(2); @@ -667,10 +669,10 @@ QuaternionBase::angularDistance(const QuaternionBase& oth { using std::acos; using std::abs; - double d = abs(this->dot(other)); - if (d>=1.0) + Scalar d = abs(this->dot(other)); + if (d>=Scalar(1)) return Scalar(0); - return static_cast(2 * acos(d)); + return Scalar(2) * acos(d); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 498fea41a..56f361566 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -194,9 +194,9 @@ public: /** type of the matrix used to represent the linear part of the transformation */ typedef Matrix LinearMatrixType; /** type of read/write reference to the linear part of the transformation */ - typedef Block LinearPart; + typedef Block LinearPart; /** type of read reference to the linear part of the transformation */ - typedef const Block ConstLinearPart; + typedef const Block ConstLinearPart; /** type of read/write reference to the affine part of the transformation */ typedef typename internal::conditional& src, const MatrixBase& dst, boo const Index n = src.cols(); // number of measurements // required for demeaning ... - const RealScalar one_over_n = 1 / static_cast(n); + const RealScalar one_over_n = RealScalar(1) / static_cast(n); // computation of mean const VectorType src_mean = src.rowwise().sum() * one_over_n; @@ -136,16 +136,16 @@ umeyama(const MatrixBase& src, const MatrixBase& dst, boo // Eq. (39) VectorType S = VectorType::Ones(m); - if (sigma.determinant()<0) S(m-1) = -1; + if (sigma.determinant() 0 ) { + if ( svd.matrixU().determinant() * svd.matrixV().determinant() > Scalar(0) ) { Rt.block(0,0,m,m).noalias() = svd.matrixU()*svd.matrixV().transpose(); } else { - const Scalar s = S(m-1); S(m-1) = -1; + const Scalar s = S(m-1); S(m-1) = Scalar(-1); Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); S(m-1) = s; } @@ -156,7 +156,7 @@ umeyama(const MatrixBase& src, const MatrixBase& dst, boo if (with_scaling) { // Eq. (42) - const Scalar c = 1/src_var * svd.singularValues().dot(S); + const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S); // Eq. (41) Rt.col(m).head(m) = dst_mean; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Householder/BlockHouseholder.h b/gtsam/3rdparty/Eigen/Eigen/src/Householder/BlockHouseholder.h index 1991c6527..60dbea5f5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Householder/BlockHouseholder.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Householder/BlockHouseholder.h @@ -48,7 +48,7 @@ void apply_block_householder_on_the_left(MatrixType& mat, const VectorsType& vec typedef typename MatrixType::Index Index; enum { TFactorSize = MatrixType::ColsAtCompileTime }; Index nbVecs = vectors.cols(); - Matrix T(nbVecs,nbVecs); + Matrix T(nbVecs,nbVecs); make_block_householder_triangular_factor(T, vectors, hCoeffs); const TriangularView& V(vectors); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h index 6fc6ab852..2b9fb7f88 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h @@ -61,6 +61,7 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, VectorType s(n), t(n); RealScalar tol2 = tol*tol; + RealScalar eps2 = NumTraits::epsilon()*NumTraits::epsilon(); int i = 0; int restarts = 0; @@ -69,7 +70,7 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, Scalar rho_old = rho; rho = r0.dot(r); - if (internal::isMuchSmallerThan(rho,r0_sqnorm)) + if (abs(rho) < eps2*r0_sqnorm) { // The new residual vector became too orthogonal to the arbitrarily choosen direction r0 // Let's restart with a new r0: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h index dfe25f424..79ab6a8c8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h @@ -20,10 +20,11 @@ namespace Eigen { * * \param MatrixType the type of the matrix of which we are computing the LU decomposition * - * This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A - * is decomposed as A = PLUQ where L is unit-lower-triangular, U is upper-triangular, and P and Q - * are permutation matrices. This is a rank-revealing LU decomposition. The eigenvalues (diagonal - * coefficients) of U are sorted in such a way that any zeros are at the end. + * This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A is + * decomposed as \f$ A = P^{-1} L U Q^{-1} \f$ where L is unit-lower-triangular, U is + * upper-triangular, and P and Q are permutation matrices. This is a rank-revealing LU + * decomposition. The eigenvalues (diagonal coefficients) of U are sorted in such a way that any + * zeros are at the end. * * This decomposition provides the generic approach to solving systems of linear equations, computing * the rank, invertibility, inverse, kernel, and determinant. @@ -511,8 +512,8 @@ typename internal::traits::Scalar FullPivLU::determinant } /** \returns the matrix represented by the decomposition, - * i.e., it returns the product: P^{-1} L U Q^{-1}. - * This function is provided for debug purpose. */ + * i.e., it returns the product: \f$ P^{-1} L U Q^{-1} \f$. + * This function is provided for debug purposes. */ template MatrixType FullPivLU::reconstructedMatrix() const { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Ordering.h b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Ordering.h index b4da6531a..f3c31f9cb 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Ordering.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Ordering.h @@ -109,7 +109,7 @@ class NaturalOrdering * \class COLAMDOrdering * * Functor computing the \em column \em approximate \em minimum \em degree ordering - * The matrix should be in column-major format + * The matrix should be in column-major and \b compressed format (see SparseMatrix::makeCompressed()). */ template class COLAMDOrdering @@ -118,10 +118,14 @@ class COLAMDOrdering typedef PermutationMatrix PermutationType; typedef Matrix IndexVector; - /** Compute the permutation vector form a sparse matrix */ + /** Compute the permutation vector \a perm form the sparse matrix \a mat + * \warning The input sparse matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()). + */ template void operator() (const MatrixType& mat, PermutationType& perm) { + eigen_assert(mat.isCompressed() && "COLAMDOrdering requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to COLAMDOrdering"); + Index m = mat.rows(); Index n = mat.cols(); Index nnz = mat.nonZeros(); @@ -132,12 +136,12 @@ class COLAMDOrdering Index stats [COLAMD_STATS]; internal::colamd_set_defaults(knobs); - Index info; IndexVector p(n+1), A(Alen); for(Index i=0; i <= n; i++) p(i) = mat.outerIndexPtr()[i]; for(Index i=0; i < nnz; i++) A(i) = mat.innerIndexPtr()[i]; // Call Colamd routine to compute the ordering - info = internal::colamd(m, n, Alen, A.data(), p.data(), knobs, stats); + Index info = internal::colamd(m, n, Alen, A.data(), p.data(), knobs, stats); + EIGEN_UNUSED_VARIABLE(info); eigen_assert( info && "COLAMD failed " ); perm.resize(n); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index bec85810c..773d1df8f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -76,7 +76,8 @@ template class ColPivHouseholderQR m_colsTranspositions(), m_temp(), m_colSqNorms(), - m_isInitialized(false) {} + m_isInitialized(false), + m_usePrescribedThreshold(false) {} /** \brief Default Constructor with memory preallocation * diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index f44995cd3..dff9e44eb 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -375,17 +375,19 @@ struct svd_precondition_2x2_block_to_be_real Scalar z; JacobiRotation rot; RealScalar n = sqrt(numext::abs2(work_matrix.coeff(p,p)) + numext::abs2(work_matrix.coeff(q,p))); + if(n==0) { z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); work_matrix.row(p) *= z; if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z); if(work_matrix.coeff(q,q)!=Scalar(0)) + { z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); - else - z = Scalar(0); - work_matrix.row(q) *= z; - if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z); + work_matrix.row(q) *= z; + if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z); + } + // otherwise the second row is already zero, so we have nothing to do. } else { @@ -415,6 +417,7 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q, JacobiRotation *j_right) { using std::sqrt; + using std::abs; Matrix m; m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)), numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q)); @@ -428,9 +431,11 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q, } else { - RealScalar u = d / t; - rot1.c() = RealScalar(1) / sqrt(RealScalar(1) + numext::abs2(u)); - rot1.s() = rot1.c() * u; + RealScalar t2d2 = numext::hypot(t,d); + rot1.c() = abs(t)/t2d2; + rot1.s() = d/t2d2; + if(tmakeJacobi(m,0,1); @@ -531,8 +536,9 @@ template class JacobiSVD JacobiSVD() : m_isInitialized(false), m_isAllocated(false), + m_usePrescribedThreshold(false), m_computationOptions(0), - m_rows(-1), m_cols(-1) + m_rows(-1), m_cols(-1), m_diagSize(0) {} @@ -545,6 +551,7 @@ template class JacobiSVD JacobiSVD(Index rows, Index cols, unsigned int computationOptions = 0) : m_isInitialized(false), m_isAllocated(false), + m_usePrescribedThreshold(false), m_computationOptions(0), m_rows(-1), m_cols(-1) { @@ -564,6 +571,7 @@ template class JacobiSVD JacobiSVD(const MatrixType& matrix, unsigned int computationOptions = 0) : m_isInitialized(false), m_isAllocated(false), + m_usePrescribedThreshold(false), m_computationOptions(0), m_rows(-1), m_cols(-1) { @@ -665,6 +673,69 @@ template class JacobiSVD eigen_assert(m_isInitialized && "JacobiSVD is not initialized."); return m_nonzeroSingularValues; } + + /** \returns the rank of the matrix of which \c *this is the SVD. + * + * \note This method has to determine which singular values should be considered nonzero. + * For that, it uses the threshold value that you can control by calling + * setThreshold(const RealScalar&). + */ + inline Index rank() const + { + using std::abs; + eigen_assert(m_isInitialized && "JacobiSVD is not initialized."); + if(m_singularValues.size()==0) return 0; + RealScalar premultiplied_threshold = m_singularValues.coeff(0) * threshold(); + Index i = m_nonzeroSingularValues-1; + while(i>=0 && m_singularValues.coeff(i) < premultiplied_threshold) --i; + return i+1; + } + + /** Allows to prescribe a threshold to be used by certain methods, such as rank() and solve(), + * which need to determine when singular values are to be considered nonzero. + * This is not used for the SVD decomposition itself. + * + * When it needs to get the threshold value, Eigen calls threshold(). + * The default is \c NumTraits::epsilon() + * + * \param threshold The new value to use as the threshold. + * + * A singular value will be considered nonzero if its value is strictly greater than + * \f$ \vert singular value \vert \leqslant threshold \times \vert max singular value \vert \f$. + * + * If you want to come back to the default behavior, call setThreshold(Default_t) + */ + JacobiSVD& setThreshold(const RealScalar& threshold) + { + m_usePrescribedThreshold = true; + m_prescribedThreshold = threshold; + return *this; + } + + /** Allows to come back to the default behavior, letting Eigen use its default formula for + * determining the threshold. + * + * You should pass the special object Eigen::Default as parameter here. + * \code svd.setThreshold(Eigen::Default); \endcode + * + * See the documentation of setThreshold(const RealScalar&). + */ + JacobiSVD& setThreshold(Default_t) + { + m_usePrescribedThreshold = false; + return *this; + } + + /** Returns the threshold that will be used by certain methods such as rank(). + * + * See the documentation of setThreshold(const RealScalar&). + */ + RealScalar threshold() const + { + eigen_assert(m_isInitialized || m_usePrescribedThreshold); + return m_usePrescribedThreshold ? m_prescribedThreshold + : (std::max)(1,m_diagSize)*NumTraits::epsilon(); + } inline Index rows() const { return m_rows; } inline Index cols() const { return m_cols; } @@ -677,11 +748,12 @@ template class JacobiSVD MatrixVType m_matrixV; SingularValuesType m_singularValues; WorkMatrixType m_workMatrix; - bool m_isInitialized, m_isAllocated; + bool m_isInitialized, m_isAllocated, m_usePrescribedThreshold; bool m_computeFullU, m_computeThinU; bool m_computeFullV, m_computeThinV; unsigned int m_computationOptions; Index m_nonzeroSingularValues, m_rows, m_cols, m_diagSize; + RealScalar m_prescribedThreshold; template friend struct internal::svd_precondition_2x2_block_to_be_real; @@ -764,6 +836,11 @@ JacobiSVD::compute(const MatrixType& matrix, unsig 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. ***/ @@ -833,6 +910,8 @@ JacobiSVD::compute(const MatrixType& matrix, unsig if(computeV()) m_matrixV.col(pos).swap(m_matrixV.col(i)); } } + + m_singularValues *= scale; m_isInitialized = true; return *this; @@ -854,11 +933,11 @@ struct solve_retval, Rhs> // So A^{-1} = V S^{-1} U^* Matrix tmp; - Index nonzeroSingVals = dec().nonzeroSingularValues(); + Index rank = dec().rank(); - tmp.noalias() = dec().matrixU().leftCols(nonzeroSingVals).adjoint() * rhs(); - tmp = dec().singularValues().head(nonzeroSingVals).asDiagonal().inverse() * tmp; - dst = dec().matrixV().leftCols(nonzeroSingVals) * tmp; + tmp.noalias() = dec().matrixU().leftCols(rank).adjoint() * rhs(); + tmp = dec().singularValues().head(rank).asDiagonal().inverse() * tmp; + dst = dec().matrixV().leftCols(rank) * tmp; } }; } // end namespace internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h index f41d7e010..e1f96ba5a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h @@ -37,6 +37,7 @@ class SimplicialCholeskyBase : internal::noncopyable { public: typedef typename internal::traits::MatrixType MatrixType; + typedef typename internal::traits::OrderingType OrderingType; enum { UpLo = internal::traits::UpLo }; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; @@ -240,15 +241,16 @@ class SimplicialCholeskyBase : internal::noncopyable RealScalar m_shiftScale; }; -template class SimplicialLLT; -template class SimplicialLDLT; -template class SimplicialCholesky; +template > class SimplicialLLT; +template > class SimplicialLDLT; +template > class SimplicialCholesky; namespace internal { -template struct traits > +template struct traits > { typedef _MatrixType MatrixType; + typedef _Ordering OrderingType; enum { UpLo = _UpLo }; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::Index Index; @@ -259,9 +261,10 @@ template struct traits struct traits > +template struct traits > { typedef _MatrixType MatrixType; + typedef _Ordering OrderingType; enum { UpLo = _UpLo }; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::Index Index; @@ -272,9 +275,10 @@ template struct traits struct traits > +template struct traits > { typedef _MatrixType MatrixType; + typedef _Ordering OrderingType; enum { UpLo = _UpLo }; }; @@ -294,11 +298,12 @@ template struct traits * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower * or Upper. Default is Lower. + * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<> * - * \sa class SimplicialLDLT + * \sa class SimplicialLDLT, class AMDOrdering, class NaturalOrdering */ -template - class SimplicialLLT : public SimplicialCholeskyBase > +template + class SimplicialLLT : public SimplicialCholeskyBase > { public: typedef _MatrixType MatrixType; @@ -382,11 +387,12 @@ public: * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower * or Upper. Default is Lower. + * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<> * - * \sa class SimplicialLLT + * \sa class SimplicialLLT, class AMDOrdering, class NaturalOrdering */ -template - class SimplicialLDLT : public SimplicialCholeskyBase > +template + class SimplicialLDLT : public SimplicialCholeskyBase > { public: typedef _MatrixType MatrixType; @@ -467,8 +473,8 @@ public: * * \sa class SimplicialLDLT, class SimplicialLLT */ -template - class SimplicialCholesky : public SimplicialCholeskyBase > +template + class SimplicialCholesky : public SimplicialCholeskyBase > { public: typedef _MatrixType MatrixType; @@ -612,15 +618,13 @@ void SimplicialCholeskyBase::ordering(const MatrixType& a, CholMatrixTy { eigen_assert(a.rows()==a.cols()); const Index size = a.rows(); - // TODO allows to configure the permutation // Note that amd compute the inverse permutation { CholMatrixType C; C = a.template selfadjointView(); - // remove diagonal entries: - // seems not to be needed - // C.prune(keep_diag()); - internal::minimum_degree_ordering(C, m_Pinv); + + OrderingType ordering; + ordering(C,m_Pinv); } if(m_Pinv.size()>0) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h index 3321fab4a..a667cb56e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h @@ -51,8 +51,8 @@ class CompressedStorage CompressedStorage& operator=(const CompressedStorage& other) { resize(other.size()); - memcpy(m_values, other.m_values, m_size * sizeof(Scalar)); - memcpy(m_indices, other.m_indices, m_size * sizeof(Index)); + internal::smart_copy(other.m_values, other.m_values + m_size, m_values); + internal::smart_copy(other.m_indices, other.m_indices + m_size, m_indices); return *this; } @@ -83,10 +83,10 @@ class CompressedStorage reallocate(m_size); } - void resize(size_t size, float reserveSizeFactor = 0) + void resize(size_t size, double reserveSizeFactor = 0) { if (m_allocatedSize::InnerIterator typedef internal::sparse_cwise_binary_op_inner_iterator_selector< BinaryOp,Lhs,Rhs, InnerIterator> Base; - EIGEN_STRONG_INLINE InnerIterator(const CwiseBinaryOpImpl& binOp, Index outer) + // NOTE: we have to prefix Index by "typename Lhs::" to avoid an ICE with VC11 + EIGEN_STRONG_INLINE InnerIterator(const CwiseBinaryOpImpl& binOp, typename Lhs::Index outer) : Base(binOp.derived(),outer) {} }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h index 54fd633a1..78411db98 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h @@ -19,7 +19,10 @@ template struct SparseDenseProductRet template struct SparseDenseProductReturnType { - typedef SparseDenseOuterProduct Type; + typedef typename internal::conditional< + Lhs::IsRowMajor, + SparseDenseOuterProduct, + SparseDenseOuterProduct >::type Type; }; template struct DenseSparseProductReturnType @@ -29,7 +32,10 @@ template struct DenseSparseProductRet template struct DenseSparseProductReturnType { - typedef SparseDenseOuterProduct Type; + typedef typename internal::conditional< + Rhs::IsRowMajor, + SparseDenseOuterProduct, + SparseDenseOuterProduct >::type Type; }; namespace internal { @@ -114,17 +120,30 @@ class SparseDenseOuterProduct::InnerIterator : public _LhsNes typedef typename SparseDenseOuterProduct::Index Index; public: EIGEN_STRONG_INLINE InnerIterator(const SparseDenseOuterProduct& prod, Index outer) - : Base(prod.lhs(), 0), m_outer(outer), m_factor(prod.rhs().coeff(outer)) - { - } + : Base(prod.lhs(), 0), m_outer(outer), m_factor(get(prod.rhs(), outer, typename internal::traits::StorageKind() )) + { } inline Index outer() const { return m_outer; } - inline Index row() const { return Transpose ? Base::row() : m_outer; } - inline Index col() const { return Transpose ? m_outer : Base::row(); } + inline Index row() const { return Transpose ? m_outer : Base::index(); } + inline Index col() const { return Transpose ? Base::index() : m_outer; } inline Scalar value() const { return Base::value() * m_factor; } protected: + static Scalar get(const _RhsNested &rhs, Index outer, Dense = Dense()) + { + return rhs.coeff(outer); + } + + static Scalar get(const _RhsNested &rhs, Index outer, Sparse = Sparse()) + { + typename Traits::_RhsNested::InnerIterator it(rhs, outer); + if (it && it.index()==0) + return it.value(); + + return Scalar(0); + } + Index m_outer; Scalar m_factor; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h index 01ce0dcfe..ba5e3a9b6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h @@ -940,7 +940,7 @@ void set_from_triplets(const InputIterator& begin, const InputIterator& end, Spa enum { IsRowMajor = SparseMatrixType::IsRowMajor }; typedef typename SparseMatrixType::Scalar Scalar; typedef typename SparseMatrixType::Index Index; - SparseMatrix trMat(mat.rows(),mat.cols()); + SparseMatrix trMat(mat.rows(),mat.cols()); if(begin!=end) { @@ -1178,7 +1178,7 @@ EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_Index>::Scalar& Sparse size_t p = m_outerIndex[outer+1]; ++m_outerIndex[outer+1]; - float reallocRatio = 1; + double reallocRatio = 1; if (m_data.allocatedSize()<=m_data.size()) { // if there is no preallocated memory, let's reserve a minimum of 32 elements @@ -1190,13 +1190,13 @@ EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_Index>::Scalar& Sparse { // we need to reallocate the data, to reduce multiple reallocations // we use a smart resize algorithm based on the current filling ratio - // in addition, we use float to avoid integers overflows - float nnzEstimate = float(m_outerIndex[outer])*float(m_outerSize)/float(outer+1); - reallocRatio = (nnzEstimate-float(m_data.size()))/float(m_data.size()); + // in addition, we use double to avoid integers overflows + double nnzEstimate = double(m_outerIndex[outer])*double(m_outerSize)/double(outer+1); + reallocRatio = (nnzEstimate-double(m_data.size()))/double(m_data.size()); // furthermore we bound the realloc ratio to: // 1) reduce multiple minor realloc when the matrix is almost filled // 2) avoid to allocate too much memory when the matrix is almost empty - reallocRatio = (std::min)((std::max)(reallocRatio,1.5f),8.f); + reallocRatio = (std::min)((std::max)(reallocRatio,1.5),8.); } } m_data.resize(m_data.size()+1,reallocRatio); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTranspose.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTranspose.h index 7c300ee8d..76d031d52 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTranspose.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTranspose.h @@ -26,7 +26,7 @@ template class TransposeImpl inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); } }; -// NOTE: VC10 trigger an ICE if don't put typename TransposeImpl:: in front of Index, +// NOTE: VC10 and VC11 trigger an ICE if don't put typename TransposeImpl:: in front of Index, // a typedef typename TransposeImpl::Index Index; // does not fix the issue. // An alternative is to define the nested class in the parent class itself. @@ -40,8 +40,8 @@ template class TransposeImpl::InnerItera EIGEN_STRONG_INLINE InnerIterator(const TransposeImpl& trans, typename TransposeImpl::Index outer) : Base(trans.derived().nestedExpression(), outer) {} - Index row() const { return Base::col(); } - Index col() const { return Base::row(); } + typename TransposeImpl::Index row() const { return Base::col(); } + typename TransposeImpl::Index col() const { return Base::row(); } }; template class TransposeImpl::ReverseInnerIterator @@ -54,8 +54,8 @@ template class TransposeImpl::ReverseInn EIGEN_STRONG_INLINE ReverseInnerIterator(const TransposeImpl& xpr, typename TransposeImpl::Index outer) : Base(xpr.derived().nestedExpression(), outer) {} - Index row() const { return Base::col(); } - Index col() const { return Base::row(); } + typename TransposeImpl::Index row() const { return Base::col(); } + typename TransposeImpl::Index col() const { return Base::row(); } }; } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h index 05023858b..0ba471320 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h @@ -84,8 +84,10 @@ template class DenseTimeSparseProduct; template class SparseDenseOuterProduct; template struct SparseSparseProductReturnType; -template::ColsAtCompileTime> struct DenseSparseProductReturnType; -template::ColsAtCompileTime> struct SparseDenseProductReturnType; +template::ColsAtCompileTime,internal::traits::RowsAtCompileTime)> struct DenseSparseProductReturnType; +template::ColsAtCompileTime,internal::traits::RowsAtCompileTime)> struct SparseDenseProductReturnType; template class SparseSymmetricPermutationProduct; namespace internal { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h index afda43bfc..4c6553bf2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h @@ -2,7 +2,7 @@ // for linear algebra. // // Copyright (C) 2012-2013 Desire Nuentsa -// Copyright (C) 2012-2013 Gael Guennebaud +// Copyright (C) 2012-2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -58,6 +58,7 @@ namespace internal { * \tparam _OrderingType The fill-reducing ordering method. See the \link OrderingMethods_Module * OrderingMethods \endlink module for the list of built-in and external ordering methods. * + * \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()). * */ template @@ -77,10 +78,23 @@ class SparseQR SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false) { } + /** Construct a QR factorization of the matrix \a mat. + * + * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()). + * + * \sa compute() + */ SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false) { compute(mat); } + + /** Computes the QR factorization of the sparse matrix \a mat. + * + * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()). + * + * \sa analyzePattern(), factorize() + */ void compute(const MatrixType& mat) { analyzePattern(mat); @@ -166,7 +180,7 @@ class SparseQR y.bottomRows(y.rows()-rank).setZero(); // Apply the column permutation - if (m_perm_c.size()) dest.topRows(cols()) = colsPermutation() * y.topRows(cols()); + if (m_perm_c.size()) dest = colsPermutation() * y.topRows(cols()); else dest = y.topRows(cols()); m_info = Success; @@ -206,7 +220,7 @@ class SparseQR /** \brief Reports whether previous computation was successful. * - * \returns \c Success if computation was succesful, + * \returns \c Success if computation was successful, * \c NumericalIssue if the QR factorization reports a numerical problem * \c InvalidInput if the input matrix is invalid * @@ -255,20 +269,24 @@ class SparseQR }; /** \brief Preprocessing step of a QR factorization + * + * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()). * * In this step, the fill-reducing permutation is computed and applied to the columns of A - * and the column elimination tree is computed as well. Only the sparcity pattern of \a mat is exploited. + * and the column elimination tree is computed as well. Only the sparsity pattern of \a mat is exploited. * * \note In this step it is assumed that there is no empty row in the matrix \a mat. */ 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"); // Compute the column fill reducing ordering OrderingType ord; ord(mat, m_perm_c); Index n = mat.cols(); Index m = mat.rows(); + Index diagSize = (std::min)(m,n); if (!m_perm_c.size()) { @@ -280,20 +298,20 @@ void SparseQR::analyzePattern(const MatrixType& mat) m_outputPerm_c = m_perm_c.inverse(); internal::coletree(mat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data()); - m_R.resize(n, n); - m_Q.resize(m, n); + m_R.resize(m, n); + m_Q.resize(m, diagSize); // Allocate space for nonzero elements : rough estimation m_R.reserve(2*mat.nonZeros()); //FIXME Get a more accurate estimation through symbolic factorization with the etree m_Q.reserve(2*mat.nonZeros()); - m_hcoeffs.resize(n); + m_hcoeffs.resize(diagSize); m_analysisIsok = true; } /** \brief Performs the numerical QR factorization of the input matrix * * The function SparseQR::analyzePattern(const MatrixType&) must have been called beforehand with - * a matrix having the same sparcity pattern than \a mat. + * a matrix having the same sparsity pattern than \a mat. * * \param mat The sparse column-major matrix */ @@ -306,11 +324,12 @@ void SparseQR::factorize(const MatrixType& mat) eigen_assert(m_analysisIsok && "analyzePattern() should be called before this step"); Index m = mat.rows(); Index n = mat.cols(); - IndexVector mark(m); mark.setConstant(-1); // Record the visited nodes - IndexVector Ridx(n), Qidx(m); // Store temporarily the row indexes for the current column of R and Q - 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 - bool found_diag; + Index diagSize = (std::min)(m,n); + IndexVector mark((std::max)(m,n)); mark.setConstant(-1); // Record the visited nodes + IndexVector Ridx(n), Qidx(m); // Store temporarily the row indexes for the current column of R and Q + 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_pmat = mat; m_pmat.uncompress(); // To have the innerNonZeroPtr allocated @@ -322,7 +341,7 @@ void SparseQR::factorize(const MatrixType& mat) m_pmat.innerNonZeroPtr()[p] = mat.outerIndexPtr()[i+1] - mat.outerIndexPtr()[i]; } - /* Compute the default threshold, see : + /* Compute the default threshold as in MatLab, see: * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3 */ @@ -330,24 +349,24 @@ 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()); - m_threshold = 20 * (m + n) * max2Norm * NumTraits::epsilon(); + pivotThreshold = 20 * (m + n) * max2Norm * NumTraits::epsilon(); } // Initialize the numerical permutation m_pivotperm.setIdentity(n); 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 < (std::min)(n,m); ++col) + for (Index col = 0; col < n; ++col) { mark.setConstant(-1); m_R.startVec(col); - m_Q.startVec(col); mark(nonzeroCol) = col; Qidx(0) = nonzeroCol; nzcolR = 0; nzcolQ = 1; - found_diag = col>=m; + bool found_diag = nonzeroCol>=m; tval.setZero(); // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e., @@ -356,7 +375,7 @@ void SparseQR::factorize(const MatrixType& mat) // 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) { - Index curIdx = nonzeroCol ; + Index curIdx = nonzeroCol; if(itp) curIdx = itp.row(); if(curIdx == nonzeroCol) found_diag = true; @@ -398,7 +417,7 @@ void SparseQR::factorize(const MatrixType& mat) // Browse all the indexes of R(:,col) in reverse order for (Index i = nzcolR-1; i >= 0; i--) { - Index curIdx = m_pivotperm.indices()(Ridx(i)); + Index curIdx = Ridx(i); // Apply the curIdx-th householder vector to the current column (temporarily stored into tval) Scalar tdot(0); @@ -427,33 +446,37 @@ void SparseQR::factorize(const MatrixType& mat) } } } // End update current column - - // Compute the Householder reflection that eliminate the current column - // FIXME this step should call the Householder module. + Scalar tau; - RealScalar beta; - Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0); + RealScalar beta = 0; - // First, the squared norm of Q((col+1):m, col) - RealScalar sqrNorm = 0.; - for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq))); - - if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0)) + if(nonzeroCol < diagSize) { - tau = RealScalar(0); - beta = numext::real(c0); - tval(Qidx(0)) = 1; - } - else - { - beta = std::sqrt(numext::abs2(c0) + sqrNorm); - if(numext::real(c0) >= RealScalar(0)) - beta = -beta; - tval(Qidx(0)) = 1; - for (Index itq = 1; itq < nzcolQ; ++itq) - tval(Qidx(itq)) /= (c0 - beta); - tau = numext::conj((beta-c0) / beta); - + // Compute the Householder reflection that eliminate the current column + // FIXME this step should call the Householder module. + Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0); + + // First, the squared norm of Q((col+1):m, col) + RealScalar sqrNorm = 0.; + 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; + } + else + { + using std::sqrt; + beta = sqrt(numext::abs2(c0) + sqrNorm); + if(numext::real(c0) >= RealScalar(0)) + beta = -beta; + tval(Qidx(0)) = 1; + for (Index itq = 1; itq < nzcolQ; ++itq) + tval(Qidx(itq)) /= (c0 - beta); + tau = numext::conj((beta-c0) / beta); + + } } // Insert values in R @@ -467,24 +490,25 @@ void SparseQR::factorize(const MatrixType& mat) } } - if(abs(beta) >= m_threshold) + if(nonzeroCol < diagSize && abs(beta) >= pivotThreshold) { m_R.insertBackByOuterInner(col, nonzeroCol) = beta; - nonzeroCol++; // The householder coefficient - m_hcoeffs(col) = tau; + m_hcoeffs(nonzeroCol) = tau; // Record the householder reflections for (Index itq = 0; itq < nzcolQ; ++itq) { Index iQ = Qidx(itq); - m_Q.insertBackByOuterInnerUnordered(col,iQ) = tval(iQ); + m_Q.insertBackByOuterInnerUnordered(nonzeroCol,iQ) = tval(iQ); tval(iQ) = Scalar(0.); - } + } + nonzeroCol++; + if(nonzeroCol::factorize(const MatrixType& mat) } } + m_hcoeffs.tail(diagSize-nonzeroCol).setZero(); + // Finalize the column pointers of the sparse matrices R and Q m_Q.finalize(); m_Q.makeCompressed(); @@ -561,14 +587,16 @@ struct SparseQR_QProduct : ReturnByValue void evalTo(DesType& res) const { + Index m = m_qr.rows(); Index n = m_qr.cols(); + Index diagSize = (std::min)(m,n); res = m_other; if (m_transpose) { eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes"); //Compute res = Q' * other column by column for(Index j = 0; j < res.cols(); j++){ - for (Index k = 0; k < n; k++) + for (Index k = 0; k < diagSize; k++) { Scalar tau = Scalar(0); tau = m_qr.m_Q.col(k).dot(res.col(j)); @@ -581,10 +609,10 @@ struct SparseQR_QProduct : ReturnByValue=0; k--) + for (Index k = diagSize-1; k >=0; k--) { Scalar tau = Scalar(0); tau = m_qr.m_Q.col(k).dot(res.col(j)); @@ -618,7 +646,7 @@ struct SparseQRMatrixQReturnType : public EigenBase(m_qr); } inline Index rows() const { return m_qr.rows(); } - inline Index cols() const { return m_qr.cols(); } + inline Index cols() const { return (std::min)(m_qr.rows(),m_qr.cols()); } // To use for operations with the transpose of Q SparseQRMatrixQTransposeReturnType transpose() const { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h index 4ee8e5c10..aaf66330b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h @@ -11,7 +11,7 @@ #ifndef EIGEN_STDDEQUE_H #define EIGEN_STDDEQUE_H -#include "Eigen/src/StlSupport/details.h" +#include "details.h" // Define the explicit instantiation (e.g. necessary for the Intel compiler) #if defined(__INTEL_COMPILER) || defined(__GNUC__) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h index 627381ece..3c742430c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h @@ -10,7 +10,7 @@ #ifndef EIGEN_STDLIST_H #define EIGEN_STDLIST_H -#include "Eigen/src/StlSupport/details.h" +#include "details.h" // Define the explicit instantiation (e.g. necessary for the Intel compiler) #if defined(__INTEL_COMPILER) || defined(__GNUC__) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h index 40a9abefa..611664a2e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h @@ -11,7 +11,7 @@ #ifndef EIGEN_STDVECTOR_H #define EIGEN_STDVECTOR_H -#include "Eigen/src/StlSupport/details.h" +#include "details.h" /** * This section contains a convenience MACRO which allows an easy specialization of diff --git a/gtsam/3rdparty/Eigen/blas/README.txt b/gtsam/3rdparty/Eigen/blas/README.txt index 07a8bd92a..63a5203b9 100644 --- a/gtsam/3rdparty/Eigen/blas/README.txt +++ b/gtsam/3rdparty/Eigen/blas/README.txt @@ -1,9 +1,6 @@ This directory contains a BLAS library built on top of Eigen. -This is currently a work in progress which is far to be ready for use, -but feel free to contribute to it if you wish. - This module is not built by default. In order to compile it, you need to type 'make blas' from within your build dir. diff --git a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake index 228d29e97..11ecc9585 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake @@ -41,7 +41,7 @@ endif() # copy ctest properties, which currently # o raise the warning levels -configure_file(${CMAKE_BINARY_DIR}/DartConfiguration.tcl ${CMAKE_BINARY_DIR}/DartConfiguration.tcl) +configure_file(${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl ${CMAKE_BINARY_DIR}/DartConfiguration.tcl) # restore default CMAKE_MAKE_PROGRAM set(CMAKE_MAKE_PROGRAM ${CMAKE_MAKE_PROGRAM_SAVE}) @@ -50,7 +50,7 @@ set(CMAKE_MAKE_PROGRAM ${CMAKE_MAKE_PROGRAM_SAVE}) set(CMAKE_MAKE_PROGRAM_SAVE) set(EIGEN_MAKECOMMAND_PLACEHOLDER) -configure_file(${CMAKE_SOURCE_DIR}/CTestCustom.cmake.in ${CMAKE_BINARY_DIR}/CTestCustom.cmake) +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/CTestCustom.cmake.in ${CMAKE_BINARY_DIR}/CTestCustom.cmake) # some documentation of this function would be nice ei_init_testing() diff --git a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt index 694d32484..1e74e0528 100644 --- a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt +++ b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt @@ -41,8 +41,8 @@ MatrixXd::Ones(rows,cols) // ones(rows,cols) C.setOnes(rows,cols) // C = ones(rows,cols) MatrixXd::Random(rows,cols) // rand(rows,cols)*2-1 // MatrixXd::Random returns uniform random numbers in (-1, 1). C.setRandom(rows,cols) // C = rand(rows,cols)*2-1 -VectorXd::LinSpace(size,low,high) // linspace(low,high,size)' -v.setLinSpace(size,low,high) // v = linspace(low,high,size)' +VectorXd::LinSpaced(size,low,high) // linspace(low,high,size)' +v.setLinSpaced(size,low,high) // v = linspace(low,high,size)' // Matrix slicing and blocks. All expressions listed here are read/write. @@ -91,6 +91,8 @@ R.adjoint() // R' R.transpose() // R.' or conj(R') R.diagonal() // diag(R) x.asDiagonal() // diag(x) +R.transpose().colwise().reverse(); // rot90(R) +R.conjugate() // conj(R) // All the same as Matlab, but matlab doesn't have *= style operators. // Matrix-vector. Matrix-matrix. Matrix-scalar. @@ -167,6 +169,8 @@ x.cross(y) // cross(x, y) Requires #include A.cast(); // double(A) A.cast(); // single(A) A.cast(); // int32(A) +A.real(); // real(A) +A.imag(); // imag(A) // if the original type equals destination type, no work is done // Note that for most operations Eigen requires all operands to have the same type: diff --git a/gtsam/3rdparty/Eigen/doc/LinearLeastSquares.dox b/gtsam/3rdparty/Eigen/doc/LinearLeastSquares.dox deleted file mode 100644 index ab21a87ae..000000000 --- a/gtsam/3rdparty/Eigen/doc/LinearLeastSquares.dox +++ /dev/null @@ -1,27 +0,0 @@ -namespace Eigen { - -/** \eigenManualPage LinearLeastSquares Solving linear least squares problems - -lede - -\eigenAutoToc - -\section LinearLeastSquaresCopied Copied - -The best way to do least squares solving is with a SVD decomposition. Eigen provides one as the JacobiSVD class, and its solve() -is doing least-squares solving. - -Here is an example: - - - - - - -
Example:Output:
\include TutorialLinAlgSVDSolve.cpp \verbinclude TutorialLinAlgSVDSolve.out
- -For more information, including faster but less reliable methods, read our page concentrating on \ref LinearLeastSquares "linear least squares problems". - -*/ - -} diff --git a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox index 981083e96..8a2968ebb 100644 --- a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox +++ b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox @@ -62,6 +62,8 @@ run time. However, these assertions do cost time and can thus be turned off. expect that any objects passed to it are aligned. This will turn off vectorization. Not defined by default. - \b EIGEN_DONT_ALIGN_STATICALLY - disables alignment of arrays on the stack. Not defined by default, unless \c EIGEN_DONT_ALIGN is defined. + - \b EIGEN_DONT_PARALLELIZE - if defined, this disables multi-threading. This is only relevant if you enabled OpenMP. + See \ref TopicMultiThreading for details. - \b EIGEN_DONT_VECTORIZE - disables explicit vectorization when defined. Not defined by default, unless alignment is disabled by %Eigen's platform test or the user defining \c EIGEN_DONT_ALIGN. - \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. This currently @@ -69,7 +71,10 @@ run time. However, these assertions do cost time and can thus be turned off. Define it to 0 to disable. - \b EIGEN_UNROLLING_LIMIT - defines the size of a loop to enable meta unrolling. Set it to zero to disable unrolling. The size of a loop here is expressed in %Eigen's own notion of "number of FLOPS", it does not - correspond to the number of iterations or the number of instructions. The default is value 100. + correspond to the number of iterations or the number of instructions. The default is value 100. + - \b EIGEN_STACK_ALLOCATION_LIMIT - defines the maximum bytes for a buffer to be allocated on the stack. For internal + temporary buffers, dynamic memory allocation is employed as a fall back. For fixed-size matrices or arrays, exceeding + this threshold raises a compile time assertion. Use 0 to set no limit. Default is 128 KB. \section TopicPreprocessorDirectivesPlugins Plugins diff --git a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox index 835c59354..fa2a3ad8b 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox @@ -253,12 +253,15 @@ SparseMatrix A, B; B = SparseMatrix(A.transpose()) + A; \endcode -Binary coefficient wise operators can also mix sparse and dense expressions: +Some binary coefficient-wise operators can also mix sparse and dense expressions: \code sm2 = sm1.cwiseProduct(dm1); -dm2 = sm1 + dm1; +dm1 += sm1; \endcode +However, it is not yet possible to add a sparse and a dense matrix as in dm2 = sm1 + dm1. +Please write this as the equivalent dm2 = dm1; dm2 += sm1 (we plan to lift this restriction +in the next release of %Eigen). %Sparse expressions also support transposition: \code diff --git a/gtsam/3rdparty/Eigen/test/block.cpp b/gtsam/3rdparty/Eigen/test/block.cpp index 189fa5aba..9ed5d7bc5 100644 --- a/gtsam/3rdparty/Eigen/test/block.cpp +++ b/gtsam/3rdparty/Eigen/test/block.cpp @@ -10,6 +10,26 @@ #define EIGEN_NO_STATIC_ASSERT // otherwise we fail at compile time on unused paths #include "main.h" +template +typename Eigen::internal::enable_if::IsComplex,typename MatrixType::Scalar>::type +block_real_only(const MatrixType &m1, Index r1, Index r2, Index c1, Index c2, const Scalar& s1) { + // check cwise-Functions: + VERIFY_IS_APPROX(m1.row(r1).cwiseMax(s1), m1.cwiseMax(s1).row(r1)); + VERIFY_IS_APPROX(m1.col(c1).cwiseMin(s1), m1.cwiseMin(s1).col(c1)); + + VERIFY_IS_APPROX(m1.block(r1,c1,r2-r1+1,c2-c1+1).cwiseMin(s1), m1.cwiseMin(s1).block(r1,c1,r2-r1+1,c2-c1+1)); + VERIFY_IS_APPROX(m1.block(r1,c1,r2-r1+1,c2-c1+1).cwiseMax(s1), m1.cwiseMax(s1).block(r1,c1,r2-r1+1,c2-c1+1)); + + return Scalar(0); +} + +template +typename Eigen::internal::enable_if::IsComplex,typename MatrixType::Scalar>::type +block_real_only(const MatrixType &, Index, Index, Index, Index, const Scalar&) { + return Scalar(0); +} + + template void block(const MatrixType& m) { typedef typename MatrixType::Index Index; @@ -37,6 +57,8 @@ template void block(const MatrixType& m) Index c1 = internal::random(0,cols-1); Index c2 = internal::random(c1,cols-1); + block_real_only(m1, r1, r2, c1, c1, s1); + //check row() and col() VERIFY_IS_EQUAL(m1.col(c1).transpose(), m1.transpose().row(c1)); //check operator(), both constant and non-constant, on row() and col() @@ -51,7 +73,8 @@ template void block(const MatrixType& m) VERIFY_IS_APPROX(m1.col(c1), m1_copy.col(c1) + s1 * m1_copy.col(c2)); m1.col(c1).col(0) += s1 * m1_copy.col(c2); VERIFY_IS_APPROX(m1.col(c1), m1_copy.col(c1) + Scalar(2) * s1 * m1_copy.col(c2)); - + + //check block() Matrix b1(1,1); b1(0,0) = m1(r1,c1); diff --git a/gtsam/3rdparty/Eigen/test/cholesky.cpp b/gtsam/3rdparty/Eigen/test/cholesky.cpp index b980dc572..64bcbccc4 100644 --- a/gtsam/3rdparty/Eigen/test/cholesky.cpp +++ b/gtsam/3rdparty/Eigen/test/cholesky.cpp @@ -68,6 +68,7 @@ template void cholesky(const MatrixType& m) Index cols = m.cols(); typedef typename MatrixType::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; typedef Matrix SquareMatrixType; typedef Matrix VectorType; @@ -179,6 +180,57 @@ template void cholesky(const MatrixType& m) // restore if(sign == -1) symm = -symm; + + // check matrices coming from linear constraints with Lagrange multipliers + if(rows>=3) + { + SquareMatrixType A = symm; + int c = internal::random(0,rows-2); + A.bottomRightCorner(c,c).setZero(); + // Make sure a solution exists: + vecX.setRandom(); + vecB = A * vecX; + vecX.setZero(); + ldltlo.compute(A); + VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix()); + vecX = ldltlo.solve(vecB); + VERIFY_IS_APPROX(A * vecX, vecB); + } + + // check non-full rank matrices + if(rows>=3) + { + int r = internal::random(1,rows-1); + Matrix a = Matrix::Random(rows,r); + SquareMatrixType A = a * a.adjoint(); + // Make sure a solution exists: + vecX.setRandom(); + vecB = A * vecX; + vecX.setZero(); + ldltlo.compute(A); + VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix()); + vecX = ldltlo.solve(vecB); + VERIFY_IS_APPROX(A * vecX, vecB); + } + + // check matrices with a wide spectrum + if(rows>=3) + { + RealScalar s = (std::min)(16,std::numeric_limits::max_exponent10/8); + Matrix a = Matrix::Random(rows,rows); + Matrix d = Matrix::Random(rows); + for(int k=0; k(-s,s)); + SquareMatrixType A = a * d.asDiagonal() * a.adjoint(); + // Make sure a solution exists: + vecX.setRandom(); + vecB = A * vecX; + vecX.setZero(); + ldltlo.compute(A); + VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix()); + vecX = ldltlo.solve(vecB); + VERIFY_IS_APPROX(A * vecX, vecB); + } } // update/downdate diff --git a/gtsam/3rdparty/Eigen/test/dynalloc.cpp b/gtsam/3rdparty/Eigen/test/dynalloc.cpp index 8bbda1c94..9a6a9d140 100644 --- a/gtsam/3rdparty/Eigen/test/dynalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/dynalloc.cpp @@ -53,7 +53,7 @@ void check_aligned_new() void check_aligned_stack_alloc() { - for(int i = 1; i < 1000; i++) + for(int i = 1; i < 400; i++) { ei_declare_aligned_stack_constructed_variable(float,p,i,0); VERIFY(size_t(p)%ALIGNMENT==0); @@ -87,6 +87,32 @@ 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 @@ -102,6 +128,12 @@ 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/jacobisvd.cpp b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp index 76157c30f..7c21f0ab3 100644 --- a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp +++ b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp @@ -67,6 +67,7 @@ template void jacobisvd_solve(const MatrixType& m, unsigned int computationOptions) { typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; typedef typename MatrixType::Index Index; Index rows = m.rows(); Index cols = m.cols(); @@ -81,9 +82,90 @@ void jacobisvd_solve(const MatrixType& m, unsigned int computationOptions) RhsType rhs = RhsType::Random(rows, internal::random(1, cols)); JacobiSVD svd(m, computationOptions); + + if(internal::is_same::value) svd.setThreshold(1e-8); + else if(internal::is_same::value) svd.setThreshold(1e-4); + SolutionType x = svd.solve(rhs); + + RealScalar residual = (m*x-rhs).norm(); + // Check that there is no significantly better solution in the neighborhood of x + if(!test_isMuchSmallerThan(residual,rhs.norm())) + { + // If the residual is very small, then we have an exact solution, so we are already good. + for(int k=0;k::epsilon(); + RealScalar residual_y = (m*y-rhs).norm(); + VERIFY( test_isApprox(residual_y,residual) || residual < residual_y ); + + y.row(k) = x.row(k).array() - 2*NumTraits::epsilon(); + residual_y = (m*y-rhs).norm(); + VERIFY( test_isApprox(residual_y,residual) || residual < residual_y ); + } + } + // evaluate normal equation which works also for least-squares solutions - VERIFY_IS_APPROX(m.adjoint()*m*x,m.adjoint()*rhs); + if(internal::is_same::value) + { + // This test is not stable with single precision. + // This is probably because squaring m signicantly affects the precision. + VERIFY_IS_APPROX(m.adjoint()*m*x,m.adjoint()*rhs); + } + + // check minimal norm solutions + { + // generate a full-rank m x n problem with m MatrixType2; + typedef Matrix RhsType2; + typedef Matrix MatrixType2T; + Index rank = RankAtCompileTime2==Dynamic ? internal::random(1,cols) : Index(RankAtCompileTime2); + MatrixType2 m2(rank,cols); + int guard = 0; + do { + m2.setRandom(); + } while(m2.jacobiSvd().setThreshold(test_precision()).rank()!=rank && (++guard)<10); + VERIFY(guard<10); + RhsType2 rhs2 = RhsType2::Random(rank); + // use QR to find a reference minimal norm solution + HouseholderQR qr(m2.adjoint()); + Matrix tmp = qr.matrixQR().topLeftCorner(rank,rank).template triangularView().adjoint().solve(rhs2); + tmp.conservativeResize(cols); + tmp.tail(cols-rank).setZero(); + SolutionType x21 = qr.householderQ() * tmp; + // now check with SVD + JacobiSVD svd2(m2, computationOptions); + SolutionType x22 = svd2.solve(rhs2); + VERIFY_IS_APPROX(m2*x21, rhs2); + VERIFY_IS_APPROX(m2*x22, rhs2); + VERIFY_IS_APPROX(x21, x22); + + // Now check with a rank deficient matrix + typedef Matrix MatrixType3; + typedef Matrix RhsType3; + Index rows3 = RowsAtCompileTime3==Dynamic ? internal::random(rank+1,2*cols) : Index(RowsAtCompileTime3); + Matrix C = Matrix::Random(rows3,rank); + MatrixType3 m3 = C * m2; + RhsType3 rhs3 = C * rhs2; + JacobiSVD svd3(m3, computationOptions); + SolutionType x3 = svd3.solve(rhs3); + if(svd3.rank()!=rank) { + std::cout << m3 << "\n\n"; + std::cout << svd3.singularValues().transpose() << "\n"; + std::cout << svd3.rank() << " == " << rank << "\n"; + std::cout << x21.norm() << " == " << x3.norm() << "\n"; + } +// VERIFY_IS_APPROX(m3*x3, rhs3); + VERIFY_IS_APPROX(m3*x21, rhs3); + VERIFY_IS_APPROX(m2*x3, rhs2); + + VERIFY_IS_APPROX(x21, x3); + } } template @@ -92,10 +174,9 @@ void jacobisvd_test_all_computation_options(const MatrixType& m) if (QRPreconditioner == NoQRPreconditioner && m.rows() != m.cols()) return; JacobiSVD fullSvd(m, ComputeFullU|ComputeFullV); - - jacobisvd_check_full(m, fullSvd); - jacobisvd_solve(m, ComputeFullU | ComputeFullV); - + CALL_SUBTEST(( jacobisvd_check_full(m, fullSvd) )); + CALL_SUBTEST(( jacobisvd_solve(m, ComputeFullU | ComputeFullV) )); + #if defined __INTEL_COMPILER // remark #111: statement is unreachable #pragma warning disable 111 @@ -103,20 +184,20 @@ void jacobisvd_test_all_computation_options(const MatrixType& m) if(QRPreconditioner == FullPivHouseholderQRPreconditioner) return; - jacobisvd_compare_to_full(m, ComputeFullU, fullSvd); - jacobisvd_compare_to_full(m, ComputeFullV, fullSvd); - jacobisvd_compare_to_full(m, 0, fullSvd); + CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU, fullSvd) )); + CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullV, fullSvd) )); + CALL_SUBTEST(( jacobisvd_compare_to_full(m, 0, fullSvd) )); if (MatrixType::ColsAtCompileTime == Dynamic) { // thin U/V are only available with dynamic number of columns - jacobisvd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd); - jacobisvd_compare_to_full(m, ComputeThinV, fullSvd); - jacobisvd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd); - jacobisvd_compare_to_full(m, ComputeThinU , fullSvd); - jacobisvd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd); - jacobisvd_solve(m, ComputeFullU | ComputeThinV); - jacobisvd_solve(m, ComputeThinU | ComputeFullV); - jacobisvd_solve(m, ComputeThinU | ComputeThinV); + CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd) )); + CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinV, fullSvd) )); + CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd) )); + CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU , fullSvd) )); + CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd) )); + CALL_SUBTEST(( jacobisvd_solve(m, ComputeFullU | ComputeThinV) )); + CALL_SUBTEST(( jacobisvd_solve(m, ComputeThinU | ComputeFullV) )); + CALL_SUBTEST(( jacobisvd_solve(m, ComputeThinU | ComputeThinV) )); // test reconstruction typedef typename MatrixType::Index Index; @@ -129,12 +210,29 @@ void jacobisvd_test_all_computation_options(const MatrixType& m) template void jacobisvd(const MatrixType& a = MatrixType(), bool pickrandom = true) { - MatrixType m = pickrandom ? MatrixType::Random(a.rows(), a.cols()) : a; + MatrixType m = a; + if(pickrandom) + { + typedef typename MatrixType::Scalar Scalar; + typedef typename MatrixType::RealScalar RealScalar; + typedef typename MatrixType::Index Index; + Index diagSize = (std::min)(a.rows(), a.cols()); + RealScalar s = std::numeric_limits::max_exponent10/4; + s = internal::random(1,s); + Matrix d = Matrix::Random(diagSize); + for(Index k=0; k(-s,s)); + m = Matrix::Random(a.rows(),diagSize) * d.asDiagonal() * Matrix::Random(diagSize,a.cols()); + // cancel some coeffs + Index n = internal::random(0,m.size()-1); + for(Index i=0; i(0,m.rows()-1), internal::random(0,m.cols()-1)) = Scalar(0); + } - jacobisvd_test_all_computation_options(m); - jacobisvd_test_all_computation_options(m); - jacobisvd_test_all_computation_options(m); - jacobisvd_test_all_computation_options(m); + CALL_SUBTEST(( jacobisvd_test_all_computation_options(m) )); + CALL_SUBTEST(( jacobisvd_test_all_computation_options(m) )); + CALL_SUBTEST(( jacobisvd_test_all_computation_options(m) )); + CALL_SUBTEST(( jacobisvd_test_all_computation_options(m) )); } template void jacobisvd_verify_assert(const MatrixType& m) @@ -328,6 +426,7 @@ void test_jacobisvd() TEST_SET_BUT_UNUSED_VARIABLE(r) TEST_SET_BUT_UNUSED_VARIABLE(c) + CALL_SUBTEST_10(( jacobisvd(MatrixXd(r,c)) )); CALL_SUBTEST_7(( jacobisvd(MatrixXf(r,c)) )); CALL_SUBTEST_8(( jacobisvd(MatrixXcd(r,c)) )); (void) r; diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index f639d900b..19e81549c 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -154,59 +154,79 @@ template void check_const_correctness(const PlainObjec VERIFY( !(Ref::Flags & LvalueBit) ); } -EIGEN_DONT_INLINE void call_ref_1(Ref ) { } -EIGEN_DONT_INLINE void call_ref_2(const Ref& ) { } -EIGEN_DONT_INLINE void call_ref_3(Ref > ) { } -EIGEN_DONT_INLINE void call_ref_4(const Ref >& ) { } -EIGEN_DONT_INLINE void call_ref_5(Ref > ) { } -EIGEN_DONT_INLINE void call_ref_6(const Ref >& ) { } +template +EIGEN_DONT_INLINE void call_ref_1(Ref a, const B &b) { VERIFY_IS_EQUAL(a,b); } +template +EIGEN_DONT_INLINE void call_ref_2(const Ref& a, const B &b) { VERIFY_IS_EQUAL(a,b); } +template +EIGEN_DONT_INLINE void call_ref_3(Ref > a, const B &b) { VERIFY_IS_EQUAL(a,b); } +template +EIGEN_DONT_INLINE void call_ref_4(const Ref >& a, const B &b) { VERIFY_IS_EQUAL(a,b); } +template +EIGEN_DONT_INLINE void call_ref_5(Ref > a, const B &b) { VERIFY_IS_EQUAL(a,b); } +template +EIGEN_DONT_INLINE void call_ref_6(const Ref >& a, const B &b) { VERIFY_IS_EQUAL(a,b); } +template +EIGEN_DONT_INLINE void call_ref_7(Ref > a, const B &b) { VERIFY_IS_EQUAL(a,b); } void call_ref() { - VectorXcf ca(10); - VectorXf a(10); + VectorXcf ca = VectorXcf::Random(10); + VectorXf a = VectorXf::Random(10); + RowVectorXf b = RowVectorXf::Random(10); + MatrixXf A = MatrixXf::Random(10,10); + RowVector3f c = RowVector3f::Random(); const VectorXf& ac(a); VectorBlock ab(a,0,3); - MatrixXf A(10,10); const VectorBlock abc(a,0,3); + - VERIFY_EVALUATION_COUNT( call_ref_1(a), 0); - //call_ref_1(ac); // does not compile because ac is const - VERIFY_EVALUATION_COUNT( call_ref_1(ab), 0); - VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4)), 0); - VERIFY_EVALUATION_COUNT( call_ref_1(abc), 0); - VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3)), 0); - // call_ref_1(A.row(3)); // does not compile because innerstride!=1 - VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3)), 0); - VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3)), 0); - //call_ref_1(a+a); // does not compile for obvious reason + 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 + VERIFY_EVALUATION_COUNT( call_ref_1(ab,ab), 0); + VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4),a.head(4)), 0); + VERIFY_EVALUATION_COUNT( call_ref_1(abc,abc), 0); + VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3),A.col(3)), 0); +// call_ref_1(A.row(3)); // does not compile because innerstride!=1 + VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3),A.row(3).transpose()), 0); + VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3),A.row(3).transpose()), 0); +// call_ref_1(a+a); // does not compile for obvious reason - VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1)), 1); // evaluated into a temp - VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5)), 0); - VERIFY_EVALUATION_COUNT( call_ref_2(ac), 0); - VERIFY_EVALUATION_COUNT( call_ref_2(a), 0); - VERIFY_EVALUATION_COUNT( call_ref_2(ab), 0); - VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4)), 0); - VERIFY_EVALUATION_COUNT( call_ref_2(a+a), 1); // evaluated into a temp - VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag()), 1); // evaluated into a temp + MatrixXf tmp = A*A.col(1); + VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1), tmp), 1); // evaluated into a temp + VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5),ac.head(5)), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(ac,ac), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(a,a), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(ab,ab), 0); + VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4),a.head(4)), 0); + tmp = a+a; + VERIFY_EVALUATION_COUNT( call_ref_2(a+a,tmp), 1); // evaluated into a temp + VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag(),ca.imag()), 1); // evaluated into a temp - VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5)), 0); - VERIFY_EVALUATION_COUNT( call_ref_4(a+a), 1); // evaluated into a temp - VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag()), 0); + VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5),ac.head(5)), 0); + tmp = a+a; + VERIFY_EVALUATION_COUNT( call_ref_4(a+a,tmp), 1); // evaluated into a temp + VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag(),ca.imag()), 0); - VERIFY_EVALUATION_COUNT( call_ref_5(a), 0); - VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3)), 0); - VERIFY_EVALUATION_COUNT( call_ref_5(A), 0); - // call_ref_5(A.transpose()); // does not compile - VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2)), 0); + VERIFY_EVALUATION_COUNT( call_ref_5(a,a), 0); + VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3),a.head(3)), 0); + VERIFY_EVALUATION_COUNT( call_ref_5(A,A), 0); +// call_ref_5(A.transpose()); // does not compile + VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2),A.block(1,1,2,2)), 0); + VERIFY_EVALUATION_COUNT( call_ref_5(b,b), 0); // storage order do not match, but this is a degenerate case that should work + VERIFY_EVALUATION_COUNT( call_ref_5(a.row(3),a.row(3)), 0); - VERIFY_EVALUATION_COUNT( call_ref_6(a), 0); - VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3)), 0); - VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3)), 1); // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix - VERIFY_EVALUATION_COUNT( call_ref_6(A+A), 1); // evaluated into a temp - VERIFY_EVALUATION_COUNT( call_ref_6(A), 0); - VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose()), 1); // evaluated into a temp because the storage orders do not match - VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2)), 0); + VERIFY_EVALUATION_COUNT( call_ref_6(a,a), 0); + VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3),a.head(3)), 0); + VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3),A.row(3)), 1); // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix + tmp = A+A; + VERIFY_EVALUATION_COUNT( call_ref_6(A+A,tmp), 1); // evaluated into a temp + VERIFY_EVALUATION_COUNT( call_ref_6(A,A), 0); + VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose(),A.transpose()), 1); // evaluated into a temp because the storage orders do not match + VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2),A.block(1,1,2,2)), 0); + + VERIFY_EVALUATION_COUNT( call_ref_7(c,c), 0); } void test_ref() diff --git a/gtsam/3rdparty/Eigen/test/simplicial_cholesky.cpp b/gtsam/3rdparty/Eigen/test/simplicial_cholesky.cpp index e93a52e9c..786468421 100644 --- a/gtsam/3rdparty/Eigen/test/simplicial_cholesky.cpp +++ b/gtsam/3rdparty/Eigen/test/simplicial_cholesky.cpp @@ -11,26 +11,31 @@ template void test_simplicial_cholesky_T() { - SimplicialCholesky, Lower> chol_colmajor_lower; - SimplicialCholesky, Upper> chol_colmajor_upper; - SimplicialLLT, Lower> llt_colmajor_lower; - SimplicialLDLT, Upper> llt_colmajor_upper; - SimplicialLDLT, Lower> ldlt_colmajor_lower; - SimplicialLDLT, Upper> ldlt_colmajor_upper; + SimplicialCholesky, Lower> chol_colmajor_lower_amd; + SimplicialCholesky, Upper> chol_colmajor_upper_amd; + SimplicialLLT, Lower> llt_colmajor_lower_amd; + SimplicialLLT, Upper> llt_colmajor_upper_amd; + SimplicialLDLT, Lower> ldlt_colmajor_lower_amd; + SimplicialLDLT, Upper> ldlt_colmajor_upper_amd; + SimplicialLDLT, Lower, NaturalOrdering > ldlt_colmajor_lower_nat; + SimplicialLDLT, Upper, NaturalOrdering > ldlt_colmajor_upper_nat; - check_sparse_spd_solving(chol_colmajor_lower); - check_sparse_spd_solving(chol_colmajor_upper); - check_sparse_spd_solving(llt_colmajor_lower); - check_sparse_spd_solving(llt_colmajor_upper); - check_sparse_spd_solving(ldlt_colmajor_lower); - check_sparse_spd_solving(ldlt_colmajor_upper); + check_sparse_spd_solving(chol_colmajor_lower_amd); + check_sparse_spd_solving(chol_colmajor_upper_amd); + check_sparse_spd_solving(llt_colmajor_lower_amd); + check_sparse_spd_solving(llt_colmajor_upper_amd); + check_sparse_spd_solving(ldlt_colmajor_lower_amd); + check_sparse_spd_solving(ldlt_colmajor_upper_amd); - check_sparse_spd_determinant(chol_colmajor_lower); - check_sparse_spd_determinant(chol_colmajor_upper); - check_sparse_spd_determinant(llt_colmajor_lower); - check_sparse_spd_determinant(llt_colmajor_upper); - check_sparse_spd_determinant(ldlt_colmajor_lower); - check_sparse_spd_determinant(ldlt_colmajor_upper); + check_sparse_spd_determinant(chol_colmajor_lower_amd); + check_sparse_spd_determinant(chol_colmajor_upper_amd); + check_sparse_spd_determinant(llt_colmajor_lower_amd); + check_sparse_spd_determinant(llt_colmajor_upper_amd); + check_sparse_spd_determinant(ldlt_colmajor_lower_amd); + check_sparse_spd_determinant(ldlt_colmajor_upper_amd); + + check_sparse_spd_solving(ldlt_colmajor_lower_nat); + check_sparse_spd_solving(ldlt_colmajor_upper_nat); } void test_simplicial_cholesky() diff --git a/gtsam/3rdparty/Eigen/test/sparseqr.cpp b/gtsam/3rdparty/Eigen/test/sparseqr.cpp index 6edba30b2..1fe4a98ee 100644 --- a/gtsam/3rdparty/Eigen/test/sparseqr.cpp +++ b/gtsam/3rdparty/Eigen/test/sparseqr.cpp @@ -2,24 +2,24 @@ // for linear algebra. // // Copyright (C) 2012 Desire Nuentsa Wakam +// Copyright (C) 2014 Gael Guennebaud // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed #include "sparse.h" #include - template -int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 300) +int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 150) { eigen_assert(maxRows >= maxCols); typedef typename MatrixType::Scalar Scalar; int rows = internal::random(1,maxRows); - int cols = internal::random(1,rows); + int cols = internal::random(1,maxCols); double density = (std::max)(8./(rows*cols), 0.01); - A.resize(rows,rows); - dA.resize(rows,rows); + A.resize(rows,cols); + dA.resize(rows,cols); initSparse(density, dA, A,ForceNonZeroDiag); A.makeCompressed(); int nop = internal::random(0, internal::random(0,1) > 0.5 ? cols/2 : 0); @@ -31,6 +31,13 @@ int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows A.col(j0) = s * A.col(j1); dA.col(j0) = s * dA.col(j1); } + +// if(rows void test_sparseqr_scalar() MatrixType A; DenseMat dA; DenseVector refX,x,b; - SparseQR > solver; + SparseQR > solver; generate_sparse_rectangular_problem(A,dA); - int n = A.cols(); - b = DenseVector::Random(n); + b = dA * DenseVector::Random(A.cols()); solver.compute(A); if (solver.info() != Success) { @@ -60,17 +66,19 @@ template void test_sparseqr_scalar() std::cerr << "sparse QR factorization failed\n"; exit(0); return; - } + } + + VERIFY_IS_APPROX(A * x, b); + //Compare with a dense QR solver ColPivHouseholderQR dqr(dA); refX = dqr.solve(b); VERIFY_IS_EQUAL(dqr.rank(), solver.rank()); - - if(solver.rank() (A * x - b).norm() ); - else + if(solver.rank()==A.cols()) // full rank VERIFY_IS_APPROX(x, refX); +// else +// VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() ); // Compute explicitly the matrix Q MatrixType Q, QtQ, idM; @@ -88,3 +96,4 @@ void test_sparseqr() CALL_SUBTEST_2(test_sparseqr_scalar >()); } } + diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h index 073367506..c8c84069e 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h @@ -2,7 +2,7 @@ // for linear algebra. // // Copyright (C) 2011 Gael Guennebaud -// Copyright (C) 2012 Kolja Brix +// Copyright (C) 2012, 2014 Kolja Brix // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed @@ -72,16 +72,20 @@ bool gmres(const MatrixType & mat, const Rhs & rhs, Dest & x, const Precondition VectorType p0 = rhs - mat*x; VectorType r0 = precond.solve(p0); -// RealScalar r0_sqnorm = r0.squaredNorm(); + + // is initial guess already good enough? + if(abs(r0.norm()) < tol) { + return true; + } VectorType w = VectorType::Zero(restart + 1); - FMatrixType H = FMatrixType::Zero(m, restart + 1); + FMatrixType H = FMatrixType::Zero(m, restart + 1); // Hessenberg matrix VectorType tau = VectorType::Zero(restart + 1); std::vector < JacobiRotation < Scalar > > G(restart); // generate first Householder vector - VectorType e; + VectorType e(m-1); RealScalar beta; r0.makeHouseholder(e, tau.coeffRef(0), beta); w(0)=(Scalar) beta; diff --git a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp index 6eb417e8d..087e7c542 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp @@ -127,46 +127,47 @@ template void forward_jacobian(const Func& f) VERIFY_IS_APPROX(j, jref); } + +// TODO also check actual derivatives! void test_autodiff_scalar() { - std::cerr << foo(1,2) << "\n"; + Vector2f p = Vector2f::Random(); typedef AutoDiffScalar AD; - AD ax(1,Vector2f::UnitX()); - AD ay(2,Vector2f::UnitY()); + AD ax(p.x(),Vector2f::UnitX()); + AD ay(p.y(),Vector2f::UnitY()); AD res = foo(ax,ay); - std::cerr << res.value() << " <> " - << res.derivatives().transpose() << "\n\n"; + VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y())); } +// TODO also check actual derivatives! void test_autodiff_vector() { - std::cerr << foo(Vector2f(1,2)) << "\n"; + Vector2f p = Vector2f::Random(); typedef AutoDiffScalar AD; typedef Matrix VectorAD; - VectorAD p(AD(1),AD(-1)); - p.x().derivatives() = Vector2f::UnitX(); - p.y().derivatives() = Vector2f::UnitY(); + VectorAD ap = p.cast(); + ap.x().derivatives() = Vector2f::UnitX(); + ap.y().derivatives() = Vector2f::UnitY(); - AD res = foo(p); - std::cerr << res.value() << " <> " - << res.derivatives().transpose() << "\n\n"; + AD res = foo(ap); + VERIFY_IS_APPROX(res.value(), foo(p)); } void test_autodiff_jacobian() { - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); - CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); - CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); - CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); - CALL_SUBTEST(( forward_jacobian(TestFunc1(3,3)) )); - } + CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); + CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); + CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); + CALL_SUBTEST(( forward_jacobian(TestFunc1()) )); + CALL_SUBTEST(( forward_jacobian(TestFunc1(3,3)) )); } void test_autodiff() { - test_autodiff_scalar(); - test_autodiff_vector(); -// test_autodiff_jacobian(); + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1( test_autodiff_scalar() ); + CALL_SUBTEST_2( test_autodiff_vector() ); + CALL_SUBTEST_3( test_autodiff_jacobian() ); + } } diff --git a/gtsam/3rdparty/gtsam_eigen_includes.h.in b/gtsam/3rdparty/gtsam_eigen_includes.h.in index ecc38d5c4..fa5a51c70 100644 --- a/gtsam/3rdparty/gtsam_eigen_includes.h.in +++ b/gtsam/3rdparty/gtsam_eigen_includes.h.in @@ -17,6 +17,10 @@ #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> diff --git a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt index 4c4e2c03f..93c546be8 100644 --- a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt @@ -9,7 +9,9 @@ if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") endif() endif() -add_definitions(-Wno-unknown-pragmas) +if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC")) + #add_definitions(-Wno-unknown-pragmas) +endif() if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.6 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.6) diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h b/gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h index 2cb80ccf2..5a21274f3 100644 --- a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h +++ b/gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h @@ -59,9 +59,10 @@ typedef ptrdiff_t ssize_t; #endif #ifdef __MSC__ +#if(_MSC_VER < 1700) /* MSC does not have rint() function */ #define rint(x) ((int)((x)+0.5)) - +#endif /* MSC does not have INFINITY defined */ #ifndef INFINITY #define INFINITY FLT_MAX diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 49528a126..78155d308 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -16,8 +16,9 @@ */ #pragma once -#include + #include +#include ////////////////// // The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 8d43dd6ea..23e5fd023 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -19,9 +19,9 @@ #include +#include #include #include -#include #include namespace gtsam { @@ -40,9 +40,12 @@ struct LieMatrix : public Matrix, public DerivedValue { /** initialize from a normal matrix */ LieMatrix(const Matrix& 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) : @@ -82,6 +85,7 @@ struct LieMatrix : public Matrix, public DerivedValue { inline LieMatrix retract(const Vector& v) const { if(v.size() != this->size()) throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size"); + return LieMatrix(*this + Eigen::Map >( &v(0), this->rows(), this->cols())); diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 6989bbfd2..a8bfe3007 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -33,10 +33,13 @@ struct LieVector : public Vector, public DerivedValue { /** initialize from a normal vector */ LieVector(const Vector& 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)) {} diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 6386bd526..16884f4c1 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -467,7 +467,7 @@ GTSAM_EXPORT Matrix Cayley(const Matrix& A); /// Implementation of Cayley transform using fixed size matrices to let /// Eigen do more optimization template -Eigen::Matrix Cayley(const Eigen::Matrix& A) { +Eigen::Matrix CayleyFixed(const Eigen::Matrix& A) { typedef Eigen::Matrix FMat; return (FMat::Identity() - A)*(FMat::Identity() + A).inverse(); } diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 3c9c4a5da..4d7104ad7 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -36,18 +36,19 @@ namespace gtsam { * Values can operate generically on Value objects, retracting or computing * local coordinates for many Value objects of different types. * - * When you implement retract_(), localCoordinates_(), and equals_(), we - * suggest first implementing versions of these functions that work directly - * with derived objects, then using the provided helper functions to - * implement the generic Value versions. This makes your implementation - * easier, and also improves performance in situations where the derived type - * is in fact known, such as in most implementations of \c evaluateError() in - * classes derived from NonlinearFactor. + * Inhereting from the DerivedValue class templated provides a generic implementation of + * the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating + * the need to implement these functions in your class. Note that you must inheret from + * DerivedValue templated on the class you are defining. For example you cannot define + * the following + * \code + * class Rot3 : public DerivedValue{ \\classdef } + * \endcode * * Using the above practice, here is an example of implementing a typical * class derived from Value: * \code - class Rot3 : public Value { + class GTSAM_EXPORT Rot3 : public DerivedValue { public: // Constructor, there is never a need to call the Value base class constructor. Rot3() { ... } @@ -74,27 +75,6 @@ namespace gtsam { // Math to implement 3D rotation localCoordinates, e.g. logarithm map return Vector(result); } - - // Equals implementing the generic Value interface (virtual, implements Value::equals_()) - virtual bool equals_(const Value& other, double tol = 1e-9) const { - // Call our provided helper function to call your Rot3-specific - // equals with appropriate casting. - return CallDerivedEquals(this, other, tol); - } - - // retract implementing the generic Value interface (virtual, implements Value::retract_()) - virtual std::auto_ptr retract_(const Vector& delta) const { - // Call our provided helper function to call your Rot3-specific - // retract and do the appropriate casting and allocation. - return CallDerivedRetract(this, delta); - } - - // localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_()) - virtual Vector localCoordinates_(const Value& value) const { - // Call our provided helper function to call your Rot3-specific - // localCoordinates and do the appropriate casting. - return CallDerivedLocalCoordinates(this, value); - } }; \endcode */ diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 48ada018f..b872aa08b 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -30,55 +30,11 @@ #include -//#ifdef WIN32 -//#include -//#endif using namespace std; namespace gtsam { -/* ************************************************************************* */ -void odprintf_(const char *format, ostream& stream, ...) { - char buf[4096], *p = buf; - - va_list args; - va_start(args, stream); -#ifdef WIN32 - _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL -#else - vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL -#endif - va_end(args); - -//#ifdef WIN32 -// OutputDebugString(buf); -//#else - stream << buf; -//#endif -} - -/* ************************************************************************* */ - -void odprintf(const char *format, ...) { - char buf[4096], *p = buf; - - va_list args; - va_start(args, format); -#ifdef WIN32 - _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL -#else - vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL -#endif - va_end(args); - -//#ifdef WIN32 -// OutputDebugString(buf); -//#else - cout << buf; -//#endif -} - /* ************************************************************************* */ bool zero(const Vector& v) { bool result = true; @@ -101,10 +57,12 @@ Vector delta(size_t n, size_t i, double value) { /* ************************************************************************* */ void print(const Vector& v, const string& s, ostream& stream) { size_t n = v.size(); - odprintf_("%s [", stream, s.c_str()); - for(size_t i=0; i Vector6; typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; -/** - * An auxiliary function to printf for Win32 compatibility, added by Kai - */ -GTSAM_EXPORT void odprintf(const char *format, ...); - /** * Create vector initialized to a constant value * @param n is the size of the vector diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 4e857b143..9f1851308 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1127,6 +1127,12 @@ TEST( matrix, svd2 ) svd(sampleA, U, s, V); + // take care of sign ambiguity + if (U(0, 1) > 0) { + U = -U; + V = -V; + } + EXPECT(assert_equal(expectedU,U)); EXPECT(assert_equal(expected_s,s,1e-9)); EXPECT(assert_equal(expectedV,V)); @@ -1143,6 +1149,13 @@ TEST( matrix, svd3 ) Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.); svd(sampleAt, U, s, V); + + // take care of sign ambiguity + if (U(0, 0) > 0) { + U = -U; + V = -V; + } + Matrix S = diag(s); Matrix t = U * S; Matrix Vt = trans(V); @@ -1176,6 +1189,17 @@ TEST( matrix, svd4 ) 0.6723, 0.7403); svd(A, U, s, V); + + // take care of sign ambiguity + if (U(0, 0) < 0) { + U.col(0) = -U.col(0); + V.col(0) = -V.col(0); + } + if (U(0, 1) < 0) { + U.col(1) = -U.col(1); + V.col(1) = -V.col(1); + } + Matrix reconstructed = U * diag(s) * trans(V); EXPECT(assert_equal(A, reconstructed, 1e-4)); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index c134860bb..03c6bff3f 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -165,6 +165,16 @@ public: */ Vector3 calibrate(const Vector3& p) const; + /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) + inline Cal3_S2 between(const Cal3_S2& q, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(5); + if(H2) *H2 = eye(5); + return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); + } + + /// @} /// @name Manifold /// @{ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 6257d8400..118d8546e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -240,7 +240,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { return retractCayley(omega); } else if(mode == Rot3::SLOW_CAYLEY) { Matrix Omega = skewSymmetric(omega); - return (*this)*Cayley<3>(-Omega/2); + return (*this)*CayleyFixed<3>(-Omega/2); } else { assert(false); exit(1); @@ -269,7 +269,7 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const // Create a fixed-size matrix Eigen::Matrix3d A(between(T).matrix()); // using templated version of Cayley - Eigen::Matrix3d Omega = Cayley<3>(A); + Eigen::Matrix3d Omega = CayleyFixed<3>(A); return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); } else { assert(false); diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index d77a534d6..86c0b7e33 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -93,6 +93,17 @@ TEST( Cal3_S2, retract) CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); } +/* ************************************************************************* */ +TEST(Cal3_S2, between) { + Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9); + Matrix H1, H2; + + EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2))); + EXPECT(assert_equal(-eye(5), H1)); + EXPECT(assert_equal(eye(5), H2)); + +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 4d34bbe3d..9d6f798cd 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -173,6 +173,12 @@ TEST (EssentialMatrix, epipoles) { Vector S; gtsam::svd(E.matrix(), U, S, V); + // take care of SVD sign ambiguity + if (U(0, 2) > 0) { + U = -U; + V = -V; + } + // check rank 2 constraint CHECK(fabs(S(2))<1e-10); @@ -182,8 +188,15 @@ TEST (EssentialMatrix, epipoles) { // Check epipoles // Epipole in image 1 is just E.direction() - Unit3 e1(U(0, 2), U(1, 2), U(2, 2)); - EXPECT(assert_equal(e1, E.epipole_a())); + Unit3 e1(-U(0, 2), -U(1, 2), -U(2, 2)); + Unit3 actual = E.epipole_a(); + EXPECT(assert_equal(e1, actual)); + + // take care of SVD sign ambiguity + if (V(0, 2) < 0) { + U = -U; + V = -V; + } // Epipole in image 2 is E.rotation().unrotate(E.direction()) Unit3 e2(V(0, 2), V(1, 2), V(2, 2)); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index fb05bcf9f..51c195d32 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -268,7 +268,7 @@ TEST( triangulation, TriangulationFactor ) { Key pointKey(1); SharedNoiseModel model; typedef TriangulationFactor<> Factor; - Factor factor(camera1, z1, model, pointKey, sharedCal); + Factor factor(camera1, z1, model, pointKey); // Use the factor to calculate the Jacobians Matrix HActual; diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index c0740f756..d40250266 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -38,7 +38,7 @@ Errors::Errors(const VectorValues& V) { /* ************************************************************************* */ void Errors::print(const std::string& s) const { - odprintf("%s:\n", s.c_str()); + cout << s << endl; BOOST_FOREACH(const Vector& v, *this) gtsam::print(v); } diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index 7b0741ed5..6c4cb969a 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -19,10 +19,6 @@ #pragma once #include -#include -#include -#include -#include namespace gtsam { @@ -59,20 +55,10 @@ namespace gtsam { model_ = model; } - /* ************************************************************************* */ - namespace internal { - static inline DenseIndex getColsJF(const std::pair& p) { - return p.second.cols(); - } - } - /* ************************************************************************* */ template void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel) { - using boost::adaptors::transformed; - namespace br = boost::range; - // Check noise model dimension if(noiseModel && (DenseIndex)noiseModel->dim() != b.size()) throw InvalidNoiseModel(b.size(), noiseModel->dim()); @@ -80,25 +66,32 @@ namespace gtsam { // Resize base class key vector Base::keys_.resize(terms.size()); - // Construct block matrix - this uses the boost::range 'transformed' construct to apply - // internal::getColsJF (defined just above here in this file) to each term. This - // transforms the list of terms into a list of variable dimensions, by extracting the - // number of columns in each matrix. This is done to avoid separately allocating an - // array of dimensions before constructing the VerticalBlockMatrix. - Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true); + // Get dimensions of matrices + std::vector dimensions; + dimensions.reserve(terms.size()); + for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) { + const std::pair& term = *it; + const Matrix& Ai = term.second; + dimensions.push_back(Ai.cols()); + } + + // Construct block matrix + Ab_ = VerticalBlockMatrix(dimensions, b.size(), true); // Check and add terms DenseIndex i = 0; // For block index - for(typename TERMS::const_iterator termIt = terms.begin(); termIt != terms.end(); ++termIt) { - const std::pair& term = *termIt; + for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) { + const std::pair& term = *it; + Key key = term.first; + const Matrix& Ai = term.second; // Check block rows - if(term.second.rows() != Ab_.rows()) - throw InvalidMatrixBlock(Ab_.rows(), term.second.rows()); + if(Ai.rows() != Ab_.rows()) + throw InvalidMatrixBlock(Ab_.rows(), Ai.rows()); // Assign key and matrix - Base::keys_[i] = term.first; - Ab_(i) = term.second; + Base::keys_[i] = key; + Ab_(i) = Ai; // Increment block index ++ i; diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 608e9b1e1..d65f96f7f 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -15,6 +15,17 @@ * @author Frank Dellaert */ +#include +#include +#include +#include +#include +#include + +#include +#include // for operator += +using namespace boost::assign; + // STL/C++ #include #include @@ -22,16 +33,6 @@ #include #include -#include // for operator += -using namespace boost::assign; - -#include -#include -#include -#include -#include -#include - using namespace std; using namespace gtsam; diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 6bb8a05e1..d7f0c2dd5 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -25,13 +25,15 @@ #include #include -#include -#include -#include #include #include #include #include +#include + +#include +#include +#include using namespace gtsam; using namespace std; diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index a81c2243a..d789c42fd 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -18,20 +18,21 @@ * @author Richard Roberts **/ +#include +#include +#include +#include +#include +#include +#include + +#include #include // for operator += using namespace boost::assign; #include #include -#include -#include -#include -#include -#include -#include -#include - using namespace std; using namespace gtsam; diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 84c16bd9c..17ad0bf42 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -15,24 +15,25 @@ * @date Dec 15, 2010 */ -#include -#include - -#include -#include - -#include #include #include #include #include #include - +#include #include + #include -using namespace std; +#include +#include +#include using namespace boost::assign; + +#include +#include + +using namespace std; using namespace gtsam; const double tol = 1e-5; diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 1fc7365e7..f70c3496a 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -105,6 +105,24 @@ TEST(JacobianFactor, constructors_and_accessors) EXPECT(noise == expected.get_model()); EXPECT(noise == actual.get_model()); } + { + // Test three-term constructor with std::map + JacobianFactor expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise); + map mapTerms; + // note order of insertion plays no role: order will be determined by keys + mapTerms.insert(terms[2]); + mapTerms.insert(terms[1]); + mapTerms.insert(terms[0]); + JacobianFactor actual(mapTerms, b, noise); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(noise == expected.get_model()); + EXPECT(noise == actual.get_model()); + } { // VerticalBlockMatrix constructor JacobianFactor expected( diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 44f543bc9..0220b9509 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -37,7 +37,15 @@ class MagFactor: public NoiseModelFactor1 { public: - /** Constructor */ + /** + * Constructor of factor that estimates nav to body rotation bRn + * @param key of the unknown rotation bRn in the factor graph + * @param measured magnetometer reading, a 3-vector + * @param scale by which a unit vector is scaled to yield a magnetometer reading + * @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm + * @param bias of the magnetometer, modeled as purely additive (after scaling) + * @param model of the additive Gaussian noise that is assumed + */ MagFactor(Key key, const Point3& measured, double scale, const Unit3& direction, const Point3& bias, const SharedNoiseModel& model) : diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index d1420d029..340f3f6bc 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -117,7 +117,7 @@ public: // casting syntactic sugar - inline bool hasLinearizationPoint() const { return linearizationPoint_; } + inline bool hasLinearizationPoint() const { return linearizationPoint_.is_initialized(); } /** * Simple checks whether this is a Jacobian or Hessian factor diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 42618bbfb..b4498bee4 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -76,9 +76,6 @@ void NonlinearOptimizer::defaultOptimize() { !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, currentError, this->error(), params.verbosity)); - if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) - cout << "Number of iterations: " << this->iterations() << endl; - // Printing if verbose if (params.verbosity >= NonlinearOptimizerParams::TERMINATION && this->iterations() >= params.maxIterations) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 1235fc6fb..1ecaaf170 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -572,7 +572,7 @@ public: FastMap KeySlotMap; for (size_t slot=0; slot < allKeys.size(); slot++) - KeySlotMap.insert(std::make_pair(allKeys[slot],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 diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index a6cc2d3f3..db85c65bf 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -177,7 +177,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, string tag; // load the poses - while (is) { + while (!is.eof()) { if (!(is >> tag)) break; @@ -213,7 +213,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, // Parse the pose constraints int id1, id2; bool haveLandmark = false; - while (is) { + while (!is.eof()) { if (!(is >> tag)) break; @@ -461,12 +461,12 @@ GraphAndValues load3D(const string& filename) { ifstream is(filename.c_str()); if (!is) - throw invalid_argument("load2D: can not find file " + filename); + throw invalid_argument("load3D: can not find file " + filename); Values::shared_ptr initial(new Values); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); - while (is) { + while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); istringstream ls(buf); @@ -493,7 +493,7 @@ GraphAndValues load3D(const string& filename) { is.clear(); /* clears the end-of-file and error flags */ is.seekg(0, ios::beg); - while (is) { + while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); istringstream ls(buf); diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index c021c7fae..3c02f6dbd 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -251,7 +251,7 @@ TEST( BayesTree, shortcutCheck ) // Check if all the cached shortcuts are cleared rootClique->deleteCachedShortcuts(); BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { - bool notCleared = clique->cachedSeparatorMarginal(); + bool notCleared = clique->cachedSeparatorMarginal().is_initialized(); CHECK( notCleared == false); } EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index c9a9e0f0f..f2223c2f4 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -359,6 +359,11 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor { Vector calcIndicatorProb(const gtsam::Values& x); void setValAValB(const gtsam::Values valA, const gtsam::Values valB); + void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph); + void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12); + Matrix get_model_inlier_cov(); + Matrix get_model_outlier_cov(); + void serializable() const; // enabling serialization functionality }; diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h new file mode 100644 index 000000000..fc525f52a --- /dev/null +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -0,0 +1,105 @@ +/* ---------------------------------------------------------------------------- + + * 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 BiasedGPSFactor.h + * @author Luca Carlone + **/ +#pragma once + +#include + +#include +#include + +namespace gtsam { + + /** + * A class to model GPS measurements, including a bias term which models + * common-mode errors and that can be partially corrected if other sensors are used + * @addtogroup SLAM + */ + class BiasedGPSFactor: public NoiseModelFactor2 { + + private: + + typedef BiasedGPSFactor This; + typedef NoiseModelFactor2 Base; + + Point3 measured_; /** The measurement */ + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + BiasedGPSFactor() {} + + /** Constructor */ + BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured, + const SharedNoiseModel& model) : + Base(model, posekey, biaskey), measured_(measured) { + } + + virtual ~BiasedGPSFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "BiasedGPSFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + measured_.print(" measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose, const Point3& bias, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + + if (H1 || H2){ + H1->resize(3,6); // jacobian wrt pose + (*H1) << Matrix3::Zero(), pose.rotation().matrix(); + H2->resize(3,3); // jacobian wrt bias + (*H2) << Matrix3::Identity(); + } + return pose.translation().vector() + bias.vector() - measured_.vector(); + } + + /** return the measured */ + const Point3 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_); + } + }; // \class BiasedGPSFactor + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h new file mode 100644 index 000000000..01b32b39b --- /dev/null +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -0,0 +1,135 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussMarkov1stOrderFactor.h + * @author Vadim Indelman, Stephen Williams, Luca Carlone + * @date Jan 17, 2012 + **/ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* + * - The 1st order GaussMarkov factor relates two keys of the same type. This relation is given via + * key_2 = exp(-1/tau*delta_t) * key1 + w_d + * where tau is the time constant and delta_t is the time difference between the two keys. + * w_d is the equivalent discrete noise, whose covariance is calculated from the continuous noise model and delta_t. + * - w_d is approximated as a Gaussian noise. + * - In the multi-dimensional case, tau is a vector, and the above equation is applied on each element + * in the state (represented by keys), using the appropriate time constant in the vector tau. + */ + +/* + * A class for a measurement predicted by "GaussMarkov1stOrderFactor(config[key1],config[key2])" + * KEY1::Value is the Lie Group type + * T is the measurement type, by default the same + */ +template +class GaussMarkov1stOrderFactor: public NoiseModelFactor2 { + +private: + + typedef GaussMarkov1stOrderFactor This; + typedef NoiseModelFactor2 Base; + + double dt_; + Vector tau_; + +public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + GaussMarkov1stOrderFactor() {} + + /** Constructor */ + GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau, + const SharedGaussian& model) : + Base(calcDiscreteNoiseModel(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) { + } + + virtual ~GaussMarkov1stOrderFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "GaussMarkov1stOrderFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + this->noiseModel_->print(" noise model"); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const VALUE& p1, const VALUE& p2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + + Vector v1( VALUE::Logmap(p1) ); + Vector v2( VALUE::Logmap(p2) ); + + Vector alpha(tau_.size()); + Vector alpha_v1(tau_.size()); + for(int i=0; i + 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_); + } + + SharedGaussian calcDiscreteNoiseModel(const SharedGaussian& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + noiseModel::Gaussian::shared_ptr gaussian_model = boost::dynamic_pointer_cast(model); + SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t))); + return model_d; + } + +}; // \class GaussMarkov1stOrderFactor + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 48071f355..35010cdc6 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include #include @@ -302,6 +304,101 @@ namespace gtsam { return currA_T_currB_msr.localCoordinates(currA_T_currB_pred); } + /* ************************************************************************* */ + SharedGaussian get_model_inlier() const { + return model_inlier_; + } + + /* ************************************************************************* */ + SharedGaussian get_model_outlier() const { + return model_outlier_; + } + + /* ************************************************************************* */ + Matrix get_model_inlier_cov() const { + return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + } + + /* ************************************************************************* */ + Matrix get_model_outlier_cov() const { + return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + } + + /* ************************************************************************* */ + void updateNoiseModels(const gtsam::Values& values, const gtsam::Marginals& marginals) { + /* given marginals version, don't need to marginal multiple times if update a lot */ + + std::vector Keys; + Keys.push_back(keyA_); + Keys.push_back(keyB_); + JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); + Matrix cov1 = joint_marginal12(keyA_, keyA_); + Matrix cov2 = joint_marginal12(keyB_, keyB_); + Matrix cov12 = joint_marginal12(keyA_, keyB_); + + updateNoiseModels_givenCovs(values, cov1, cov2, cov12); + } + + /* ************************************************************************* */ + void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ + + // get joint covariance of the involved states + + Marginals marginals(graph, values, Marginals::QR); + + this->updateNoiseModels(values, marginals); + } + + /* ************************************************************************* */ + void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){ + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ + + const T& p1 = values.at(keyA_); + const T& p2 = values.at(keyB_); + + Matrix H1, H2; + T hx = p1.between(p2, H1, H2); // h(x) + + Matrix H; + H.resize(H1.rows(), H1.rows()+H2.rows()); + H << H1, H2; // H = [H1 H2] + + Matrix joint_cov; + joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); + joint_cov << cov1, cov12, + cov12.transpose(), cov2; + + Matrix cov_state = H*joint_cov*H.transpose(); + + // model_inlier_->print("before:"); + + // update inlier and outlier noise models + Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); + + Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); + + // model_inlier_->print("after:"); + // std::cout<<"covRinlier + cov_state: "< +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::symbol_shorthand; +using namespace gtsam::noiseModel; +// Convenience for named keys + +using symbol_shorthand::X; +using symbol_shorthand::B; + +TEST(BiasedGPSFactor, errorNoiseless) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + Point3 noise(0.0,0.0,0.0); + Point3 measured = t + noise; + + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 ); + Vector actualError = factor.evaluateError(pose,bias); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +TEST(BiasedGPSFactor, errorNoisy) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + Point3 noise(1.0,2.0,3.0); + Point3 measured = t - noise; + + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 ); + Vector actualError = factor.evaluateError(pose,bias); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +TEST(BiasedGPSFactor, jacobian) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + + Point3 noise(0.0,0.0,0.0); + Point3 measured = t + noise; + + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + + Matrix actualH1, actualH2; + factor.evaluateError(pose,bias, actualH1, actualH2); + + Matrix numericalH1 = numericalDerivative21( + boost::function(boost::bind( + &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), pose, bias, 1e-5); + EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); + + Matrix numericalH2 = numericalDerivative22( + boost::function(boost::bind( + &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), pose, bias, 1e-5); + EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp new file mode 100644 index 000000000..c708c1949 --- /dev/null +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGaussMarkov1stOrderFactor.cpp + * @brief Unit tests for the GaussMarkov1stOrder factor + * @author Vadim Indelman + * @date Jan 17, 2012 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//! Factors +typedef GaussMarkov1stOrderFactor GaussMarkovFactor; + +/* ************************************************************************* */ +gtsam::LieVector predictionError(const gtsam::LieVector& v1, const gtsam::LieVector& v2, const GaussMarkovFactor factor) { + return factor.evaluateError(v1, v2); +} + +/* ************************************************************************* */ +TEST( GaussMarkovFactor, equals ) +{ + // Create two identical factors and make sure they're equal + gtsam::Key x1(1); + gtsam::Key x2(2); + double delta_t = 0.10; + Vector tau = (Vector(3) << 100.0, 150.0, 10.0); + gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0); + + GaussMarkovFactor factor1(x1, x2, delta_t, tau, model); + GaussMarkovFactor factor2(x1, x2, delta_t, tau, model); + + CHECK(gtsam::assert_equal(factor1, factor2)); +} + +/* ************************************************************************* */ +TEST( GaussMarkovFactor, error ) +{ + gtsam::Values linPoint; + gtsam::Key x1(1); + gtsam::Key x2(2); + double delta_t = 0.10; + Vector tau = (Vector(3) << 100.0, 150.0, 10.0); + gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0); + + gtsam::LieVector v1 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 12.0, 13.0)); + gtsam::LieVector v2 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 15.0, 14.0)); + + // Create two nodes + linPoint.insert(x1, v1); + linPoint.insert(x2, v2); + + GaussMarkovFactor factor(x1, x2, delta_t, tau, model); + gtsam::Vector Err1( factor.evaluateError(v1, v2) ); + + // Manually calculate the error + Vector alpha(tau.size()); + Vector alpha_v1(tau.size()); + for(int i=0; i(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + numerical_H2 = gtsam::numericalDerivative22(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + + // Verify they are equal for this choice of state + CHECK( gtsam::assert_equal(numerical_H1, computed_H1, 1e-9)); + CHECK( gtsam::assert_equal(numerical_H2, computed_H2, 1e-9)); +} + +/* ************************************************************************* */ +int main() +{ + TestResult tr; return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 3a7bf0cd0..7c370b534 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -49,12 +49,12 @@ using symbol_shorthand::T; typedef ProjectionFactorPPP TestProjectionFactor; /* ************************************************************************* */ -TEST( ProjectionFactor, nonStandard ) { +TEST( ProjectionFactorPPP, nonStandard ) { ProjectionFactorPPP f; } /* ************************************************************************* */ -TEST( ProjectionFactor, Constructor) { +TEST( ProjectionFactorPPP, Constructor) { Key poseKey(X(1)); Key transformKey(T(1)); Key pointKey(L(1)); @@ -65,7 +65,7 @@ TEST( ProjectionFactor, Constructor) { } /* ************************************************************************* */ -TEST( ProjectionFactor, ConstructorWithTransform) { +TEST( ProjectionFactorPPP, ConstructorWithTransform) { Key poseKey(X(1)); Key transformKey(T(1)); Key pointKey(L(1)); @@ -75,7 +75,7 @@ TEST( ProjectionFactor, ConstructorWithTransform) { } /* ************************************************************************* */ -TEST( ProjectionFactor, Equals ) { +TEST( ProjectionFactorPPP, Equals ) { // Create two identical factors and make sure they're equal Point2 measurement(323.0, 240.0); @@ -86,7 +86,7 @@ TEST( ProjectionFactor, Equals ) { } /* ************************************************************************* */ -TEST( ProjectionFactor, EqualsWithTransform ) { +TEST( ProjectionFactorPPP, EqualsWithTransform ) { // Create two identical factors and make sure they're equal Point2 measurement(323.0, 240.0); Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); @@ -98,7 +98,7 @@ TEST( ProjectionFactor, EqualsWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactor, Error ) { +TEST( ProjectionFactorPPP, Error ) { // Create the factor with a measurement that is 3 pixels off in x Key poseKey(X(1)); Key transformKey(T(1)); @@ -121,7 +121,7 @@ TEST( ProjectionFactor, Error ) { } /* ************************************************************************* */ -TEST( ProjectionFactor, ErrorWithTransform ) { +TEST( ProjectionFactorPPP, ErrorWithTransform ) { // Create the factor with a measurement that is 3 pixels off in x Key poseKey(X(1)); Key transformKey(T(1)); @@ -145,7 +145,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactor, Jacobian ) { +TEST( ProjectionFactorPPP, Jacobian ) { // Create the factor with a measurement that is 3 pixels off in x Key poseKey(X(1)); Key transformKey(T(1)); @@ -179,7 +179,7 @@ TEST( ProjectionFactor, Jacobian ) { } /* ************************************************************************* */ -TEST( ProjectionFactor, JacobianWithTransform ) { +TEST( ProjectionFactorPPP, JacobianWithTransform ) { // Create the factor with a measurement that is 3 pixels off in x Key poseKey(X(1)); Key transformKey(T(1)); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 5b572fb69..36f7fdf52 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -50,19 +50,19 @@ using symbol_shorthand::K; typedef ProjectionFactorPPPC TestProjectionFactor; /* ************************************************************************* */ -TEST( ProjectionFactor, nonStandard ) { +TEST( ProjectionFactorPPPC, nonStandard ) { ProjectionFactorPPPC f; } /* ************************************************************************* */ -TEST( ProjectionFactor, Constructor) { +TEST( ProjectionFactorPPPC, Constructor) { Point2 measurement(323.0, 240.0); TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1)); // TODO: Actually check something } /* ************************************************************************* */ -TEST( ProjectionFactor, Equals ) { +TEST( ProjectionFactorPPPC, Equals ) { // Create two identical factors and make sure they're equal Point2 measurement(323.0, 240.0); @@ -73,7 +73,7 @@ TEST( ProjectionFactor, Equals ) { } /* ************************************************************************* */ -TEST( ProjectionFactor, Error ) { +TEST( ProjectionFactorPPPC, Error ) { // Create the factor with a measurement that is 3 pixels off in x Point2 measurement(323.0, 240.0); TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1)); @@ -93,7 +93,7 @@ TEST( ProjectionFactor, Error ) { } /* ************************************************************************* */ -TEST( ProjectionFactor, ErrorWithTransform ) { +TEST( ProjectionFactorPPPC, ErrorWithTransform ) { // Create the factor with a measurement that is 3 pixels off in x Point2 measurement(323.0, 240.0); Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); @@ -114,7 +114,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactor, Jacobian ) { +TEST( ProjectionFactorPPPC, Jacobian ) { // Create the factor with a measurement that is 3 pixels off in x Point2 measurement(323.0, 240.0); TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1)); @@ -149,7 +149,7 @@ TEST( ProjectionFactor, Jacobian ) { } /* ************************************************************************* */ -TEST( ProjectionFactor, JacobianWithTransform ) { +TEST( ProjectionFactorPPPC, JacobianWithTransform ) { // Create the factor with a measurement that is 3 pixels off in x Point2 measurement(323.0, 240.0); Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); diff --git a/package.xml b/package.xml new file mode 100644 index 000000000..f7b3e0dc5 --- /dev/null +++ b/package.xml @@ -0,0 +1,16 @@ + + + gtsam + 3.1.0 + gtsam + + Frank Dellaert + BSD + + cmake + + + cmake + + +