diff --git a/CMakeLists.txt b/CMakeLists.txt index 9ef48a56e..6fcce54b6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -174,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/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/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/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 Vector9; 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/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/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/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index bcd33a095..fc9d4f62b 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_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index aacca18ea..b732b8088 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 20490a8e4..cda588d90 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 + + +